public void setDesiredAcceleration(
     FrameVector angularAcceleration, FrameVector linearAcceleration) {
   desiredAcceleration.reshape(6, 1);
   MatrixTools.insertTuple3dIntoEJMLVector(
       angularAcceleration.getVector(), desiredAcceleration, 0);
   MatrixTools.insertTuple3dIntoEJMLVector(linearAcceleration.getVector(), desiredAcceleration, 3);
 }
  public void compute(
      FrameVector output,
      FrameOrientation desiredOrientation,
      FrameVector desiredAngularVelocity,
      FrameVector currentAngularVelocity,
      FrameVector feedForward) {
    computeProportionalTerm(desiredOrientation);
    if (currentAngularVelocity != null)
      computeDerivativeTerm(desiredAngularVelocity, currentAngularVelocity);
    computeIntegralTerm();

    output.setToZero(proportionalTerm.getReferenceFrame());
    output.add(proportionalTerm);
    output.add(derivativeTerm);
    output.add(integralTerm);

    // Limit the max acceleration of the feedback, but not of the feedforward...
    // JEP changed 150430 based on Atlas hitting limit stops.
    double feedbackAngularActionMagnitude = output.length();
    double maximumAction = gains.getMaximumFeedback();
    if (feedbackAngularActionMagnitude > maximumAction) {
      output.scale(maximumAction / feedbackAngularActionMagnitude);
    }

    feedbackAngularAction.set(output);
    rateLimitedFeedbackAngularAction.update();
    rateLimitedFeedbackAngularAction.getFrameTuple(output);

    feedForward.changeFrame(bodyFrame);
    output.add(feedForward);
  }
 public void getSteeringWheelPose(
     RobotSide robotSide,
     FramePoint centerToPack,
     FrameVector rotationAxisToPack,
     FrameVector zeroAxisToPack) {
   centerToPack.setIncludingFrame(
       worldFrame, steeringWheelCenterAtomic.get(robotSide).getAndSet(null));
   rotationAxisToPack.setIncludingFrame(
       worldFrame, steeringWheelRotationAxisAtomic.get(robotSide).getAndSet(null));
   zeroAxisToPack.setIncludingFrame(
       worldFrame, steeringWheelZeroAxisAtomic.get(robotSide).getAndSet(null));
 }
  @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);
    }
  }
  protected void updateClosedJoint() {
    super.updateClosedJoint();

    integralForce.scale(
        integralStiffness.getDoubleValue(), yoConnectionPositionIntegratedError.getFrameTuple());

    connectionPointA.getForce(tempForce);
    tempForce.add(integralForce.getVector());

    connectionPointA.setForce(tempForce);
    tempForce.scale(-1.0);
    connectionPointB.setForce(tempForce);
  }
 public void getVelocity(FrameVector linearVelocityToPack) {
   linearVelocityToPack.setIncludingFrame(
       ReferenceFrame.getWorldFrame(),
       qd_x.getDoubleValue(),
       qd_y.getDoubleValue(),
       qd_z.getDoubleValue());
 }
 public void getLinearAcceleration(FrameVector linearAccelerationToPack) {
   linearAccelerationToPack.setIncludingFrame(
       ReferenceFrame.getWorldFrame(),
       qdd_x.getDoubleValue(),
       qdd_y.getDoubleValue(),
       qdd_z.getDoubleValue());
 }
  @ContinuousIntegrationTest(estimatedDuration = 0.0)
  @Test(timeout = 30000)
  public void testSingleRigidBodyRotation() {
    Random random = new Random(1766L);

    RigidBody elevator = new RigidBody("elevator", world);
    Vector3d jointAxis = RandomTools.generateRandomVector(random);
    jointAxis.normalize();
    RigidBodyTransform transformToParent = new RigidBodyTransform();
    transformToParent.setIdentity();
    RevoluteJoint joint =
        ScrewTools.addRevoluteJoint("joint", elevator, transformToParent, jointAxis);
    RigidBody body =
        ScrewTools.addRigidBody(
            "body",
            joint,
            RandomTools.generateRandomDiagonalMatrix3d(random),
            random.nextDouble(),
            new Vector3d());

    joint.setQ(random.nextDouble());
    joint.setQd(random.nextDouble());

    Momentum momentum = computeMomentum(elevator, world);

    momentum.changeFrame(world);
    FrameVector linearMomentum =
        new FrameVector(momentum.getExpressedInFrame(), momentum.getLinearPartCopy());
    FrameVector angularMomentum =
        new FrameVector(momentum.getExpressedInFrame(), momentum.getAngularPartCopy());

    FrameVector linearMomentumCheck = new FrameVector(world);
    Matrix3d inertia = body.getInertia().getMassMomentOfInertiaPartCopy();
    Vector3d angularMomentumCheckVector = new Vector3d(jointAxis);
    angularMomentumCheckVector.scale(joint.getQd());
    inertia.transform(angularMomentumCheckVector);
    FrameVector angularMomentumCheck =
        new FrameVector(body.getInertia().getExpressedInFrame(), angularMomentumCheckVector);
    angularMomentumCheck.changeFrame(world);

    double epsilon = 1e-9;
    JUnitTools.assertTuple3dEquals(
        linearMomentumCheck.getVector(), linearMomentum.getVector(), epsilon);
    JUnitTools.assertTuple3dEquals(
        angularMomentumCheck.getVector(), angularMomentum.getVector(), epsilon);
    assertTrue(angularMomentum.length() > epsilon);
  }
  protected List<FramePoint> getContactPoints(RobotSide swingSide) {
    double stepPitch = this.stepPitch.getDoubleValue();
    List<FramePoint> allContactPoints = contactableBodies.get(swingSide).getContactPointsCopy();
    if (stepPitch == 0.0) {
      return allContactPoints;
    } else {
      FrameVector forwardInFootFrame =
          new FrameVector(contactableBodies.get(swingSide).getFrameAfterParentJoint());
      ReferenceFrame frame = allContactPoints.get(0).getReferenceFrame();
      forwardInFootFrame.changeFrame(frame);
      forwardInFootFrame.scale(Math.signum(stepPitch));
      int nPoints = 2;

      return DesiredFootstepCalculatorTools.computeMaximumPointsInDirection(
          allContactPoints, forwardInFootFrame, nPoints);
    }
  }
  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());
  }
  @ContinuousIntegrationTest(estimatedDuration = 0.0)
  @Test(timeout = 30000)
  public void testSetToNaN() throws Exception {
    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);

    testedYoFrameSO3TrajectoryPoint.setToNaN();
    assertTrue(Double.isNaN(testedYoFrameSO3TrajectoryPoint.getTime()));
    assertTrue(testedYoFrameSO3TrajectoryPoint.getOrientation().containsNaN());
    assertTrue(testedYoFrameSO3TrajectoryPoint.getAngularVelocity().containsNaN());

    expectedFrame = ReferenceFrame.generateRandomReferenceFrame("blop", random, worldFrame);
    testedYoFrameSO3TrajectoryPoint.registerReferenceFrame(expectedFrame);
    expectedTime = RandomTools.generateRandomDouble(random, 0.0, 1000.0);
    expectedOrientation = FrameOrientation.generateRandomFrameOrientation(random, worldFrame);
    expectedAngularVelocity = FrameVector.generateRandomFrameVector(random, worldFrame);
    testedYoFrameSO3TrajectoryPoint.switchCurrentReferenceFrame(worldFrame);
    testedYoFrameSO3TrajectoryPoint.registerReferenceFrame(worldFrame);
    testedYoFrameSO3TrajectoryPoint.set(expectedTime, expectedOrientation, expectedAngularVelocity);

    testedYoFrameSO3TrajectoryPoint.setToNaN(expectedFrame);

    assertTrue(expectedFrame == testedYoFrameSO3TrajectoryPoint.getReferenceFrame());
    assertTrue(Double.isNaN(testedYoFrameSO3TrajectoryPoint.getTime()));
    assertTrue(testedYoFrameSO3TrajectoryPoint.getOrientation().containsNaN());
    assertTrue(testedYoFrameSO3TrajectoryPoint.getAngularVelocity().containsNaN());
  }
  private void computeIntegralTerm() {
    if (gains.getMaximumIntegralError() < 1e-5) {
      integralTerm.setToZero(bodyFrame);
      return;
    }

    double integratedErrorAngle = errorAngleAxis.getAngle() * dt;
    double errorIntegratedX = errorAngleAxis.getX() * integratedErrorAngle;
    double errorIntegratedY = errorAngleAxis.getY() * integratedErrorAngle;
    double errorIntegratedZ = errorAngleAxis.getZ() * integratedErrorAngle;
    rotationErrorCumulated.add(errorIntegratedX, errorIntegratedY, errorIntegratedZ);

    double errorMagnitude = rotationErrorCumulated.length();
    if (errorMagnitude > gains.getMaximumIntegralError()) {
      rotationErrorCumulated.scale(gains.getMaximumIntegralError() / errorMagnitude);
    }

    rotationErrorCumulated.getFrameTuple(integralTerm);
    integralGainMatrix.transform(integralTerm.getVector());
  }
  public void getCoMAcceleration(FrameVector comAccelerationToPack) {
    boolean firstIteration = true;
    double totalMass = 0.0;

    for (RigidBody rigidBody : rigidBodies) {
      double mass = rigidBody.getInertia().getMass();
      rigidBody.getCoMOffset(comLocation);

      spatialAccelerationCalculator.getLinearAccelerationOfBodyFixedPoint(
          linkLinearMomentumDot, base, rigidBody, comLocation);
      linkLinearMomentumDot.scale(mass);

      if (firstIteration) comAccelerationToPack.setIncludingFrame(linkLinearMomentumDot);
      else comAccelerationToPack.add(linkLinearMomentumDot);

      totalMass += mass;
      firstIteration = false;
    }

    comAccelerationToPack.scale(1.0 / totalMass);
  }
  /** Computes using Twists, ignores linear part */
  public void compute(Twist twistToPack, FramePose desiredPose, Twist desiredTwist) {
    checkBodyFrames(desiredTwist, twistToPack);
    checkBaseFrames(desiredTwist, twistToPack);
    checkExpressedInFrames(desiredTwist, twistToPack);

    twistToPack.setToZero(bodyFrame, desiredTwist.getBaseFrame(), bodyFrame);

    desiredPose.getOrientationIncludingFrame(desiredOrientation);
    desiredTwist.getAngularPart(desiredAngularVelocity);
    desiredTwist.getAngularPart(feedForwardAngularAction);
    compute(
        angularActionFromOrientationController,
        desiredOrientation,
        desiredAngularVelocity,
        null,
        feedForwardAngularAction);
    twistToPack.setAngularPart(angularActionFromOrientationController.getVector());
  }
  private void computeDerivativeTerm(
      FrameVector desiredAngularVelocity, FrameVector currentAngularVelocity) {
    desiredAngularVelocity.changeFrame(bodyFrame);
    currentAngularVelocity.changeFrame(bodyFrame);

    derivativeTerm.sub(desiredAngularVelocity, currentAngularVelocity);

    // Limit the maximum velocity error considered for control action
    double maximumVelocityError = gains.getMaximumDerivativeError();
    double velocityErrorMagnitude = derivativeTerm.length();
    if (velocityErrorMagnitude > maximumVelocityError) {
      derivativeTerm.scale(maximumVelocityError / velocityErrorMagnitude);
    }

    velocityError.set(derivativeTerm);
    derivativeGainMatrix.transform(derivativeTerm.getVector());
  }
  @ContinuousIntegrationTest(estimatedDuration = 0.0)
  @Test(timeout = 30000)
  public void testSetters() {
    double epsilon = 1.0e-20;
    Random random = new Random(21651016L);
    ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();

    ReferenceFrame expectedFrame = worldFrame;
    double expectedTime = 0.0;
    FrameOrientation expectedOrientation = new FrameOrientation(expectedFrame);
    FrameVector expectedAngularVelocity = new FrameVector(expectedFrame);

    String expectedNamePrefix = "test";
    String expectedNameSuffix = "blop";
    YoFrameSO3TrajectoryPoint testedYoFrameSO3TrajectoryPoint =
        new YoFrameSO3TrajectoryPoint(
            expectedNamePrefix,
            expectedNameSuffix,
            new YoVariableRegistry("schnoop"),
            expectedFrame);

    assertWaypointContainsExpectedData(
        expectedNamePrefix,
        expectedNameSuffix,
        expectedFrame,
        expectedTime,
        expectedOrientation,
        expectedAngularVelocity,
        testedYoFrameSO3TrajectoryPoint,
        epsilon);

    expectedFrame = worldFrame;
    expectedTime = RandomTools.generateRandomDouble(random, 0.0, 1000.0);
    expectedOrientation = FrameOrientation.generateRandomFrameOrientation(random, expectedFrame);
    expectedAngularVelocity = FrameVector.generateRandomFrameVector(random, expectedFrame);

    testedYoFrameSO3TrajectoryPoint.set(expectedTime, expectedOrientation, expectedAngularVelocity);

    assertWaypointContainsExpectedData(
        expectedNamePrefix,
        expectedNameSuffix,
        expectedFrame,
        expectedTime,
        expectedOrientation,
        expectedAngularVelocity,
        testedYoFrameSO3TrajectoryPoint,
        epsilon);

    expectedFrame = worldFrame;
    expectedTime = RandomTools.generateRandomDouble(random, 0.0, 1000.0);
    expectedOrientation = FrameOrientation.generateRandomFrameOrientation(random, expectedFrame);
    expectedAngularVelocity = FrameVector.generateRandomFrameVector(random, expectedFrame);

    testedYoFrameSO3TrajectoryPoint.set(
        expectedTime, expectedOrientation.getQuaternion(), expectedAngularVelocity.getVector());

    assertWaypointContainsExpectedData(
        expectedNamePrefix,
        expectedNameSuffix,
        expectedFrame,
        expectedTime,
        expectedOrientation,
        expectedAngularVelocity,
        testedYoFrameSO3TrajectoryPoint,
        epsilon);

    expectedFrame = worldFrame;
    expectedTime = RandomTools.generateRandomDouble(random, 0.0, 1000.0);
    expectedOrientation = FrameOrientation.generateRandomFrameOrientation(random, expectedFrame);
    expectedAngularVelocity = FrameVector.generateRandomFrameVector(random, expectedFrame);

    YoFrameSO3TrajectoryPoint expectedYoFrameSO3TrajectoryPoint =
        new YoFrameSO3TrajectoryPoint(
            "sdfsd", "asd", new YoVariableRegistry("asawe"), expectedFrame);

    testedYoFrameSO3TrajectoryPoint.set(expectedYoFrameSO3TrajectoryPoint);

    assertTrue(
        expectedYoFrameSO3TrajectoryPoint.epsilonEquals(testedYoFrameSO3TrajectoryPoint, epsilon));
    assertWaypointContainsExpectedData(
        expectedNamePrefix,
        expectedNameSuffix,
        testedYoFrameSO3TrajectoryPoint.getReferenceFrame(),
        testedYoFrameSO3TrajectoryPoint.getTime(),
        testedYoFrameSO3TrajectoryPoint.getOrientation().getFrameOrientationCopy(),
        testedYoFrameSO3TrajectoryPoint.getAngularVelocity().getFrameVectorCopy(),
        testedYoFrameSO3TrajectoryPoint,
        epsilon);
  }
  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();
  }
 public void getAngularAcceleration(
     FrameVector angularAccelerationToPack, ReferenceFrame bodyFrame) {
   angularAccelerationToPack.setIncludingFrame(
       bodyFrame, qdd_wx.getDoubleValue(), qdd_wy.getDoubleValue(), qdd_wz.getDoubleValue());
 }
 public void getAngularVelocity(FrameVector angularVelocityToPack, ReferenceFrame bodyFrame) {
   angularVelocityToPack.setIncludingFrame(
       bodyFrame, qd_wx.getDoubleValue(), qd_wy.getDoubleValue(), qd_wz.getDoubleValue());
 }
  private void assertWaypointContainsExpectedData(
      String expectedNamePrefix,
      String expectedNameSuffix,
      ReferenceFrame expectedFrame,
      double expectedTime,
      FrameOrientation expectedOrientation,
      FrameVector expectedAngularVelocity,
      YoFrameSO3TrajectoryPoint testedYoFrameSO3TrajectoryPoint,
      double epsilon) {
    assertTrue(expectedFrame == testedYoFrameSO3TrajectoryPoint.getReferenceFrame());
    assertEquals(expectedTime, testedYoFrameSO3TrajectoryPoint.getTime(), epsilon);
    assertEquals(expectedNamePrefix, testedYoFrameSO3TrajectoryPoint.getNamePrefix());
    assertEquals(expectedNameSuffix, testedYoFrameSO3TrajectoryPoint.getNameSuffix());
    assertTrue(
        expectedOrientation.epsilonEquals(
            testedYoFrameSO3TrajectoryPoint.getOrientation().getFrameOrientationCopy(), epsilon));
    assertTrue(
        expectedAngularVelocity.epsilonEquals(
            testedYoFrameSO3TrajectoryPoint.getAngularVelocity().getFrameVectorCopy(), epsilon));

    FrameSO3TrajectoryPoint actualFrameSO3TrajectoryPoint = new FrameSO3TrajectoryPoint();
    testedYoFrameSO3TrajectoryPoint.getIncludingFrame(actualFrameSO3TrajectoryPoint);
    FrameSO3TrajectoryPointTest.assertTrajectoryPointContainsExpectedData(
        expectedFrame,
        expectedTime,
        expectedOrientation,
        expectedAngularVelocity,
        actualFrameSO3TrajectoryPoint,
        epsilon);
    actualFrameSO3TrajectoryPoint = new FrameSO3TrajectoryPoint(expectedFrame);
    testedYoFrameSO3TrajectoryPoint.get(actualFrameSO3TrajectoryPoint);
    FrameSO3TrajectoryPointTest.assertTrajectoryPointContainsExpectedData(
        expectedFrame,
        expectedTime,
        expectedOrientation,
        expectedAngularVelocity,
        actualFrameSO3TrajectoryPoint,
        epsilon);

    Quat4d actualOrientation = new Quat4d();
    Vector3d actualAngularVelocity = new Vector3d();

    testedYoFrameSO3TrajectoryPoint.getOrientation(actualOrientation);
    testedYoFrameSO3TrajectoryPoint.getAngularVelocity(actualAngularVelocity);

    assertTrue(expectedOrientation.epsilonEquals(actualOrientation, epsilon));
    assertTrue(expectedAngularVelocity.epsilonEquals(actualAngularVelocity, epsilon));

    FrameOrientation actualFrameOrientation = new FrameOrientation(expectedFrame);
    FrameVector actualFrameAngularVelocity = new FrameVector(expectedFrame);

    testedYoFrameSO3TrajectoryPoint.getOrientation(actualFrameOrientation);
    testedYoFrameSO3TrajectoryPoint.getAngularVelocity(actualFrameAngularVelocity);

    assertTrue(expectedOrientation.epsilonEquals(actualFrameOrientation, epsilon));
    assertTrue(expectedAngularVelocity.epsilonEquals(actualFrameAngularVelocity, epsilon));

    actualFrameOrientation = new FrameOrientation();
    actualFrameAngularVelocity = new FrameVector();

    testedYoFrameSO3TrajectoryPoint.getOrientationIncludingFrame(actualFrameOrientation);
    testedYoFrameSO3TrajectoryPoint.getAngularVelocityIncludingFrame(actualFrameAngularVelocity);

    assertTrue(expectedOrientation.epsilonEquals(actualFrameOrientation, epsilon));
    assertTrue(expectedAngularVelocity.epsilonEquals(actualFrameAngularVelocity, epsilon));
  }
  @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));
  }
 public void setDesiredVelocity(FrameVector angularVelocity, FrameVector linearVelocity) {
   desiredVelocity.reshape(6, 1);
   MatrixTools.insertTuple3dIntoEJMLVector(angularVelocity.getVector(), desiredVelocity, 0);
   MatrixTools.insertTuple3dIntoEJMLVector(linearVelocity.getVector(), desiredVelocity, 3);
 }