@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);