/** * 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 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); }
/** * 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); }
if (footstepPlan.getLowLevelPlanGoal() != null) messager.submitMessage(LowLevelGoalPositionTopic, new Point3D(footstepPlan.getLowLevelPlanGoal().getPosition())); messager.submitMessage(LowLevelGoalOrientationTopic, new Quaternion(footstepPlan.getLowLevelPlanGoal().getOrientation()));