/** * Sets both position and orientation. * * @param position the tuple with the new position coordinates. Not modified. * @param orientation the new orientation. Not modified. * @throws ReferenceFrameMismatchException if {@code this}, {@code position}, and * {@code orientation} are not expressed in the same reference frame. */ default void set(FrameTuple3DReadOnly position, FrameOrientation3DReadOnly orientation) { checkReferenceFrameMatch(position); checkReferenceFrameMatch(orientation); Pose3DBasics.super.set(position, orientation); }
/** * Sets this pose 3D to represent the pose of the given {@code referenceFrame} expressed in * {@code this.getReferenceFrame()}. * * @param referenceFrame the reference frame of interest. */ default void setFromReferenceFrame(ReferenceFrame referenceFrame) { setToZero(); referenceFrame.transformFromThisToDesiredFrame(getReferenceFrame(), this); }
/** * Sets the pose from the given {@code pose3DReadOnly} that is expressed in the given * {@code referenceFrame}. * * @param referenceFrame the reference frame in which the given {@code pose3DReadOnly} is expressed * in. * @param pose3DReadOnly the pose 3D used to set the pose of this frame pose 3D. Not modified. * @throws ReferenceFrameMismatchException if {@code this.getReferenceFrame() != referenceFrame}. */ default void set(ReferenceFrame referenceFrame, Pose3DReadOnly pose3DReadOnly) { checkReferenceFrameMatch(referenceFrame); set(pose3DReadOnly); }
public void getFramePose(FixedFramePose3DBasics framePoseToPack) { orientation.getFrameOrientationIncludingFrame(tempFrameOrientation); framePoseToPack.setPosition(position); framePoseToPack.setOrientation(tempFrameOrientation); }
/** * Sets this pose 3D to be the same as the given one expressed in the reference frame of this. * <p> * If {@code other} is expressed in the frame as {@code this}, then this method is equivalent to * {@link #set(FramePose3DReadOnly)}. * </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 frame pose 3D to set this to. Not modified. */ default void setMatchingFrame(FramePose3DReadOnly other) { Pose3DBasics.super.set(other); other.getReferenceFrame().transformFromThisToDesiredFrame(getReferenceFrame(), this); }
@Test public void testSetMatchingFrame() throws Exception { Random random = new Random(544354); for (int i = 0; i < ITERATIONS; i++) { ReferenceFrame sourceFrame = EuclidFrameRandomTools.nextReferenceFrame(random); ReferenceFrame destinationFrame = EuclidFrameRandomTools.nextReferenceFrame(random); FramePose3DReadOnly source = EuclidFrameRandomTools.nextFramePose3D(random, sourceFrame); FixedFramePose3DBasics actual = EuclidFrameRandomTools.nextFramePose3D(random, destinationFrame); actual.setMatchingFrame(source); FramePose3D expected = new FramePose3D(source); expected.changeFrame(destinationFrame); EuclidGeometryTestTools.assertPose3DEquals(expected, actual, EPSILON); } } }
/** * 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}. * @throws ReferenceFrameMismatchException if {@code this}, {@code pose1}, and {@code pose2} are not * expressed in the same reference frame. */ default void interpolate(FramePose3DReadOnly pose1, FramePose3DReadOnly pose2, double alpha) { checkReferenceFrameMatch(pose1); checkReferenceFrameMatch(pose2); Pose3DBasics.super.interpolate(pose1, pose2, alpha); } }
/** * Sets the pose to represent the same pose as the given {@code pose2DReadOnly} that is expressed in * the given {@code referenceFrame}. * * @param referenceFrame the reference frame in which the given {@code pose2DReadOnly} is expressed * in. * @param pose2DReadOnly the pose 2D used to set the pose of this frame pose 3D. Not modified. * @throws ReferenceFrameMismatchException if {@code this.getReferenceFrame() != referenceFrame}. */ default void set(ReferenceFrame referenceFrame, Pose2DReadOnly pose2DReadOnly) { checkReferenceFrameMatch(referenceFrame); set(pose2DReadOnly); }
/** * Sets this frame pose 3D to the represent the same pose as the given {@code framePose2DReadOnly}. * * @param framePose2DReadOnly the other frame pose 2D. Not modified. * @throws ReferenceFrameMismatchException if {@code this} and {@code framePose2DReadOnly} are not * expressed in the same reference frame. */ default void set(FramePose2DReadOnly framePose2DReadOnly) { checkReferenceFrameMatch(framePose2DReadOnly); Pose3DBasics.super.set(framePose2DReadOnly); }
/** * Sets this frame pose 3D to the {@code other} frame pose 3D. * * @param other the other frame pose 3D. Not modified. * @throws ReferenceFrameMismatchException if {@code this} and {@code other} are not expressed in * the same reference frame. */ default void set(FramePose3DReadOnly other) { checkReferenceFrameMatch(other); Pose3DBasics.super.set(other); }
/** * Sets the position from the given frame tuple 3D. * * @param position the tuple with the new position coordinates. Not modified. * @throws ReferenceFrameMismatchException if {@code this} and {@code position} are not expressed in * the same reference frame. */ default void setPosition(FrameTuple3DReadOnly position) { checkReferenceFrameMatch(position); Pose3DBasics.super.setPosition(position); }
/** * Sets the position from the given frame tuple 2D and the given {@code z} coordinate. * * @param position the tuple with the x and y position coordinates. Not modified. * @param z the new z-coordinate. * @throws ReferenceFrameMismatchException if {@code this} and {@code position} are not expressed in * the same reference frame. */ default void setPosition(FrameTuple2DReadOnly position, double z) { checkReferenceFrameMatch(position); Pose3DBasics.super.setPosition(position, z); }
/** * Sets both position and orientation. * * @param position the tuple with the new position coordinates. Not modified. * @param orientation the new orientation. Not modified. * @throws ReferenceFrameMismatchException if {@code this} and {@code orientation} are not expressed * in the same reference frame. */ default void set(Tuple3DReadOnly position, FrameOrientation3DReadOnly orientation) { checkReferenceFrameMatch(orientation); Pose3DBasics.super.set(position, orientation); }
/** * Sets the orientation from the given frame orientation 2D. * * @param orientation the orientation with the new angle value for this. Not modified. * @throws ReferenceFrameMismatchException if {@code this} and {@code orientation} are not expressed * in the same reference frame. */ default void setOrientation(FrameOrientation2DReadOnly orientation) { checkReferenceFrameMatch(orientation); Pose3DBasics.super.setOrientation(orientation); }
/** * Sets the orientation from the given frame orientation. * * @param orientation the orientation to set the orientation part of this frame pose. Not modified. * @throws ReferenceFrameMismatchException if {@code this} and {@code orientation} are not expressed * in the same reference frame. */ default void setOrientation(FrameOrientation3DReadOnly orientation) { checkReferenceFrameMatch(orientation); Pose3DBasics.super.setOrientation(orientation); }
/** * Sets both position and orientation. * * @param position the tuple with the new position coordinates. Not modified. * @param orientation the new orientation. Not modified. * @throws ReferenceFrameMismatchException if {@code this} and {@code position} are not expressed in * the same reference frame. */ default void set(FrameTuple3DReadOnly position, Orientation3DReadOnly orientation) { checkReferenceFrameMatch(position); Pose3DBasics.super.set(position, orientation); }
/** * Rotates the position part of this pose 3D by the given {@code rotation} and prepends it to the * orientation part. * * @param rotation the rotation to prepend to this pose 3D. Not modified. * @throws ReferenceFrameMismatchException if {@code this} and {@code rotation} are not expressed in * the same reference frame. * @see Orientation3DBasics#prepend(Orientation3DReadOnly) */ default void prependRotation(FrameOrientation3DReadOnly rotation) { checkReferenceFrameMatch(rotation); Pose3DBasics.super.prependRotation(rotation); }
/** * Rotates, then adds the given {@code translation} to this pose 3D. * <p> * Use this method if the {@code translation} is expressed in the local coordinates described by * this pose 3D. Otherwise, use {@link #prependTranslation(FrameTuple3DReadOnly)}. * </p> * * @param translation tuple containing the translation to apply to this pose 3D. Not modified. * @throws ReferenceFrameMismatchException if {@code this} and {@code translation} are not expressed * in the same reference frame. */ default void appendTranslation(FrameTuple3DReadOnly translation) { checkReferenceFrameMatch(translation); Pose3DBasics.super.appendTranslation(translation); }
/** * Sets the position from the given frame tuple 2D. * <p> * The z component remains unchanged. * </p> * * @param position the tuple with the new position coordinates. Not modified. * @throws ReferenceFrameMismatchException if {@code this} and {@code position} are not expressed in * the same reference frame. */ default void setPosition(FrameTuple2DReadOnly position) { checkReferenceFrameMatch(position); Pose3DBasics.super.setPosition(position); }
/** * Appends the given rotation to this pose 3D. * <p> * More precisely, the position part is unchanged while the orientation part is updated as * follows:<br> * {@code this.orientation = this.orientation * rotation} * </p> * * @param rotation the rotation to append to this pose 3D. Not modified. * @throws ReferenceFrameMismatchException if {@code this} and {@code rotation} are not expressed in * the same reference frame. * @see Orientation3DBasics#append(Orientation3DReadOnly) */ default void appendRotation(FrameOrientation3DReadOnly rotation) { checkReferenceFrameMatch(rotation); Pose3DBasics.super.appendRotation(rotation); }