How to use

setToNaN

method

in

us.ihmc.euclid.tuple4D.Quaternion

@Override
public void setOrientationToNaN()
{
 orientation.setToNaN();
}

@Override
public void clear()
{
 hasPrivilegedRootJointPosition = false;
 privilegedRootJointPosition.setToNaN();
 hasPrivilegedRootJointOrientation = false;
 privilegedRootJointOrientation.setToNaN();
}

public NumericalMovingReferenceFrame(String nameSuffix, ReferenceFrame originalFrame, double updateDT)
{
 super(originalFrame.getName() + nameSuffix, originalFrame.getRootFrame());
 this.originalFrame = originalFrame;
 this.updateDT = updateDT;
 previousRotation.setToNaN();
 previousTranslation.setToNaN();
}

public static FootstepStatusMessage createFootstepStatus(FootstepStatus status, int footstepIndex)
{
 FootstepStatusMessage message = new FootstepStatusMessage();
 message.setFootstepStatus(status.toByte());
 message.setFootstepIndex(footstepIndex);
 message.getDesiredFootPositionInWorld().setToNaN();
 message.getDesiredFootOrientationInWorld().setToNaN();
 message.getActualFootPositionInWorld().setToNaN();
 message.getActualFootOrientationInWorld().setToNaN();
 message.setRobotSide((byte) 255);
 return message;
}

public static FootstepStatusMessage createFootstepStatus(FootstepStatus status, int footstepIndex, RobotSide robotSide)
{
 FootstepStatusMessage message = new FootstepStatusMessage();
 message.setFootstepStatus(status.toByte());
 message.setFootstepIndex(footstepIndex);
 message.getDesiredFootPositionInWorld().setToNaN();
 message.getDesiredFootOrientationInWorld().setToNaN();
 message.getActualFootPositionInWorld().setToNaN();
 message.getActualFootOrientationInWorld().setToNaN();
 message.setRobotSide(robotSide.toByte());
 return message;
}

@Test
public void testTransformQuaternion() throws Exception
{
 Random random = new Random(435L);
 Quaternion actual = new Quaternion();
 Quaternion expected = new Quaternion();
 for (int i = 0; i < NUMBER_OF_ITERATIONS; i++)
 {
   RotationScaleMatrix matrix = EuclidCoreRandomTools.nextRotationScaleMatrix(random, 10.0);
   Quaternion original = EuclidCoreRandomTools.nextQuaternion(random);
   matrix.transform(original, expected);
   actual.set(original);
   matrix.transform(actual);
   EuclidCoreTestTools.assertQuaternionEquals(expected, actual, EPS);
   actual.setToNaN();
   matrix.transform(original, actual);
   EuclidCoreTestTools.assertQuaternionEquals(expected, actual, EPS);
 }
}

@Test
public void testTransformQuaternion() throws Exception
{
 Random random = new Random(435L);
 Quaternion actual = new Quaternion();
 Quaternion expected = new Quaternion();
 for (int i = 0; i < ITERATIONS; i++)
 {
   RotationMatrix matrix = EuclidCoreRandomTools.nextRotationMatrix(random);
   Quaternion original = EuclidCoreRandomTools.nextQuaternion(random);
   matrix.transform(original, expected);
   actual.set(original);
   matrix.transform(actual);
   EuclidCoreTestTools.assertQuaternionEquals(expected, actual, EPS);
   actual.setToNaN();
   matrix.transform(original, actual);
   EuclidCoreTestTools.assertQuaternionEquals(expected, actual, EPS);
 }
}

public static FootstepStatusMessage createFootstepStatus(FootstepStatus status, int footstepIndex, Point3D actualFootPositionInWorld,
                            Quaternion actualFootOrientationInWorld)
{
 FootstepStatusMessage message = new FootstepStatusMessage();
 message.setFootstepStatus(status.toByte());
 message.setFootstepIndex(footstepIndex);
 message.getDesiredFootPositionInWorld().setToNaN();
 message.getDesiredFootOrientationInWorld().setToNaN();
 message.getActualFootPositionInWorld().set(actualFootPositionInWorld);
 message.getActualFootOrientationInWorld().set(actualFootOrientationInWorld);
 message.setRobotSide((byte) 255);
 return message;
}

