/** * Gets the position and orientation. * * @param positionToPack the tuple used to store the position. Modified. * @param orientationToPack the orientation used to store the orientation. Modified. */ default void get(Tuple3DBasics positionToPack, FrameOrientation3DBasics orientationToPack) { positionToPack.set(getPosition()); orientationToPack.setIncludingFrame(getOrientation()); }
public void set(FramePose3DReadOnly framePose) { framePose.checkReferenceFrameMatch(getReferenceFrame()); position.set(framePose.getPosition()); orientation.set(framePose.getOrientation()); }
public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, FramePose3DReadOnly rawPose) { this(namePrefix, nameSuffix, registry, maxRate, dt, rawPose, rawPose.getReferenceFrame()); }
/** * 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. * @throws ReferenceFrameMismatchException if {@code other} is not expressed in the same reference * frame as {@code this}. */ default double getPositionDistance(FramePose3DReadOnly other) { return getPositionDistance(other.getPosition()); }
/** * 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>]. * @throws ReferenceFrameMismatchException if {@code other} is not expressed in the same reference * frame as {@code this}. */ default double getOrientationDistance(FramePose3DReadOnly other) { return getOrientationDistance(other.getOrientation()); }
public void setFromAnklePose(FramePose3DReadOnly anklePose, RigidBodyTransform transformFromAnkleToSole) { tempTransform.setRotation(anklePose.getOrientation()); tempTransform.setTranslation(anklePose.getPosition()); tempTransform.multiplyInvertOther(transformFromAnkleToSole); footstepPose.setIncludingFrame(anklePose.getReferenceFrame(), tempTransform); }
/** * 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>]. * @throws ReferenceFrameMismatchException if {@code orientation} is not expressed in the same * reference frame as {@code this}. */ default double getOrientationDistance(FrameQuaternionReadOnly orientation) { return getOrientation().distance(orientation); }
/** * 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}. * @throws ReferenceFrameMismatchException if {@code point} is not expressed in the same reference * frame as {@code this}. */ default double getPositionDistance(FramePoint3DReadOnly point) { return getPosition().distance(point); }
/** * 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); }
/** * Computes and packs the orientation described by the orientation part of this pose as a rotation * vector. * <p> * WARNING: a rotation vector is different from a yaw-pitch-roll or Euler angles representation. A * rotation vector is equivalent to the axis of an axis-angle that is multiplied by the angle of the * same axis-angle. * </p> * * @param rotationVectorToPack the vector in which the rotation vector is stored. Modified. */ default void getRotationVector(FrameVector3DBasics rotationVectorToPack) { getOrientation().getRotationVector(rotationVectorToPack); }
public FramePose3DReadOnly getLowLevelPlanGoal() { if (hasLowLevelPlanGoal()) lowLevelPlanGoal.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); return lowLevelPlanGoal; } }
public void blendInitialConstraint(FramePose3DReadOnly initialPose, double initialTime, double blendDuration) { blendedPositionTrajectory.blendInitialConstraint(initialPose.getPosition(), initialTime, blendDuration); blendedOrientationTrajectory.blendInitialConstraint(initialPose.getOrientation(), initialTime, blendDuration); }
/** * Computes and packs the orientation described by the orientation part of this pose as a rotation * vector. * <p> * WARNING: a rotation vector is different from a yaw-pitch-roll or Euler angles representation. A * rotation vector is equivalent to the axis of an axis-angle that is multiplied by the angle of the * same axis-angle. * </p> * * @param rotationVectorToPack the vector in which the rotation vector is stored. Modified. * @throws ReferenceFrameMismatchException if {@code rotationVectorToPack} is not expressed in the * same reference frame as {@code this}. */ default void getRotationVector(FixedFrameVector3DBasics rotationVectorToPack) { getOrientation().getRotationVector(rotationVectorToPack); }
/** * 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); }
public void setPose(FramePose3DReadOnly pose) { setPosition(pose.getPosition()); setOrientation(pose.getOrientation()); }
/** * 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); } }
public void setMatchingFrame(FramePose3DReadOnly framePose) { position.setMatchingFrame(framePose.getPosition()); orientation.setMatchingFrame(framePose.getOrientation()); }
public PoseReferenceFrame(String frameName, FramePose3DReadOnly pose) { this(frameName, pose.getReferenceFrame()); setPoseAndUpdate(pose); }
/** * Gets the position and orientation. * * @param positionToPack the tuple used to store the position. Modified. * @param orientationToPack the orientation used to store the orientation. Modified. */ default void get(FrameTuple3DBasics positionToPack, Orientation3DBasics orientationToPack) { positionToPack.setIncludingFrame(getPosition()); orientationToPack.set(getOrientation()); }
/** * Sets this frame pose 2D to the x, y, yaw components and reference frame of the given * {@code framePose3DReadOnly}. * * @param framePose3DReadOnly the frame pose 3D. Not modified. */ default void setIncludingFrame(FramePose3DReadOnly framePose3DReadOnly) { setIncludingFrame(framePose3DReadOnly.getReferenceFrame(), framePose3DReadOnly); } }