@Override public void clear() { hasPrivilegedRootJointPosition = false; privilegedRootJointPosition.setToNaN(); hasPrivilegedRootJointOrientation = false; privilegedRootJointOrientation.setToNaN(); }
/** * Sets the point and normal of this plane to {@link Double#NaN}. After calling this method, this * plane becomes invalid. A new valid point and valid normal will have to be set so this plane is * again usable. */ @Override public void setToNaN() { point.setToNaN(); normal.setToNaN(); }
this.points[i].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; }
EuclidCoreTestTools.assertTuple3DEquals(pointOnEllipsoid1, intersection1, LARGE_EPSILON); EuclidCoreTestTools.assertTuple3DEquals(pointOnEllipsoid2, intersection2, LARGE_EPSILON); intersection1.setToNaN(); intersection2.setToNaN(); EuclidCoreTestTools.assertTuple3DEquals(pointOnEllipsoid1, intersection2, LARGE_EPSILON); EuclidCoreTestTools.assertTuple3DEquals(pointOnEllipsoid2, intersection1, LARGE_EPSILON); intersection1.setToNaN(); intersection2.setToNaN(); EuclidCoreTestTools.assertTuple3DEquals(pointOnEllipsoid1, intersection1, LARGE_EPSILON); EuclidCoreTestTools.assertTuple3DContainsOnlyNaN(intersection2); intersection1.setToNaN(); intersection2.setToNaN(); EuclidCoreTestTools.assertTuple3DEquals(pointOnEllipsoid1, intersection1, LARGE_EPSILON); EuclidCoreTestTools.assertTuple3DContainsOnlyNaN(intersection2); intersection1.setToNaN(); intersection2.setToNaN(); EuclidCoreTestTools.assertTuple3DEquals(pointOnEllipsoid2, intersection1, LARGE_EPSILON); EuclidCoreTestTools.assertTuple3DContainsOnlyNaN(intersection2); intersection1.setToNaN(); intersection2.setToNaN(); EuclidCoreTestTools.assertTuple3DEquals(pointOnEllipsoid2, intersection1, LARGE_EPSILON); EuclidCoreTestTools.assertTuple3DContainsOnlyNaN(intersection2); intersection1.setToNaN(); intersection2.setToNaN();
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; }
EuclidCoreTestTools.assertTuple3DEquals(pointOnBottom, actualIntersection1, EPSILON); EuclidCoreTestTools.assertTuple3DEquals(pointOnTop, actualIntersection2, EPSILON); actualIntersection1.setToNaN(); actualIntersection2.setToNaN(); EuclidCoreTestTools.assertTuple3DEquals(pointOnBottom, actualIntersection1, EPSILON); EuclidCoreTestTools.assertTuple3DEquals(pointOnTop, actualIntersection2, EPSILON); actualIntersection1.setToNaN(); actualIntersection2.setToNaN(); EuclidCoreTestTools.assertTuple3DEquals(pointOnBottom, actualIntersection1, EPSILON); EuclidCoreTestTools.assertTuple3DContainsOnlyNaN(actualIntersection2); actualIntersection1.setToNaN(); actualIntersection2.setToNaN(); EuclidCoreTestTools.assertTuple3DEquals(pointOnBottom, actualIntersection1, EPSILON); EuclidCoreTestTools.assertTuple3DContainsOnlyNaN(actualIntersection2); actualIntersection1.setToNaN(); actualIntersection2.setToNaN(); EuclidCoreTestTools.assertTuple3DEquals(pointOnTop, actualIntersection1, EPSILON); EuclidCoreTestTools.assertTuple3DContainsOnlyNaN(actualIntersection2); actualIntersection1.setToNaN(); actualIntersection2.setToNaN(); EuclidCoreTestTools.assertTuple3DEquals(pointOnTop, actualIntersection1, EPSILON); EuclidCoreTestTools.assertTuple3DContainsOnlyNaN(actualIntersection2); actualIntersection1.setToNaN(); actualIntersection2.setToNaN();
@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()); } }
@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); } }
EuclidCoreTestTools.assertTuple3DEquals(pointOnEllipsoid1, intersection1, LARGE_EPSILON); EuclidCoreTestTools.assertTuple3DEquals(pointOnEllipsoid2, intersection2, LARGE_EPSILON); intersection1.setToNaN(); intersection2.setToNaN(); EuclidCoreTestTools.assertTuple3DEquals(pointOnEllipsoid2, intersection1, LARGE_EPSILON); EuclidCoreTestTools.assertTuple3DContainsOnlyNaN(intersection2); intersection1.setToNaN(); intersection2.setToNaN(); EuclidCoreTestTools.assertTuple3DContainsOnlyNaN(intersection1); EuclidCoreTestTools.assertTuple3DContainsOnlyNaN(intersection2); intersection1.setToNaN(); intersection2.setToNaN();
EuclidCoreTestTools.assertTuple3DEquals(pointOnEllipsoid1, intersection1, LARGE_EPSILON); EuclidCoreTestTools.assertTuple3DEquals(pointOnEllipsoid2, intersection2, LARGE_EPSILON); intersection1.setToNaN(); intersection2.setToNaN(); EuclidCoreTestTools.assertTuple3DEquals(pointOnEllipsoid1, intersection2, LARGE_EPSILON); EuclidCoreTestTools.assertTuple3DEquals(pointOnEllipsoid2, intersection1, LARGE_EPSILON); intersection1.setToNaN(); intersection2.setToNaN(); EuclidCoreTestTools.assertTuple3DEquals(pointOnEllipsoid1, intersection1, LARGE_EPSILON); EuclidCoreTestTools.assertTuple3DEquals(pointOnEllipsoid2, intersection2, LARGE_EPSILON); intersection1.setToNaN(); intersection2.setToNaN(); EuclidCoreTestTools.assertTuple3DEquals(pointOnEllipsoid1, intersection2, LARGE_EPSILON); EuclidCoreTestTools.assertTuple3DEquals(pointOnEllipsoid2, intersection1, LARGE_EPSILON); intersection1.setToNaN(); intersection2.setToNaN();
EuclidCoreTestTools.assertTuple3DEquals(expectedIntersection1, actualIntersection1, EPSILON); EuclidCoreTestTools.assertTuple3DEquals(expectedIntersection2, actualIntersection2, EPSILON); actualIntersection1.setToNaN(); actualIntersection2.setToNaN(); EuclidCoreTestTools.assertTuple3DEquals(expectedIntersection1, actualIntersection1, EPSILON); EuclidCoreTestTools.assertTuple3DEquals(expectedIntersection2, actualIntersection2, EPSILON); actualIntersection1.setToNaN(); actualIntersection2.setToNaN(); EuclidCoreTestTools.assertTuple3DEquals(expectedIntersection1, actualIntersection1, EPSILON); EuclidCoreTestTools.assertTuple3DEquals(expectedIntersection2, actualIntersection2, EPSILON); actualIntersection1.setToNaN(); actualIntersection2.setToNaN(); EuclidCoreTestTools.assertTuple3DEquals(expectedIntersection2, actualIntersection2, LARGE_EPSILON); errors.add(expectedIntersection2.distance(actualIntersection2)); actualIntersection1.setToNaN(); actualIntersection2.setToNaN(); EuclidCoreTestTools.assertTuple3DEquals(expectedIntersection2, actualIntersection2, LARGE_EPSILON); errors.add(expectedIntersection2.distance(actualIntersection2)); actualIntersection1.setToNaN(); actualIntersection2.setToNaN(); EuclidCoreTestTools.assertTuple3DEquals(expectedIntersection2, actualIntersection2, LARGE_EPSILON); errors.add(expectedIntersection2.distance(actualIntersection2)); actualIntersection1.setToNaN(); actualIntersection2.setToNaN();
EuclidCoreTestTools.assertTuple3DEquals(expectedIntersection1, actualIntersection1, EPSILON); EuclidCoreTestTools.assertTuple3DEquals(expectedIntersection2, actualIntersection2, EPSILON); actualIntersection1.setToNaN(); actualIntersection2.setToNaN(); EuclidCoreTestTools.assertTuple3DEquals(expectedIntersection, actualIntersection1, EPSILON); EuclidCoreTestTools.assertTuple3DContainsOnlyNaN(actualIntersection2); actualIntersection1.setToNaN(); EuclidCoreTestTools.assertTuple3DEquals(expectedIntersection, actualIntersection1, EPSILON); EuclidCoreTestTools.assertTuple3DContainsOnlyNaN(actualIntersection2); actualIntersection1.setToNaN(); EuclidCoreTestTools.assertTuple3DEquals(expectedIntersection2, actualIntersection2, LARGE_EPSILON); errors.add(expectedIntersection2.distance(actualIntersection2)); actualIntersection1.setToNaN(); actualIntersection2.setToNaN(); EuclidCoreTestTools.assertTuple3DEquals(expectedIntersection2, actualIntersection2, LARGE_EPSILON); errors.add(expectedIntersection2.distance(actualIntersection2)); actualIntersection1.setToNaN(); actualIntersection2.setToNaN(); EuclidCoreTestTools.assertTuple3DEquals(pointOnCylinder, actualIntersection2, LARGE_EPSILON); errors.add(pointOnCylinder.distance(actualIntersection2)); actualIntersection1.setToNaN(); actualIntersection2.setToNaN(); errors.add(pointOnCylinder.distance(actualIntersection1));