/** * Sets this pose 3D to the {@code other} pose 3D. * * @param other the other pose 3D. Not modified. */ @Override public void set(Pose3D other) { Pose3DBasics.super.set(other); }
/** * Packs the pose of this shape into the given {@code poseToPack}. * * @param poseToPack the pose in which the position and orientation of this shape are stored. * Modified. */ public final void getPose(Pose3DBasics poseToPack) { poseToPack.set(shapePose); }
/** * 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 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 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); checkReferenceFrameMatch(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. * <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); }
private void getFloatingJointStateFromSCS() { FloatingJoint scsJoint = (FloatingJoint) robot.getRootJoints().get(0); RigidBodyTransform jointTransform3D = scsJoint.getJointTransform3D(); floatingJoint.getJointPose().set(jointTransform3D); floatingJoint.getFrameAfterJoint().update(); FrameVector3D linearVelocity = new FrameVector3D(); scsJoint.getVelocity(linearVelocity); linearVelocity.changeFrame(floatingJoint.getFrameAfterJoint()); floatingJoint.getJointTwist().set(scsJoint.getAngularVelocityInBody(), linearVelocity); }