@Override
  public void onEntry() {
    // initialize body orientation
    bodyOrientation.setToZero(bodyZUpFrame);
    bodyOrientation.changeFrame(worldFrame);
    bodyYaw = bodyOrientation.getYaw();

    // initialize step queue
    updateXGaitSettings();
    supportCentroid.setToZero(supportFrame);
    double initialTime = timestamp.getDoubleValue() + initialStepDelayParameter.get();
    Vector3d initialVelocity = planarVelocityProvider.get();
    RobotQuadrant initialQuadrant =
        (xGaitSettings.getEndPhaseShift() < 90)
            ? RobotQuadrant.HIND_LEFT
            : RobotQuadrant.FRONT_LEFT;
    xGaitStepPlanner.computeInitialPlan(
        xGaitPreviewSteps,
        initialVelocity,
        initialQuadrant,
        supportCentroid,
        initialTime,
        bodyYaw,
        xGaitSettings);
    for (int i = 0; i < 2; i++) {
      RobotEnd robotEnd = xGaitPreviewSteps.get(i).getRobotQuadrant().getEnd();
      xGaitCurrentSteps.get(robotEnd).set(xGaitPreviewSteps.get(i));
    }
    this.process();
  }
 public void setInitialPoseToMatchReferenceFrame(ReferenceFrame referenceFrame) {
   initialPosition.setToZero(referenceFrame);
   initialOrientation.setToZero(referenceFrame);
   initialPosition.changeFrame(trajectoryFrame);
   initialOrientation.changeFrame(trajectoryFrame);
   yoInitialPosition.set(initialPosition);
   yoInitialOrientation.set(initialOrientation);
 }
  private void rotateInitialOrientation(
      FrameOrientation orientationToPack, double angleFromInitialOrientation) {
    initialOrientation.changeFrame(circleFrame);
    orientationToPack.setIncludingFrame(initialOrientation);

    axisRotationTransform.rotZ(angleFromInitialOrientation);

    orientationToPack.applyTransform(axisRotationTransform);
  }
  @ContinuousIntegrationTest(estimatedDuration = 0.2)
  @Test(timeout = 30000)
  public void testChangeFrame() throws Exception {
    double epsilon = 1.0e-10;
    Random random = new Random(21651016L);
    ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();

    ReferenceFrame expectedFrame = worldFrame;
    double expectedTime = RandomTools.generateRandomDouble(random, 0.0, 1000.0);
    FrameOrientation expectedOrientation =
        FrameOrientation.generateRandomFrameOrientation(random, expectedFrame);
    FrameVector expectedAngularVelocity =
        FrameVector.generateRandomFrameVector(random, expectedFrame);
    String expectedNamePrefix = "test";
    String expectedNameSuffix = "blop";
    YoFrameSO3TrajectoryPoint testedYoFrameSO3TrajectoryPoint =
        new YoFrameSO3TrajectoryPoint(
            expectedNamePrefix,
            expectedNameSuffix,
            new YoVariableRegistry("schnoop"),
            expectedFrame);

    testedYoFrameSO3TrajectoryPoint.set(expectedTime, expectedOrientation, expectedAngularVelocity);

    ReferenceFrame[] randomFrames = new ReferenceFrame[10];
    randomFrames[0] = worldFrame;
    for (int i = 1; i < 10; i++)
      randomFrames[i] =
          ReferenceFrame.generateRandomReferenceFrame(
              "randomFrame" + i,
              random,
              random.nextBoolean() ? worldFrame : randomFrames[random.nextInt(i)]);

    for (int i = 0; i < 10000; i++) {
      expectedFrame = randomFrames[random.nextInt(10)];
      testedYoFrameSO3TrajectoryPoint.registerReferenceFrame(expectedFrame);

      expectedOrientation.changeFrame(expectedFrame);
      expectedAngularVelocity.changeFrame(expectedFrame);
      testedYoFrameSO3TrajectoryPoint.changeFrame(expectedFrame);

      assertWaypointContainsExpectedData(
          expectedNamePrefix,
          expectedNameSuffix,
          expectedFrame,
          expectedTime,
          expectedOrientation,
          expectedAngularVelocity,
          testedYoFrameSO3TrajectoryPoint,
          epsilon);
    }
  }
  public void initialize() {
    currentTime.set(0.0);
    desiredTrajectoryTime.set(desiredTrajectoryTimeProvider.getValue());

    yoInitialPosition.getFrameTupleIncludingFrame(initialPosition);
    initialPosition.changeFrame(circleFrame);

    if (rotateHandAngleAboutAxis)
      rotateInitialOrientation(finalOrientation, desiredRotationAngle.getDoubleValue());
    else finalOrientation.setIncludingFrame(initialOrientation);
    finalOrientation.changeFrame(trajectoryFrame);
    yoFinalOrientation.set(finalOrientation);

    double x = initialPosition.getX();
    double y = initialPosition.getY();
    initialZ.set(initialPosition.getZ());

    initialRadius.set(Math.sqrt(x * x + y * y));
    initialAngle.set(Math.atan2(y, x));
    finalAngle.set(initialAngle.getDoubleValue() + desiredRotationAngle.getDoubleValue());

    //      anglePolynomial.setLinear(0.0, desiredTrajectoryTime.getDoubleValue(),
    // initialAngle.getDoubleValue(), finalAngle.getDoubleValue());
    anglePolynomial.setCubic(
        0.0,
        desiredTrajectoryTime.getDoubleValue(),
        initialAngle.getDoubleValue(),
        0.0,
        finalAngle.getDoubleValue(),
        0.0);
    //      anglePolynomial.setQuintic(0.0, desiredTrajectoryTime.getDoubleValue(),
    // initialAngle.getDoubleValue(), 0.0, 0.0, finalAngle.getDoubleValue(), 0.0, 0.0);

    double xFinal = initialRadius.getDoubleValue() * Math.cos(finalAngle.getDoubleValue());
    double yFinal = initialRadius.getDoubleValue() * Math.sin(finalAngle.getDoubleValue());
    double zFinal = initialZ.getDoubleValue();
    finalPosition.setIncludingFrame(circleFrame, xFinal, yFinal, zFinal);
    yoFinalPosition.setAndMatchFrame(finalPosition);

    rotateInitialOrientation(finalOrientation, finalAngle.getDoubleValue());

    currentAngleTrackingError.set(0.0);
    currentControlledFrameRelativeAngle.set(initialAngle.getDoubleValue());

    updateTangentialCircleFrame();

    if (visualize) visualizeTrajectory();
  }
  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());
  }
  @Override
  public void process() {
    double currentTime = timestamp.getDoubleValue();

    // update body orientation
    bodyYaw += planarVelocityProvider.get().getZ() * controlDT;
    bodyOrientation.changeFrame(worldFrame);
    bodyOrientation.setYawPitchRoll(bodyYaw, 0.0, 0.0);

    // update xgait current steps
    for (int i = 0; i < xGaitPreviewSteps.size(); i++) {
      QuadrupedTimedStep xGaitPreviewStep = xGaitPreviewSteps.get(i);
      if (xGaitPreviewStep.getTimeInterval().getStartTime() <= currentTime) {
        xGaitCurrentSteps.get(xGaitPreviewStep.getRobotEnd()).set(xGaitPreviewStep);
      }
    }

    // update xgait preview steps
    updateXGaitSettings();
    Vector3d inputVelocity = planarVelocityProvider.get();
    xGaitStepPlanner.computeOnlinePlan(
        xGaitPreviewSteps, xGaitCurrentSteps, inputVelocity, currentTime, bodyYaw, xGaitSettings);

    // update step sequence
    stepSequence.clear();
    for (RobotEnd robotEnd : RobotEnd.values) {
      if (xGaitCurrentSteps.get(robotEnd).getTimeInterval().getEndTime() >= currentTime) {
        stepSequence.add();
        stepSequence.get(stepSequence.size() - 1).set(xGaitCurrentSteps.get(robotEnd));
      }
    }
    for (int i = 0; i < xGaitPreviewSteps.size(); i++) {
      if (xGaitPreviewSteps.get(i).getTimeInterval().getEndTime() >= currentTime) {
        stepSequence.add();
        stepSequence.get(stepSequence.size() - 1).set(xGaitPreviewSteps.get(i));
      }
    }
  }
  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();
  }
  @ContinuousIntegrationTest(estimatedDuration = 0.0)
  @Test(timeout = 30000)
  public void testSomeMoreSettersAndGetters() {
    String namePrefix = "point";
    String nameSuffix = "toTest";
    YoVariableRegistry registry = new YoVariableRegistry("myRegistry");

    ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();

    YoFrameSO3TrajectoryPoint yoFrameSO3TrajectoryPoint =
        new YoFrameSO3TrajectoryPoint(namePrefix, nameSuffix, registry, worldFrame);
    yoFrameSO3TrajectoryPoint.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());

    double time = 3.4;
    FrameOrientation orientation =
        new FrameOrientation(worldFrame, new Quat4d(0.1, 0.22, 0.34, 0.56));

    FrameVector angularVelocity = new FrameVector(worldFrame, 1.7, 8.4, 2.2);

    yoFrameSO3TrajectoryPoint.setTime(time);
    yoFrameSO3TrajectoryPoint.setOrientation(orientation);
    yoFrameSO3TrajectoryPoint.setAngularVelocity(angularVelocity);

    PoseReferenceFrame poseFrame = new PoseReferenceFrame("poseFrame", new FramePose(worldFrame));

    FramePoint poseFramePosition = new FramePoint(worldFrame, new Point3d(0.5, 7.7, 9.2));
    poseFrame.setPositionAndUpdate(poseFramePosition);

    FrameOrientation poseOrientation =
        new FrameOrientation(worldFrame, new AxisAngle4d(1.2, 3.9, 4.7, 2.2));
    poseFrame.setOrientationAndUpdate(poseOrientation);

    yoFrameSO3TrajectoryPoint.registerReferenceFrame(poseFrame);

    yoFrameSO3TrajectoryPoint.changeFrame(poseFrame);

    assertFalse(
        orientation.epsilonEquals(
            yoFrameSO3TrajectoryPoint.getOrientation().getFrameOrientationCopy(), 1e-10));
    assertFalse(
        angularVelocity.epsilonEquals(
            yoFrameSO3TrajectoryPoint.getAngularVelocity().getFrameVectorCopy(), 1e-10));

    orientation.changeFrame(poseFrame);
    angularVelocity.changeFrame(poseFrame);

    assertTrue(
        orientation.epsilonEquals(
            yoFrameSO3TrajectoryPoint.getOrientation().getFrameOrientationCopy(), 1e-10));
    assertTrue(
        angularVelocity.epsilonEquals(
            yoFrameSO3TrajectoryPoint.getAngularVelocity().getFrameVectorCopy(), 1e-10));

    YoFrameSO3TrajectoryPoint yoFrameSO3TrajectoryPointTwo =
        new YoFrameSO3TrajectoryPoint(namePrefix, nameSuffix + "Two", registry, poseFrame);

    yoFrameSO3TrajectoryPointTwo.setTime(time);
    yoFrameSO3TrajectoryPointTwo.setOrientation(orientation);
    yoFrameSO3TrajectoryPointTwo.setAngularVelocity(angularVelocity);

    assertTrue(yoFrameSO3TrajectoryPointTwo.epsilonEquals(yoFrameSO3TrajectoryPointTwo, 1e-10));
  }