/** * 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()); }
/** * 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 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); }
/** * 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 setMatchingFrame(FramePose3DReadOnly framePose) { position.setMatchingFrame(framePose.getPosition()); orientation.setMatchingFrame(framePose.getOrientation()); }
/** * 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()); }
public void blendInitialConstraint(FramePose3DReadOnly initialPose, double initialTime, double blendDuration) { blendedPositionTrajectory.blendInitialConstraint(initialPose.getPosition(), initialTime, blendDuration); blendedOrientationTrajectory.blendInitialConstraint(initialPose.getOrientation(), initialTime, blendDuration); }
public void setPose(FramePose3DReadOnly pose) { setPosition(pose.getPosition()); setOrientation(pose.getOrientation()); }
public void blendFinalConstraint(FramePose3DReadOnly finalPose, double finalTime, double blendDuration) { blendedPositionTrajectory.blendFinalConstraint(finalPose.getPosition(), finalTime, blendDuration); blendedOrientationTrajectory.blendFinalConstraint(finalPose.getOrientation(), finalTime, blendDuration); }
/** * 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, FrameOrientation3DBasics orientationToPack) { positionToPack.setIncludingFrame(getPosition()); orientationToPack.setIncludingFrame(getOrientation()); }
/** * 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. * @throws ReferenceFrameMismatchException if {@code positionToPack} is not expressed in the same * reference frame as this frame pose. */ default void get(FixedFrameTuple3DBasics positionToPack, Orientation3DBasics orientationToPack) { positionToPack.set(getPosition()); orientationToPack.set(getOrientation()); }
/** * 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. * @throws ReferenceFrameMismatchException if {@code orientationToPack} is not expressed in the same * reference frame as this frame pose. */ default void get(Tuple3DBasics positionToPack, FixedFrameOrientation3DBasics orientationToPack) { positionToPack.set(getPosition()); orientationToPack.set(getOrientation()); }
/** * 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. * @throws ReferenceFrameMismatchException if {@code positionToPack} and/or * {@code orientationToPack} are not expressed in the same reference frame as this frame * pose. */ default void get(FixedFrameTuple3DBasics positionToPack, FixedFrameOrientation3DBasics orientationToPack) { positionToPack.set(getPosition()); orientationToPack.set(getOrientation()); }
/** * Sets this frame pose to {@code other}. * <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} once transformed to be expressed in {@code this.getReferenceFrame()}. * </p> * * @param other the other frame pose to set this to. Not modified. */ public void setMatchingFrame(FramePose3DReadOnly other) { position.setMatchingFrame(other.getPosition()); orientation.setMatchingFrame(other.getOrientation()); }
public void set(FramePose3DReadOnly framePose) { framePose.checkReferenceFrameMatch(getReferenceFrame()); position.set(framePose.getPosition()); orientation.set(framePose.getOrientation()); }
public void update(FramePose3DReadOnly framePoseUnfiltered) { checkReferenceFrameMatch(framePoseUnfiltered); position.update(framePoseUnfiltered.getPosition()); orientation.update(framePoseUnfiltered.getOrientation()); set(position, orientation); }
public void setFromAnklePose(FramePose3DReadOnly anklePose, RigidBodyTransform transformFromAnkleToSole) { tempTransform.setRotation(anklePose.getOrientation()); tempTransform.setTranslation(anklePose.getPosition()); tempTransform.multiplyInvertOther(transformFromAnkleToSole); footstepPose.setIncludingFrame(anklePose.getReferenceFrame(), tempTransform); }
public void blendInitialConstraint(FramePose3DReadOnly initialPose, TwistReadOnly initialTwist, double initialTime, double blendDuration) { tempVelocity.setIncludingFrame(initialTwist.getLinearPart()); tempAngularVelocity.setIncludingFrame(initialTwist.getAngularPart()); blendedPositionTrajectory.blendInitialConstraint(initialPose.getPosition(), tempVelocity, initialTime, blendDuration); blendedOrientationTrajectory.blendInitialConstraint(initialPose.getOrientation(), tempAngularVelocity, initialTime, blendDuration); }
private RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, FramePose3DReadOnly rawPose, ReferenceFrame referenceFrame) { if (rawPose != null) { this.position = new RateLimitedYoFramePoint(namePrefix, "Position" + nameSuffix, registry, maxRate, dt, rawPose.getPosition()); this.orientation = new RateLimitedYoFrameQuaternion(namePrefix, "Orientation" + nameSuffix, registry, maxRate, dt, rawPose.getOrientation()); } else { this.position = new RateLimitedYoFramePoint(namePrefix, "Position" + nameSuffix, registry, maxRate, dt, referenceFrame); this.orientation = new RateLimitedYoFrameQuaternion(namePrefix, "Orientation" + nameSuffix, registry, maxRate, dt, referenceFrame); } reset(); }
public void blendFinalConstraint(FramePose3DReadOnly finalPose, TwistReadOnly finalTwist, double finalTime, double blendDuration) { tempVelocity.setIncludingFrame(finalTwist.getLinearPart()); tempAngularVelocity.setIncludingFrame(finalTwist.getAngularPart()); blendedPositionTrajectory.blendFinalConstraint(finalPose.getPosition(), tempVelocity, finalTime, blendDuration); blendedOrientationTrajectory.blendFinalConstraint(finalPose.getOrientation(), tempAngularVelocity, finalTime, blendDuration); }