us.ihmc.euclid.referenceFrame.interfaces.FixedFramePose3DBasics Maven / Gradle / Ivy
package us.ihmc.euclid.referenceFrame.interfaces;
import us.ihmc.euclid.geometry.interfaces.Pose2DReadOnly;
import us.ihmc.euclid.geometry.interfaces.Pose3DBasics;
import us.ihmc.euclid.geometry.interfaces.Pose3DReadOnly;
import us.ihmc.euclid.orientation.interfaces.Orientation3DBasics;
import us.ihmc.euclid.orientation.interfaces.Orientation3DReadOnly;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.referenceFrame.exceptions.ReferenceFrameMismatchException;
import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly;
/**
* Write and read interface for a 3D pose expressed in a constant reference frame, i.e. the
* reference frame of this object cannot be changed via this interface.
*
* In addition to representing a {@link Pose3DBasics}, a {@link ReferenceFrame} is associated to a
* {@code FixedFramePose3DBasics}. This allows, for instance, to enforce, at runtime, that
* operations on poses occur in the same coordinate system.
*
*
* Because a {@code FixedFramePose3DBasics} extends {@code Pose3DBasics}, it is compatible with
* methods only requiring {@code Pose3DBasics}. However, these methods do NOT assert that the
* operation occur in the proper coordinate system. Use this feature carefully and always prefer
* using methods requiring {@code FixedFramePose3DBasics}.
*
*/
public interface FixedFramePose3DBasics extends FramePose3DReadOnly, Pose3DBasics
{
/** {@inheritDoc} */
@Override
FixedFramePoint3DBasics getPosition();
/** {@inheritDoc} */
@Override
FixedFrameQuaternionBasics getOrientation();
/** {@inheritDoc} */
@Override
default FixedFramePoint3DBasics getTranslation()
{
return getPosition();
}
/** {@inheritDoc} */
@Override
default FixedFrameQuaternionBasics getRotation()
{
return getOrientation();
}
/**
* 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 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 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);
}
/**
* 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 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);
}
/**
* 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 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, orientation);
Pose3DBasics.super.set(position, orientation);
}
/**
* Sets this pose 3D to be the same as the given one expressed in the reference frame of this.
*
* If {@code other} is expressed in the frame as {@code this}, then this method is equivalent to
* {@link #set(FramePose3DReadOnly)}.
*
*
* 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()}.
*
*
* @param other the other frame pose 3D to set this to. Not modified.
*/
default void setMatchingFrame(FramePose3DReadOnly other)
{
setMatchingFrame(other.getReferenceFrame(), other);
}
/**
* Sets this pose 3D to be the same as the given one expressed in the reference frame of this.
*
* If {@code other} is expressed in the frame as {@code this}, then this method is equivalent to
* {@link #set(FramePose3DReadOnly)}.
*
*
* 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()}.
*
*
* @param other the other frame pose 3D to set this to. Not modified.
*/
default void setMatchingFrame(ReferenceFrame referenceFrame, Pose3DReadOnly pose3DReadOnly)
{
set(pose3DReadOnly);
referenceFrame.transformFromThisToDesiredFrame(getReferenceFrame(), this);
}
/**
* Sets both position and orientation while performing the necessary transformation to be expressed
* in the frame of this.
*
* If the arguments are expressed in the frame as {@code this}, then this method is equivalent to
* {@link #set(FrameTuple3DReadOnly, FrameOrientation3DReadOnly)}.
*
*
* If the arguments are expressed in a different frame than {@code this}, then {@code this} is set
* to {@code position} and {@code orientation} and then transformed to be expressed in
* {@code this.getReferenceFrame()}.
*
*
* @param position the tuple with the new position coordinates. Not modified.
* @param orientation the new orientation. Not modified.
* @throws ReferenceFrameMismatchException if the arguments are expressed in different frames.
*/
default void setMatchingFrame(FrameTuple3DReadOnly position, FrameOrientation3DReadOnly orientation)
{
position.checkReferenceFrameMatch(orientation);
setMatchingFrame(position.getReferenceFrame(), position, orientation);
}
/**
* Sets both position and orientation while performing the necessary transformation to be expressed
* in the frame of this.
*
* If the arguments are expressed in the frame as {@code this}, then this method is equivalent to
* {@link #set(FrameTuple3DReadOnly, FrameOrientation3DReadOnly)}.
*
*
* If the arguments are expressed in a different frame than {@code this}, then {@code this} is set
* to {@code position} and {@code orientation} and then transformed to be expressed in
* {@code this.getReferenceFrame()}.
*
*
* @param position the tuple with the new position coordinates. Not modified.
* @param orientation the new orientation. Not modified.
*/
default void setMatchingFrame(ReferenceFrame referenceFrame, Tuple3DReadOnly position, Orientation3DReadOnly orientation)
{
set(position, orientation);
referenceFrame.transformFromThisToDesiredFrame(getReferenceFrame(), this);
}
/**
* Adds the given {@code translation} to this pose 3D assuming it is expressed in the coordinates in
* which this pose is expressed.
*
* If the {@code translation} is expressed in the local coordinates described by this pose 3D, use
* {@link #appendTranslation(FrameTuple3DReadOnly)}.
*
*
* @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 prependTranslation(FrameTuple3DReadOnly translation)
{
checkReferenceFrameMatch(translation);
Pose3DBasics.super.prependTranslation(translation);
}
/**
* 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.
*
* Use this method if the {@code translation} is expressed in the local coordinates described by
* this pose 3D. Otherwise, use {@link #prependTranslation(FrameTuple3DReadOnly)}.
*
*
* @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);
}
/**
* Appends the given rotation to this pose 3D.
*
* More precisely, the position part is unchanged while the orientation part is updated as
* follows:
* {@code this.orientation = this.orientation * rotation}
*
*
* @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);
}
/**
* Performs a linear interpolation from {@code this} to {@code other} given the percentage
* {@code alpha}.
*
* this.position = (1.0 - alpha) * this.position + alpha * other.position
* this.orientation = (1.0 - alpha) * this.orientation + alpha * other.orientation
*
*
* @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}.
* @throws ReferenceFrameMismatchException if {@code this} and {@code other} are not expressed in
* the same reference frame.
*/
default void interpolate(FramePose3DReadOnly other, double alpha)
{
checkReferenceFrameMatch(other);
Pose3DBasics.super.interpolate(other, alpha);
}
/**
* Performs a linear interpolation from {@code pose1} to {@code pose2} given the percentage
* {@code alpha}.
*
* this.position = (1.0 - alpha) * pose1.position + alpha * pose2.position
* this.orientation = (1.0 - alpha) * pose1.orientation + alpha * pose2.orientation
*
*
* @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} and {@code pose1} are not expressed in
* the same reference frame.
*/
default void interpolate(FramePose3DReadOnly pose1, Pose3DReadOnly pose2, double alpha)
{
checkReferenceFrameMatch(pose1);
Pose3DBasics.super.interpolate(pose1, pose2, alpha);
}
/**
* Performs a linear interpolation from {@code pose1} to {@code pose2} given the percentage
* {@code alpha}.
*
* this.position = (1.0 - alpha) * pose1.position + alpha * pose2.position
* this.orientation = (1.0 - alpha) * pose1.orientation + alpha * pose2.orientation
*
*
* @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} and {@code pose2} are not expressed in
* the same reference frame.
*/
default void interpolate(Pose3DReadOnly pose1, FramePose3DReadOnly pose2, double alpha)
{
checkReferenceFrameMatch(pose2);
Pose3DBasics.super.interpolate(pose1, pose2, alpha);
}
/**
* Performs a linear interpolation from {@code pose1} to {@code pose2} given the percentage
* {@code alpha}.
*
* this.position = (1.0 - alpha) * pose1.position + alpha * pose2.position
* this.orientation = (1.0 - alpha) * pose1.orientation + alpha * pose2.orientation
*
*
* @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, pose2);
Pose3DBasics.super.interpolate(pose1, pose2, alpha);
}
}