public void setIncludingFrame(double time, FrameQuaternionReadOnly orientation, FrameVector3DReadOnly angularVelocity) { orientation.checkReferenceFrameMatch(angularVelocity); setToZero(orientation.getReferenceFrame()); geometryObject.set(time, orientation, angularVelocity); }
/** * Computes the smallest angle representing the difference between the orientation part of this pose * 3D and the give {@code orientation}. * * @param orientation the orientation used to compute the orientation distance. Not modified. * @return the angle difference between {@code this} and {@code orientation}, it is contained in [0, * 2<i>pi</i>]. * @throws ReferenceFrameMismatchException if {@code orientation} is not expressed in the same * reference frame as {@code this}. */ default double getOrientationDistance(FrameQuaternionReadOnly orientation) { return getOrientation().distance(orientation); }
/** * Computes and packs the orientation described by the orientation part of this pose as a rotation * vector. * <p> * WARNING: a rotation vector is different from a yaw-pitch-roll or Euler angles representation. A * rotation vector is equivalent to the axis of an axis-angle that is multiplied by the angle of the * same axis-angle. * </p> * * @param rotationVectorToPack the vector in which the rotation vector is stored. Modified. */ default void getRotationVector(FrameVector3DBasics rotationVectorToPack) { getOrientation().getRotationVector(rotationVectorToPack); }
public static void insertFrameQuaternionIntoEJMLVector(FrameQuaternionReadOnly frameOrientation, DenseMatrix64F matrix, int rowStart) { int index = rowStart; matrix.set(index++, 0, frameOrientation.getX()); matrix.set(index++, 0, frameOrientation.getY()); matrix.set(index++, 0, frameOrientation.getZ()); matrix.set(index++, 0, frameOrientation.getS()); }
public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, FrameQuaternionReadOnly rawQuaternion) { this(namePrefix, nameSuffix, registry, maxRate, dt, rawQuaternion.getReferenceFrame(), rawQuaternion); }
/** * Computes and returns the distance from this quaternion to {@code other}. * * @param other the other quaternion to measure the distance. Not modified. * @return the angle representing the distance between the two quaternions. It is contained in [0, * 2<i>pi</i>] * @throws ReferenceFrameMismatchException if reference frame of {@code this} and {@code other} do * not match. */ default double distance(FrameQuaternionReadOnly other) { checkReferenceFrameMatch(other); return QuaternionReadOnly.super.distance(other); }
public void set(FrameQuaternionReadOnly orientation, boolean notifyListeners) { orientation.checkReferenceFrameMatch(getReferenceFrame()); orientation.getYawPitchRoll(tempYawPitchRoll); yaw.set(tempYawPitchRoll[0], notifyListeners); pitch.set(tempYawPitchRoll[1], notifyListeners); roll.set(tempYawPitchRoll[2], notifyListeners); }
assertTrue(expectedPosition.epsilonEquals(testedYoFrameSE3TrajectoryPoint.getPosition(), epsilon)); Quaternion trajectoryPointQuaternion = new Quaternion(testedYoFrameSE3TrajectoryPoint.getOrientation()); assertEquals(expectedOrientation.getReferenceFrame(), testedYoFrameSE3TrajectoryPoint.getOrientation().getReferenceFrame()); EuclidCoreTestTools.assertQuaternionGeometricallyEquals(expectedOrientation, trajectoryPointQuaternion, epsilon); assertTrue(expectedLinearVelocity.epsilonEquals(testedYoFrameSE3TrajectoryPoint.getLinearVelocity(), epsilon)); assertTrue(expectedOrientation.geometricallyEquals(actualOrientation, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualLinearVelocity, epsilon)); assertTrue(expectedAngularVelocity.epsilonEquals(actualAngularVelocity, epsilon)); assertTrue(expectedOrientation.geometricallyEquals(actualFrameOrientation, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, epsilon)); assertTrue(expectedAngularVelocity.epsilonEquals(actualFrameAngularVelocity, epsilon)); assertTrue(expectedOrientation.geometricallyEquals(actualFrameOrientation, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, epsilon)); assertTrue(expectedAngularVelocity.epsilonEquals(actualFrameAngularVelocity, epsilon));
static void assertTrajectoryPointContainsExpectedData(ReferenceFrame expectedFrame, double expectedTime, FrameQuaternionReadOnly expectedOrientation, FrameVector3DReadOnly expectedAngularVelocity, FrameSO3TrajectoryPoint testedFrameSO3TrajectoryPoint, double epsilon) { assertTrue(expectedFrame == testedFrameSO3TrajectoryPoint.getReferenceFrame()); assertEquals(expectedTime, testedFrameSO3TrajectoryPoint.getTime(), epsilon); assertTrue(expectedOrientation.geometricallyEquals(testedFrameSO3TrajectoryPoint.getGeometryObject().getOrientation(), epsilon)); assertTrue(expectedAngularVelocity.epsilonEquals(testedFrameSO3TrajectoryPoint.getGeometryObject().getAngularVelocity(), epsilon)); Quaternion actualOrientation = new Quaternion(); Vector3D actualAngularVelocity = new Vector3D(); testedFrameSO3TrajectoryPoint.getOrientation(actualOrientation); testedFrameSO3TrajectoryPoint.getAngularVelocity(actualAngularVelocity); assertTrue(expectedOrientation.geometricallyEquals(actualOrientation, epsilon)); assertTrue(expectedAngularVelocity.epsilonEquals(actualAngularVelocity, epsilon)); FrameQuaternion actualFrameOrientation = new FrameQuaternion(); FrameVector3D actualFrameAngularVelocity = new FrameVector3D(); testedFrameSO3TrajectoryPoint.getOrientationIncludingFrame(actualFrameOrientation); testedFrameSO3TrajectoryPoint.getAngularVelocityIncludingFrame(actualFrameAngularVelocity); assertTrue(expectedOrientation.geometricallyEquals(actualFrameOrientation, epsilon)); assertTrue(expectedAngularVelocity.epsilonEquals(actualFrameAngularVelocity, epsilon)); actualFrameOrientation = new FrameQuaternion(expectedFrame); actualFrameAngularVelocity = new FrameVector3D(expectedFrame); testedFrameSO3TrajectoryPoint.getOrientation(actualFrameOrientation); testedFrameSO3TrajectoryPoint.getAngularVelocity(actualFrameAngularVelocity); assertTrue(expectedOrientation.geometricallyEquals(actualFrameOrientation, epsilon)); assertTrue(expectedAngularVelocity.epsilonEquals(actualFrameAngularVelocity, epsilon)); }
@Override @Test public void testEquals() throws Exception { super.testEquals(); Random random = new Random(621541L); ReferenceFrame frame1 = ReferenceFrame.getWorldFrame(); ReferenceFrame frame2 = EuclidFrameRandomTools.nextReferenceFrame(random); double x = random.nextDouble(); double y = random.nextDouble(); double z = random.nextDouble(); double s = random.nextDouble(); F tuple1 = createFrameTuple(frame1, x, y, z, s); F tuple2 = createFrameTuple(frame1, x, y, z, s); F tuple3 = createFrameTuple(frame2, x, y, z, s); F tuple4 = createFrameTuple(frame2, x, y, z, s); assertTrue(tuple1.equals(tuple2)); assertFalse(tuple1.equals(tuple3)); assertFalse(tuple3.equals(tuple2)); assertTrue(tuple3.equals(tuple4)); assertTrue(tuple1.equals(tuple2)); assertFalse(tuple1.equals(tuple3)); assertFalse(tuple3.equals(tuple2)); assertTrue(tuple3.equals(tuple4)); }
@Override @Test public void testEpsilonEquals() throws Exception { super.testEpsilonEquals(); Random random = new Random(621541L); double epsilon = 0.0; ReferenceFrame frame1 = ReferenceFrame.getWorldFrame(); ReferenceFrame frame2 = EuclidFrameRandomTools.nextReferenceFrame(random); double x = random.nextDouble(); double y = random.nextDouble(); double z = random.nextDouble(); double s = random.nextDouble(); F tuple1 = createFrameTuple(frame1, x, y, z, s); F tuple2 = createFrameTuple(frame1, x, y, z, s); F tuple3 = createFrameTuple(frame2, x, y, z, s); F tuple4 = createFrameTuple(frame2, x, y, z, s); assertTrue(tuple1.epsilonEquals(tuple2, epsilon)); assertFalse(tuple1.epsilonEquals(tuple3, epsilon)); assertFalse(tuple3.epsilonEquals(tuple2, epsilon)); assertTrue(tuple3.epsilonEquals(tuple4, epsilon)); assertTrue(tuple1.epsilonEquals(tuple2, epsilon)); assertFalse(tuple1.epsilonEquals(tuple3, epsilon)); assertFalse(tuple3.epsilonEquals(tuple2, epsilon)); assertTrue(tuple3.epsilonEquals(tuple4, epsilon)); }
public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, FrameQuaternionReadOnly rawQuaternion) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawQuaternion.getReferenceFrame(), rawQuaternion); }
/** * Compares {@code this} to {@code other} to determine if the two frame quaternions are * geometrically similar, i.e. the magnitude of their difference is less than or equal to * {@code epsilon}. * * @param other the frame quaternion to compare to. Not modified. * @param epsilon the tolerance of the comparison. * @return {@code true} if the two frame quaternions represent the same geometry, {@code false} * otherwise. * @throws ReferenceFrameMismatchException if {@code other} is not expressed in the same reference * frame as {@code this}. */ default boolean geometricallyEquals(FrameQuaternionReadOnly other, double epsilon) { checkReferenceFrameMatch(other); return QuaternionReadOnly.super.geometricallyEquals(other, epsilon); } }
assertEquals(expectedNamePrefix, testedYoFrameSO3TrajectoryPoint.getNamePrefix()); assertEquals(expectedNameSuffix, testedYoFrameSO3TrajectoryPoint.getNameSuffix()); assertTrue(expectedOrientation.geometricallyEquals(testedYoFrameSO3TrajectoryPoint.getOrientation(), epsilon)); assertTrue(expectedAngularVelocity.epsilonEquals(testedYoFrameSO3TrajectoryPoint.getAngularVelocity(), epsilon)); testedYoFrameSO3TrajectoryPoint.getAngularVelocity(actualAngularVelocity); assertTrue(expectedOrientation.geometricallyEquals(actualOrientation, epsilon)); assertTrue(expectedAngularVelocity.epsilonEquals(actualAngularVelocity, epsilon)); testedYoFrameSO3TrajectoryPoint.getAngularVelocity(actualFrameAngularVelocity); assertTrue(expectedOrientation.geometricallyEquals(actualFrameOrientation, epsilon)); assertTrue(expectedAngularVelocity.epsilonEquals(actualFrameAngularVelocity, epsilon)); testedYoFrameSO3TrajectoryPoint.getAngularVelocityIncludingFrame(actualFrameAngularVelocity); assertTrue(expectedOrientation.geometricallyEquals(actualFrameOrientation, epsilon)); assertTrue(expectedAngularVelocity.epsilonEquals(actualFrameAngularVelocity, epsilon));
/** * Sets this frame tuple to {@code other}. * * @param other the other frame tuple to copy the values and reference frame from. Not modified. */ default void setIncludingFrame(FrameQuaternionReadOnly other) { setIncludingFrame(other.getReferenceFrame(), other); } }
/** * Computes and returns the distance from this quaternion to {@code other}. * <p> * This method is equivalent to {@link #distance(QuaternionReadOnly)} but is more accurate when * computing the distance between two quaternions that are very close. Note that it is also more * expensive. * </p> * * @param other the other quaternion to measure the distance. Not modified. * @return the angle representing the distance between the two quaternions. It is contained in [0, * 2<i>pi</i>] * @throws ReferenceFrameMismatchException if reference frame of {@code this} and {@code other} do * not match. */ default double distancePrecise(FrameQuaternionReadOnly other) { checkReferenceFrameMatch(other); return QuaternionReadOnly.super.distancePrecise(other); }
assertEquals(expectedTime, testedFrameSE3TrajectoryPoint.getTime(), epsilon); assertTrue(expectedPosition.epsilonEquals(testedFrameSE3TrajectoryPoint.getGeometryObject().getEuclideanWaypoint().getPosition(), epsilon)); assertTrue(expectedOrientation.geometricallyEquals(testedFrameSE3TrajectoryPoint.getGeometryObject().getSO3Waypoint().getOrientation(), epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(testedFrameSE3TrajectoryPoint.getGeometryObject().getEuclideanWaypoint().getLinearVelocity(), epsilon)); assertTrue(expectedAngularVelocity.epsilonEquals(testedFrameSE3TrajectoryPoint.getGeometryObject().getSO3Waypoint().getAngularVelocity(), epsilon)); assertTrue(expectedOrientation.geometricallyEquals(actualOrientation, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualLinearVelocity, epsilon)); assertTrue(expectedAngularVelocity.epsilonEquals(actualAngularVelocity, epsilon)); assertTrue(expectedOrientation.geometricallyEquals(actualFrameOrientation, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, epsilon)); assertTrue(expectedAngularVelocity.epsilonEquals(actualFrameAngularVelocity, epsilon)); assertTrue(expectedOrientation.geometricallyEquals(actualFrameOrientation, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, epsilon)); assertTrue(expectedAngularVelocity.epsilonEquals(actualFrameAngularVelocity, epsilon));
/** * Computes and packs the orientation described by the orientation part of this pose as a rotation * vector. * <p> * WARNING: a rotation vector is different from a yaw-pitch-roll or Euler angles representation. A * rotation vector is equivalent to the axis of an axis-angle that is multiplied by the angle of the * same axis-angle. * </p> * * @param rotationVectorToPack the vector in which the rotation vector is stored. Modified. * @throws ReferenceFrameMismatchException if {@code rotationVectorToPack} is not expressed in the * same reference frame as {@code this}. */ default void getRotationVector(FixedFrameVector3DBasics rotationVectorToPack) { getOrientation().getRotationVector(rotationVectorToPack); }
/** * Sets this frame orientation 2D to {@code frameQuaternionReadOnly}. * * @param frameQuaternionReadOnly the frame quaternion to get the yaw angle and reference frame from. Not modified. */ default void setIncludingFrame(FrameQuaternionReadOnly frameQuaternionReadOnly) { setIncludingFrame(frameQuaternionReadOnly.getReferenceFrame(), frameQuaternionReadOnly); } }
public void setOrientationAndUpdate(FrameQuaternionReadOnly frameOrientation) { frameOrientation.checkReferenceFrameMatch(getParent()); originPose.setOrientation(frameOrientation); this.update(); }