private Matrix3d computeDesiredFootRotation(
double angleToDestination, RobotSide swingLegSide, ReferenceFrame supportFootFrame) {
RigidBodyTransform supportFootToWorldTransform =
supportFootFrame.getTransformToDesiredFrame(ReferenceFrame.getWorldFrame());
Matrix3d supportFootToWorldRotation = new Matrix3d();
supportFootToWorldTransform.get(supportFootToWorldRotation);
double maxTurnInAngle = 0.25;
double maxTurnOutAngle = 0.4;
double amountToYaw;
switch (blindWalkingDirection.getEnumValue()) {
case BACKWARD:
{
amountToYaw = AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, Math.PI);
break;
}
case LEFT:
{
amountToYaw =
AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, Math.PI / 2.0);
break;
}
case RIGHT:
{
amountToYaw =
AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, -Math.PI / 2.0);
break;
}
case FORWARD:
{
amountToYaw = angleToDestination;
break;
}
default:
{
throw new RuntimeException("Shouldn't get here!");
}
}
if (swingLegSide == RobotSide.LEFT) {
amountToYaw = MathTools.clipToMinMax(amountToYaw, -maxTurnInAngle, maxTurnOutAngle);
} else {
amountToYaw = MathTools.clipToMinMax(amountToYaw, -maxTurnOutAngle, maxTurnInAngle);
}
Matrix3d yawRotation = new Matrix3d();
yawRotation.rotZ(amountToYaw);
Matrix3d ret = new Matrix3d();
ret.mul(yawRotation, supportFootToWorldRotation);
return ret;
}
private void computeProportionalTerm(FrameOrientation desiredOrientation) {
desiredOrientation.changeFrame(bodyFrame);
desiredOrientation.getQuaternion(errorQuaternion);
errorAngleAxis.set(errorQuaternion);
errorAngleAxis.setAngle(AngleTools.trimAngleMinusPiToPi(errorAngleAxis.getAngle()));
// Limit the maximum position error considered for control action
double maximumError = gains.getMaximumProportionalError();
if (errorAngleAxis.getAngle() > maximumError) {
errorAngleAxis.setAngle(Math.signum(errorAngleAxis.getAngle()) * maximumError);
}
proportionalTerm.set(errorAngleAxis.getX(), errorAngleAxis.getY(), errorAngleAxis.getZ());
proportionalTerm.scale(errorAngleAxis.getAngle());
rotationErrorInBody.set(proportionalTerm);
proportionalGainMatrix.transform(proportionalTerm.getVector());
}
private FrameVector2d computeDesiredOffsetFromSupportAnkle(
ReferenceFrame supportAnkleZUpFrame,
RobotSide swingLegSide,
double angleToDestination,
double distanceToDestination) {
if (distanceToDestination < DISTANCE_TO_DESTINATION_FOR_STEP_IN_PLACE) {
return new FrameVector2d(
supportAnkleZUpFrame,
0.0,
swingLegSide.negateIfRightSide(desiredStepWidth.getDoubleValue()));
}
double absoluteAngleToDestination;
switch (blindWalkingDirection.getEnumValue()) {
case BACKWARD:
{
absoluteAngleToDestination =
Math.abs(AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, Math.PI));
break;
}
case LEFT:
{
absoluteAngleToDestination =
Math.abs(
AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, Math.PI / 2.0));
break;
}
case RIGHT:
{
absoluteAngleToDestination =
Math.abs(
AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, -Math.PI / 2.0));
break;
}
case FORWARD:
{
absoluteAngleToDestination = Math.abs(angleToDestination);
break;
}
default:
{
throw new RuntimeException("Shouldn't get here!");
}
}
double minAngleBeforeShorterSteps = 0.1;
double maxAngleBeforeTurnInPlace = 0.5;
double percentToStepInXY =
1.0
- (absoluteAngleToDestination - minAngleBeforeShorterSteps)
/ (maxAngleBeforeTurnInPlace - minAngleBeforeShorterSteps);
if (percentToStepInXY > 1.0) percentToStepInXY = 1.0;
if (percentToStepInXY < 0.0) percentToStepInXY = 0.0;
switch (blindWalkingDirection.getEnumValue()) {
case BACKWARD:
{
double backwardsDistanceReduction = 0.75;
desiredOffsetFromSquaredUp.set(
-backwardsDistanceReduction * percentToStepInXY * desiredStepForward.getDoubleValue(),
0.0);
break;
}
case LEFT:
{
desiredOffsetFromSquaredUp.set(
0.0, percentToStepInXY * desiredStepSideward.getDoubleValue());
break;
}
case RIGHT:
{
desiredOffsetFromSquaredUp.set(
0.0, -percentToStepInXY * desiredStepSideward.getDoubleValue());
break;
}
case FORWARD:
{
desiredOffsetFromSquaredUp.set(
percentToStepInXY * desiredStepForward.getDoubleValue(), 0.0);
break;
}
default:
{
throw new RuntimeException("Shouldn't get here!");
}
}
double stepLength = desiredOffsetFromSquaredUp.length();
double maxDistanceToAllow =
distanceToDestination - 0.5 * DISTANCE_TO_DESTINATION_FOR_STEP_IN_PLACE;
if (maxDistanceToAllow < 0.0) maxDistanceToAllow = 0.0;
if (stepLength > maxDistanceToAllow) {
desiredOffsetFromSquaredUp.scale(maxDistanceToAllow / stepLength);
stepLength = desiredOffsetFromSquaredUp.length();
}
if (stepLength > maxStepLength.getDoubleValue()) {
desiredOffsetFromSquaredUp.scale(maxStepLength.getDoubleValue() / stepLength);
}
FrameVector2d desiredOffsetFromAnkle =
new FrameVector2d(
supportAnkleZUpFrame,
desiredOffsetFromSquaredUp.getX(),
desiredOffsetFromSquaredUp.getY()
+ swingLegSide.negateIfRightSide(desiredStepWidth.getDoubleValue()));
if (swingLegSide == RobotSide.LEFT) {
desiredOffsetFromAnkle.setY(
MathTools.clipToMinMax(
desiredOffsetFromAnkle.getY(),
minStepWidth.getDoubleValue(),
maxStepWidth.getDoubleValue()));
} else {
desiredOffsetFromAnkle.setY(
MathTools.clipToMinMax(
desiredOffsetFromAnkle.getY(),
-maxStepWidth.getDoubleValue(),
-minStepWidth.getDoubleValue()));
}
return desiredOffsetFromAnkle;
}