How to use

set

method

in

us.ihmc.euclid.geometry.interfaces.Pose3DBasics

/**
* 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);
}