/** * 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()); }
/** * 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(); }
/** * 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(); }
/** * 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); }
/** * Gets the z-coordinate of the position part of this pose 3D. * * @return the z-coordinate of this pose 3D. */ default double getZ() { return getPosition().getZ(); }
/** * 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); }
public void setPose(Pose3DReadOnly pose) { box3d.setPose(pose.getPosition(), pose.getOrientation()); }
/** * 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()); }
/** * 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()); }
/** * Tests on a per component basis, if this pose 3D is exactly equal to {@code other}. * * @param other the other pose 3D to compare against this. Not modified. * @return {@code true} if the two poses are exactly equal component-wise, {@code false} otherwise. */ default boolean equals(Pose3DReadOnly other) { if (other == null) return false; else return getPosition().equals(other.getPosition()) && getOrientation().equals(other.getOrientation()); } }
/** * 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. */ default boolean geometricallyEquals(Pose3DReadOnly other, double epsilon) { return getPosition().geometricallyEquals(other.getPosition(), epsilon) && getOrientation().geometricallyEquals(other.getOrientation(), epsilon); }
/** * Gets the position and orientation parts of this pose 3D. * * @param positionToPack tuple in which the position is stored. Modified. * @param orientationToPack orientation in which the orientation is stored. Modified. */ default void get(Tuple3DBasics positionToPack, Orientation3DBasics orientationToPack) { positionToPack.set(getPosition()); orientationToPack.set(getOrientation()); }
/** * Sets this pose 3D to the {@code other} pose 3D. * * @param other the other pose 3D. Not modified. */ default void set(Pose3DReadOnly other) { setPosition(other.getPosition()); setOrientation(other.getOrientation()); }
/** * Tests if this pose contains a {@link Double#NaN}. * * @return {@code true} if this pose contains a {@link Double#NaN}, {@code false} otherwise. */ default boolean containsNaN() { return getOrientation().containsNaN() || getPosition().containsNaN(); }
/** * Sets this pose 2D to the given {@code pose3DReadOnly}. * * @param pose3DReadOnly the pose 3D. Not modified. */ default void set(Pose3DReadOnly pose3DReadOnly) { setPosition(pose3DReadOnly.getPosition()); setOrientation(pose3DReadOnly.getOrientation()); }
/** * Performs a linear interpolation from {@code pose1} to {@code pose2} given the percentage * {@code alpha}. * <p> * this.position = (1.0 - alpha) * pose1.position + alpha * pose2.position<br> * this.orientation = (1.0 - alpha) * pose1.orientation + alpha * pose2.orientation * </p> * * @param pose1 the first pose 3D used in the interpolation. Not modified. * @param pose2 the second pose 3D used in the interpolation. Not modified. * @param alpha the percentage to use for the interpolation. A value of 0 will result in setting * {@code this} to {@code pose1}, while a value of 1 is equivalent to setting {@code this} * to {@code pose2}. */ default void interpolate(Pose3DReadOnly pose1, Pose3DReadOnly pose2, double alpha) { getPosition().interpolate(pose1.getPosition(), pose2.getPosition(), alpha); getOrientation().interpolate(pose1.getOrientation(), pose2.getOrientation(), alpha); }
/** {@inheritDoc} */ @Override default void getJointConfiguration(RigidBodyTransform jointTransform) { jointTransform.set(getJointPose().getOrientation(), getJointPose().getPosition()); } }
private static Graphics3DObject createEndEffectorKeyFrameVisualization(Pose3DReadOnly pose) { Graphics3DObject object = new Graphics3DObject(); object.transform(new RigidBodyTransform(pose.getOrientation(), pose.getPosition())); object.addSphere(0.01); return object; }
/** * Performs a linear interpolation from {@code this} to {@code other} given the percentage * {@code alpha}. * <p> * this.position = (1.0 - alpha) * this.position + alpha * other.position<br> * this.orientation = (1.0 - alpha) * this.orientation + alpha * other.orientation * </p> * * @param other the other pose 3D used for the interpolation. Not modified. * @param alpha the percentage used for the interpolation. A value of 0 will result in not modifying * {@code this}, while a value of 1 is equivalent to setting {@code this} to * {@code other}. */ default void interpolate(Pose3DReadOnly other, double alpha) { getPosition().interpolate(other.getPosition(), alpha); getOrientation().interpolate(other.getOrientation(), alpha); }
/** {@inheritDoc} */ @Override default int getJointConfiguration(int rowStart, DenseMatrix64F matrixToPack) { getJointPose().getOrientation().get(rowStart, matrixToPack); getJointPose().getPosition().get(rowStart + 4, matrixToPack); return rowStart + getConfigurationMatrixSize(); }