How to use

applyTransform

method

in

us.ihmc.euclid.tuple4D.Quaternion

@Override
public void applyTransform(Transform transform)
{
 orientation.applyTransform(transform);
 angularVelocity.applyTransform(transform);
}

  @Override
  public void transform(AdjustFootstepMessage object, RigidBodyTransform rigidBodyTransformToApply)
  {
   object.getLocation().applyTransform(rigidBodyTransformToApply);
   object.getOrientation().applyTransform(rigidBodyTransformToApply);
  }
});

@Test
public void testChangeFrame() throws Exception
{
 Random random = new Random(43563);
 for (int i = 0; i < ITERATIONS; i++)
 {
   ReferenceFrame[] referenceFrames = EuclidFrameRandomTools.nextReferenceFrameTree(random);
   ReferenceFrame initialFrame = referenceFrames[random.nextInt(referenceFrames.length)];
   ReferenceFrame anotherFrame = referenceFrames[random.nextInt(referenceFrames.length)];
   Quaternion expected = EuclidCoreRandomTools.nextQuaternion(random);
   FrameQuaternion quaternion = new FrameQuaternion(initialFrame, expected);
   RigidBodyTransform transform = initialFrame.getTransformToDesiredFrame(anotherFrame);
   expected.applyTransform(transform);
   quaternion.changeFrame(anotherFrame);
   assertTrue(anotherFrame == quaternion.getReferenceFrame());
   EuclidCoreTestTools.assertQuaternionGeometricallyEquals(expected, quaternion, EPSILON);
   ReferenceFrame differentRootFrame = ReferenceFrameTools.constructARootFrame("anotherRootFrame");
   try
   {
    quaternion.changeFrame(differentRootFrame);
    fail("Should have thrown a RuntimeException");
   }
   catch (RuntimeException e)
   {
    // good
   }
 }
}

@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 = RandomNumbers.nextDouble(random, 0.0, 1000.0);
 Quaternion expectedOrientation = new Quaternion(RandomGeometry.nextQuaternion(random));
 Vector3D expectedAngularVelocity = new Vector3D(RandomGeometry.nextVector3D(random));
 SimpleSO3TrajectoryPoint testedSimpleSO3TrajectoryPoint = new SimpleSO3TrajectoryPoint(expectedTime, expectedOrientation,
    expectedAngularVelocity);
 for (int i = 0; i < 10000; i++)
 {
   expectedFrame = EuclidFrameRandomTools.nextReferenceFrame("randomFrame" + i, random, random.nextBoolean() ? worldFrame : expectedFrame);
   expectedOrientation.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame));
   expectedAngularVelocity.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame));
   testedSimpleSO3TrajectoryPoint.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame));
   assertTrajectoryPointContainsExpectedData(expectedTime, expectedOrientation, 
      expectedAngularVelocity, testedSimpleSO3TrajectoryPoint, epsilon);
 }
}

  @Override
  public void transform(FootstepDataMessage message, RigidBodyTransform rigidBodyTransformToApply)
  {
   if (message.getLocation() != null)
     message.getLocation().applyTransform(rigidBodyTransformToApply);
   if (message.getOrientation() != null)
     message.getOrientation().applyTransform(rigidBodyTransformToApply);
   if (message.getCustomPositionWaypoints() != null)
   {
     for (int i = 0; i < message.getCustomPositionWaypoints().size(); i++)
      message.getCustomPositionWaypoints().get(i).applyTransform(rigidBodyTransformToApply);
   }
  }
});

@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 = RandomNumbers.nextDouble(random, 0.0, 1000.0);
 Point3D expectedPosition = new Point3D(RandomGeometry.nextPoint3D(random, 10.0, 10.0, 10.0));
 Quaternion expectedOrientation = new Quaternion(RandomGeometry.nextQuaternion(random));
 Vector3D expectedLinearVelocity = new Vector3D(RandomGeometry.nextVector3D(random));
 Vector3D expectedAngularVelocity = new Vector3D(RandomGeometry.nextVector3D(random));
 SimpleSE3TrajectoryPoint testedSimpleSE3TrajectoryPoint = new SimpleSE3TrajectoryPoint(expectedTime, expectedPosition, expectedOrientation,
    expectedLinearVelocity, expectedAngularVelocity);
 for (int i = 0; i < 10000; i++)
 {
   expectedFrame = EuclidFrameRandomTools.nextReferenceFrame("randomFrame" + i, random, random.nextBoolean() ? worldFrame : expectedFrame);
   expectedPosition.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame));
   expectedOrientation.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame));
   expectedLinearVelocity.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame));
   expectedAngularVelocity.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame));
   testedSimpleSE3TrajectoryPoint.applyTransform(worldFrame.getTransformToDesiredFrame(expectedFrame));
   assertTrajectoryPointContainsExpectedData(expectedTime, expectedPosition, expectedOrientation, expectedLinearVelocity,
      expectedAngularVelocity, testedSimpleSE3TrajectoryPoint, epsilon);
 }
}

desiredPosition2d.applyTransform(fromWorldToMidFeetZUpTransform);
Quaternion desiredOrientationCorrected = new Quaternion(desiredOrientation);
desiredOrientationCorrected.applyTransform(fromWorldToMidFeetZUpTransform);

desiredPosition2d.applyTransform(fromWorldToMidFeetZUpTransform);
Quaternion desiredOrientationCorrected = new Quaternion(desiredOrientation);
desiredOrientationCorrected.applyTransform(fromWorldToMidFeetZUpTransform);

orientation.applyTransform(transformToPoseFrame);
transformToPoseFrame.transform(angularVelocity);

orientation.applyTransform(transformToPoseFrame);
transformToPoseFrame.transform(angularVelocity);

orientation.applyTransform(transformToPoseFrame);
transformToPoseFrame.transform(angularVelocity);

orientation.applyTransform(transformToPoseFrame);
transformToPoseFrame.transform(linearVelocity);
transformToPoseFrame.transform(angularVelocity);

orientation.applyTransform(transformToPoseFrame);
transformToPoseFrame.transform(linearVelocity);
transformToPoseFrame.transform(angularVelocity);

orientation.applyTransform(transformToPoseFrame);
transformToPoseFrame.transform(linearVelocity);
transformToPoseFrame.transform(angularVelocity);

assertFalse(angularVelocity.epsilonEquals(simpleSO3TrajectoryPoint.getAngularVelocityCopy(), 1e-10));
orientation.applyTransform(worldFrame.getTransformToDesiredFrame(poseFrame));
angularVelocity.applyTransform(worldFrame.getTransformToDesiredFrame(poseFrame));

orientation.applyTransform(worldFrame.getTransformToDesiredFrame(poseFrame));
linearVelocity.applyTransform(worldFrame.getTransformToDesiredFrame(poseFrame));
angularVelocity.applyTransform(worldFrame.getTransformToDesiredFrame(poseFrame));