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;
  }
  @Override
  public void compute(double time) {
    double trajectoryTime = stepTime.getDoubleValue();
    isDone.set(time >= trajectoryTime);

    time = MathTools.clipToMinMax(time, 0.0, trajectoryTime);
    timeIntoStep.set(time);

    double percent = time / trajectoryTime;
    trajectory.compute(percent);
  }
  public void compute(double time, boolean adjustAngle) {
    this.currentTime.set(time);
    time = MathTools.clipToMinMax(time, 0.0, desiredTrajectoryTime.getDoubleValue());
    anglePolynomial.compute(time);

    double angle = anglePolynomial.getPosition();
    double angleDot = anglePolynomial.getVelocity();
    double angleDDot = anglePolynomial.getAcceleration();

    double cos = Math.cos(angle);
    double sin = Math.sin(angle);
    double r = initialRadius.getDoubleValue();

    double x = r * cos;
    double y = r * sin;
    double z = initialZ.getDoubleValue();

    currentPosition.setIncludingFrame(circleFrame, x, y, z);
    yoCurrentPositionWorld.setAndMatchFrame(currentPosition);

    currentRelativeAngle.set(
        computeAngleDifferenceMinusPiToPi(angle, initialAngle.getDoubleValue()));
    if (adjustAngle)
      currentAdjustedRelativeAngle.set(
          adjustCurrentDesiredRelativeAngle(currentRelativeAngle.getDoubleValue()));
    else currentAdjustedRelativeAngle.set(currentRelativeAngle.getDoubleValue());

    angle =
        trimAngleMinusPiToPi(
            currentAdjustedRelativeAngle.getDoubleValue() + initialAngle.getDoubleValue());

    if (isDone()) {
      angle = finalAngle.getDoubleValue();
      angleDot = 0.0;
      angleDDot = 0.0;
    }

    cos = Math.cos(angle);
    sin = Math.sin(angle);

    x = r * cos;
    y = r * sin;

    double xDot = -r * sin * angleDot;
    double yDot = x * angleDot;
    double zDot = 0.0;

    double xDDot = -r * cos * angleDot * angleDot - y * angleDDot;
    double yDDot = xDot * angleDot + x * angleDDot;
    double zDDot = 0.0;

    currentPosition.setIncludingFrame(circleFrame, x, y, z);
    currentVelocity.setIncludingFrame(circleFrame, xDot, yDot, zDot);
    currentAcceleration.setIncludingFrame(circleFrame, xDDot, yDDot, zDDot);

    if (rotateHandAngleAboutAxis) {
      rotateInitialOrientation(currentOrientation, angle - initialAngle.getDoubleValue());
      currentAngularVelocity.setIncludingFrame(circleFrame, 0.0, 0.0, angleDot);
      currentAngularAcceleration.setIncludingFrame(circleFrame, 0.0, 0.0, angleDDot);
    } else {
      currentOrientation.setIncludingFrame(initialOrientation);
      currentAngularVelocity.setIncludingFrame(circleFrame, 0.0, 0.0, 0.0);
      currentAngularAcceleration.setIncludingFrame(circleFrame, 0.0, 0.0, 0.0);
    }

    yoCurrentPosition.setAndMatchFrame(currentPosition);
    yoCurrentAdjustedPositionWorld.setAndMatchFrame(currentPosition);
    yoCurrentVelocity.setAndMatchFrame(currentVelocity);
    yoCurrentAcceleration.setAndMatchFrame(currentAcceleration);

    currentOrientation.changeFrame(trajectoryFrame);
    yoCurrentOrientation.set(currentOrientation);
    yoCurrentAngularVelocity.setAndMatchFrame(currentAngularVelocity);
    yoCurrentAngularAcceleration.setAndMatchFrame(currentAngularAcceleration);

    updateTangentialCircleFrame();
  }
  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;
  }
 public void setIntegralLeakRatio(double integralLeakRatio) {
   this.integralLeakRatio.set(MathTools.clipToMinMax(integralLeakRatio, 0.0, 1.0));
 }