public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, FrameQuaternionReadOnly rawQuaternion) { this(namePrefix, nameSuffix, registry, maxRate, dt, rawQuaternion.getReferenceFrame(), rawQuaternion); }
/** * 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 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); }
/** * 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); } }
/** * Asserts on a per component basis that the two frame quaternions represent the same geometry to an * {@code epsilon}. * <p> * Note: the two arguments are considered to be equal if they are both equal to {@code null}. * </p> * * @param messagePrefix prefix to add to the error message. * @param expected the expected frame tuple. Not modified. * @param actual the actual frame tuple. Not modified. * @param epsilon the tolerance to use. * @param format the format to use for printing each component when an {@code AssertionError} is * thrown. * @throws AssertionError if the two quaternions do not represent the same geometry or not expressed * in the reference frame. If only one of the arguments is equal to {@code null}. */ public static void assertFrameQuaternionGeometricallyEquals(String messagePrefix, FrameQuaternionReadOnly expected, FrameQuaternionReadOnly actual, double epsilon, String format) { if (expected == null && actual == null) return; if (expected != null ^ actual != null) throwNotEqualAssertionError(messagePrefix, expected, actual, format); if (expected.getReferenceFrame() != actual.getReferenceFrame()) { throwNotEqualAssertionError(messagePrefix, expected, actual, format); } assertQuaternionGeometricallyEquals(messagePrefix, expected, actual, epsilon, format); }
public void setIncludingFrame(double time, FrameQuaternionReadOnly orientation, FrameVector3DReadOnly angularVelocity) { orientation.checkReferenceFrameMatch(angularVelocity); setToZero(orientation.getReferenceFrame()); geometryObject.set(time, orientation, angularVelocity); }
/** * Sets this frame quaternion to {@code other}. * <p> * If {@code other} is expressed in the frame as {@code this}, then this method is equivalent to * {@link #set(FrameQuaternionReadOnly)}. * </p> * <p> * If {@code other} is expressed in a different frame than {@code this}, then {@code this} is set to * {@code other} and then transformed to be expressed in {@code this.getReferenceFrame()}. * </p> * * @param other the other quaternion to copy the values from. Not modified. */ default void setMatchingFrame(FrameQuaternionReadOnly other) { QuaternionBasics.super.set(other); other.getReferenceFrame().transformFromThisToDesiredFrame(getReferenceFrame(), this); }
private void computeInitialConstraintError(FrameQuaternionReadOnly initialOrientation, double initialTime) { trajectory.compute(initialTime); trajectoryFrame.checkReferenceFrameMatch(initialOrientation.getReferenceFrame()); trajectory.getOrientation(tempOrientation); tempOrientation.changeFrame(trajectoryFrame); initialConstraintOrientationError.difference(tempOrientation, initialOrientation); }
private void computeFinalConstraintError(FrameQuaternionReadOnly finalOrientation, double finalTime) { trajectory.compute(finalTime); trajectoryFrame.checkReferenceFrameMatch(finalOrientation.getReferenceFrame()); trajectory.getOrientation(tempOrientation); tempOrientation.changeFrame(trajectoryFrame); finalConstraintOrientationError.difference(tempOrientation, finalOrientation); }
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));