/** * Tests on a per-component basis if this pose is equal to {@code other} with the tolerance * {@code epsilon}. * * @param other the query. Not modified. * @param epsilon the tolerance to use. * @return {@code true} if the two poses are equal, {@code false} otherwise. */ default boolean epsilonEquals(Pose3DReadOnly other, double epsilon) { return getPosition().epsilonEquals(other.getPosition(), epsilon) && getOrientation().epsilonEquals(other.getOrientation(), epsilon); }
/** * Sets the pose, i.e. position and orientation, of this shape. * * @param pose pose holding the new position and orientation for this shape. Not modified. */ public final void setPose(Pose3DReadOnly pose) { pose.get(shapePose); }
/** * Computes the absolute angle difference between this pose 3D and {@code other}. * * @param other the other pose 3D used to compute the orientation distance. Not modified. * @return the angle difference between {@code this.orientation} and {@code other.orientation}, it * is contained in [0, 2<i>pi</i>]. */ default double getOrientationDistance(Pose3DReadOnly other) { return getOrientation().distance(other.getOrientation()); }
/** * Computes the distances between the position part of the two poses. * * @param other the other pose used to measure the distance. Not modified. * @return the distance between the position part of the two poses. */ default double getPositionDistance(Pose3DReadOnly other) { return getPosition().distance(other.getPosition()); }
/** {@inheritDoc} */ @Override default int getJointConfiguration(int rowStart, DenseMatrix64F matrixToPack) { int index = rowStart; matrixToPack.set(index++, 0, getJointPose().getPitch()); matrixToPack.set(index++, 0, getJointPose().getX()); matrixToPack.set(index++, 0, getJointPose().getZ()); return rowStart + getDegreesOfFreedom(); }
/** * Tests if this pose is equal to the given {@code other} to an {@code epsilon}. * <p> * If the two poses have different frames, this method returns {@code false}. * </p> * * @param other the other pose to compare against this. Not modified. * @param epsilon the tolerance to use when comparing.. * @return {@code true} if the two poses are equal and are expressed in the same reference frame, * {@code false} otherwise. */ default boolean epsilonEquals(FramePose3DReadOnly other, double epsilon) { if (getReferenceFrame() != other.getReferenceFrame()) return false; return Pose3DReadOnly.super.epsilonEquals(other, epsilon); }
/** * Compares {@code this} to {@code other} to determine if the two poses are geometrically similar. * <p> * Two poses are geometrically equal if both their position and orientation are geometrically equal. * </p> * * @param other the pose to compare to. Not modified. * @param epsilon the tolerance of the comparison. * @return {@code true} if the two poses 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(FramePose3DReadOnly other, double epsilon) { checkReferenceFrameMatch(other); return Pose3DReadOnly.super.geometricallyEquals(other, epsilon); }
/** * Tests if this pose is exactly equal to {@code other}. * <p> * If the two poses have different frames, this method returns {@code false}. * </p> * * @param other the other pose to compare against this. Not modified. * @return {@code true} if the two poses are exactly equal and are expressed in the same reference * frame, {@code false} otherwise. */ default boolean equals(FramePose3DReadOnly other) { if (other == null || getReferenceFrame() != other.getReferenceFrame()) return false; return Pose3DReadOnly.super.equals(other); } }
/** * 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>]. */ default double getOrientationDistance(QuaternionReadOnly orientation) { return getOrientation().distance(orientation); }
/** * Gets the y-coordinate of the position part of this pose 3D. * * @return the y-coordinate of this pose 3D. */ default double getY() { return getPosition().getY(); }
/** * Asserts on a per component basis that the two pose 3Ds are equal 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 pose 3D. Not modified. * @param actual the actual pose 3D. 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 pose 3Ds are not equal. If only one of the arguments is * equal to {@code null}. */ public static void assertPose3DEquals(String messagePrefix, Pose3DReadOnly expected, Pose3DReadOnly actual, double epsilon, String format) { if (expected == null && actual == null) return; if (!(expected != null && actual != null)) throwNotEqualAssertionError(messagePrefix, expected, actual, format); if (!expected.epsilonEquals(actual, epsilon)) { throwNotEqualAssertionError(messagePrefix, expected, actual, format); } }
/** * Asserts that the two pose 3Ds 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 pose 3D. Not modified. * @param actual the actual pose 3D. 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 pose 3Ds do not represent the same geometry. If only one of * the arguments is equal to {@code null}. */ public static void assertPose3DGeometricallyEquals(String messagePrefix, Pose3DReadOnly expected, Pose3DReadOnly actual, double epsilon, String format) { if (expected == null && actual == null) return; if (!(expected != null && actual != null)) throwNotEqualAssertionError(messagePrefix, expected, actual, format); if (!expected.geometricallyEquals(actual, epsilon)) { throwNotEqualAssertionError(messagePrefix, expected, actual, format); } }
/** * Packs this pose 3D into the given {@code transformToPack}. * * @param transformToPack the rigid-body transform that is set to represent this pose 3D. Modified. */ default void get(RigidBodyTransform transformToPack) { transformToPack.set(getOrientation(), getPosition()); }
/** * Computes and returns the yaw angle from the yaw-pitch-roll representation of the orientation part * of this pose 3D. * <p> * WARNING: the Euler angles or yaw-pitch-roll representation is sensitive to gimbal lock and is * sometimes undefined. * </p> * * @return the yaw angle around the z-axis. */ default double getYaw() { return getOrientation().getYaw(); }
/** * Computes the distance between the position of this pose 3D and the given {@code point}. * * @param point the other point used to measure the distance. Not modified. * @return the distance between this pose and the given {@code point}. */ default double getPositionDistance(Point3DReadOnly point) { return getPosition().distance(point); }
/** * Computes the snap transform which snaps the given node to the given pose * * @param node * @param footstepPose * @return */ public static RigidBodyTransform computeSnapTransform(FootstepNode node, Pose3DReadOnly footstepPose) { RigidBodyTransform snapTransform = new RigidBodyTransform(); RigidBodyTransform stepTransform = new RigidBodyTransform(); footstepPose.get(stepTransform); FootstepNodeTools.getNodeTransform(node, snapTransform); snapTransform.preMultiplyInvertThis(stepTransform); return snapTransform; }
/** * Packs this pose 3D into the given {@code transformToPack}. * * @param transformToPack the quaternion-based transform that is set to represent this pose 3D. * Modified. */ default void get(QuaternionBasedTransform transformToPack) { transformToPack.set(getOrientation(), getPosition()); }
/** * Computes and returns the pitch angle from the yaw-pitch-roll representation of the orientation * part of this pose 3D. * <p> * WARNING: the Euler angles or yaw-pitch-roll representation is sensitive to gimbal lock and is * sometimes undefined. * </p> * * @return the pitch angle around the y-axis. */ default double getPitch() { return getOrientation().getPitch(); }
/** * Gets the x-coordinate of the position part of this pose 3D. * * @return the x-coordinate of this pose 3D. */ default double getX() { return getPosition().getX(); }
public void setPose(Pose3DReadOnly pose) { box3d.setPose(pose.getPosition(), pose.getOrientation()); }