public static FootstepStatusMessage createFootstepStatus(FootstepStatus status, int footstepIndex, Point3D actualFootPositionInWorld,
                            Quaternion actualFootOrientationInWorld, RobotSide robotSide)
{
 FootstepStatusMessage message = new FootstepStatusMessage();
 message.setFootstepStatus(status.toByte());
 message.setFootstepIndex(footstepIndex);
 message.getDesiredFootPositionInWorld().setToNaN();
 message.getDesiredFootOrientationInWorld().setToNaN();
 message.getActualFootPositionInWorld().set(actualFootPositionInWorld);
 message.getActualFootOrientationInWorld().set(actualFootOrientationInWorld);
 message.setRobotSide(robotSide.toByte());
 return message;
}

@Override
public void set(KinematicsToolboxConfigurationCommand other)
{
 hasPrivilegedRootJointPosition = other.hasPrivilegedRootJointPosition;
 if (hasPrivilegedRootJointPosition)
   privilegedRootJointPosition.set(other.getPrivilegedRootJointPosition());
 else
   privilegedRootJointPosition.setToNaN();
 hasPrivilegedRootJointOrientation = other.hasPrivilegedRootJointOrientation;
 if (hasPrivilegedRootJointOrientation)
   privilegedRootJointOrientation.set(other.getPrivilegedRootJointOrientation());
 else
   privilegedRootJointOrientation.setToNaN();
 hasPrivilegedJointAngles = other.hasPrivilegedJointAngles;
 jointHashCodes.reset();
 privilegedJointAngles.reset();
 if (hasPrivilegedJointAngles)
 {
   jointHashCodes.addAll(other.getJointHashCodes());
   privilegedJointAngles.addAll(other.getPrivilegedJointAngles());
 }
}

@Test
public void testTransformAQuaternion() throws Exception
{
 Random random = new Random(2352L);
 Quaternion quaternion = new Quaternion();
 Quaternion quaternionOriginal = new Quaternion();
 Quaternion quaternionExpected = new Quaternion();
 Quaternion quaternionActual = new Quaternion();
 // Test with the multiply: qTransformed = q * qOriginal and check that  q = inverseTransform(transform(q))
 for (int i = 0; i < ITERATIONS; i++)
 {
   quaternion = EuclidCoreRandomTools.nextQuaternion(random);
   quaternionOriginal = EuclidCoreRandomTools.nextQuaternion(random);
   QuaternionTools.multiply(quaternion, quaternionOriginal, quaternionExpected);
   assertFalse(quaternionOriginal.epsilonEquals(quaternionExpected, EPSILON));
   quaternionActual.set(quaternionOriginal);
   QuaternionTools.transform(quaternion, quaternionActual, quaternionActual);
   EuclidCoreTestTools.assertQuaternionEquals(quaternionExpected, quaternionActual, EPSILON);
   quaternionActual.setToNaN();
   QuaternionTools.transform(quaternion, quaternionOriginal, quaternionActual);
   EuclidCoreTestTools.assertQuaternionEquals(quaternionExpected, quaternionActual, EPSILON);
   QuaternionTools.inverseTransform(quaternion, quaternionActual, quaternionActual);
   EuclidCoreTestTools.assertQuaternionEquals(quaternionOriginal, quaternionActual, EPSILON);
 }
}

@Override
public void setFromMessage(KinematicsToolboxConfigurationMessage message)
{
 hasPrivilegedRootJointPosition = message.getPrivilegedRootJointPosition() != null;
 if (hasPrivilegedRootJointPosition)
   privilegedRootJointPosition.set(message.getPrivilegedRootJointPosition());
 else
   privilegedRootJointPosition.setToNaN();
 hasPrivilegedRootJointOrientation = message.getPrivilegedRootJointOrientation() != null;
 if (hasPrivilegedRootJointOrientation)
   privilegedRootJointOrientation.set(message.getPrivilegedRootJointOrientation());
 else
   privilegedRootJointOrientation.setToNaN();
 TIntArrayList messageHashCodes = message.getPrivilegedJointHashCodes();
 TFloatArrayList messageJointAngles = message.getPrivilegedJointAngles();
 hasPrivilegedJointAngles = messageHashCodes != null && messageJointAngles != null;
 jointHashCodes.reset();
 privilegedJointAngles.reset();
 if (hasPrivilegedJointAngles)
 {
   jointHashCodes.addAll(messageHashCodes);
   privilegedJointAngles.addAll(messageJointAngles);
 }
}

actualQuaternion.setToNaN();
transform.getRotation(actualQuaternion);
EuclidCoreTestTools.assertQuaternionGeometricallyEquals(expectedQuaternion, actualQuaternion, EPS);