How to use

Pose3DBasics

in

us.ihmc.euclid.geometry.interfaces

/**
* Prepends a rotation about the z-axis to this pose 3D: Rotates the position part and prepends the
* rotation to the orientation part.
*
* @param yaw the angle to rotate about the z-axis.
*/
default void prependYawRotation(double yaw)
{
 RotationMatrixTools.applyYawRotation(yaw, getPosition(), getPosition());
 getOrientation().prependYawRotation(yaw);
}

/**
* Rotates, then adds the translation (x, y, z) to this pose 3D.
* <p>
* Use this method if the translation (x, y, z) is expressed in the local coordinates described by
* this pose 3D. Otherwise, use {@link #prependTranslation(double, double, double)}.
* </p>
*
* @param x the translation distance along the x-axis.
* @param y the translation distance along the y-axis.
* @param z the translation distance along the z-axis.
*/
default void appendTranslation(double x, double y, double z)
{
 double thisX = getX();
 double thisY = getY();
 double thisZ = getZ();
 setPosition(x, y, z);
 getOrientation().transform(getPosition());
 getPosition().add(thisX, thisY, thisZ);
}

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

/**
* Sets both position and orientation.
*
* @param position the tuple with the new position coordinates. Not modified.
* @param orientation the new orientation. Not modified.
*/
default void set(Tuple3DReadOnly position, Orientation3DReadOnly orientation)
{
 setOrientation(orientation);
 setPosition(position);
}

/**
* Sets all the components of this pose 3D.
* <p>
* WARNING: the Euler angles or yaw-pitch-roll representation is sensitive to gimbal lock and is
* sometimes undefined.
* </p>
*
* @param x the x-coordinate of the position.
* @param y the y-coordinate of the position.
* @param z the z-coordinate of the position.
* @param yaw the angle to rotate about the z-axis.
* @param pitch the angle to rotate about the y-axis.
* @param roll the angle to rotate about the x-axis.
*/
default void set(double x, double y, double z, double yaw, double pitch, double roll)
{
 setPosition(x, y, z);
 setOrientationYawPitchRoll(yaw, pitch, roll);
}

/**
* Sets the orientation part of this pose 3D with the given orientation.
*
* @param orientation the orientation used to set this pose's orientation. Not modified.
*/
default void setOrientation(Orientation3DReadOnly orientation)
{
 getOrientation().set(orientation);
}

public void updateFullRobotModel(KinematicsToolboxOutputStatus solution)
{
 if (jointsHashCode != solution.getJointNameHash())
   throw new RuntimeException("Hashes are different.");
 for (int i = 0; i < oneDoFJoints.length; i++)
 {
   float q = solution.getDesiredJointAngles().get(i);
   OneDoFJointBasics joint = oneDoFJoints[i];
   joint.setQ(q);
 }
 Vector3D translation = solution.getDesiredRootTranslation();
 rootJoint.getJointPose().setPosition(translation.getX(), translation.getY(), translation.getZ());
 Quaternion orientation = solution.getDesiredRootOrientation();
 rootJoint.getJointPose().getOrientation().setQuaternion(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getS());
 fullRobotModelToUseForConversion.updateFrames();
}

/**
* Sets the x-coordinate of the position.
*
* @param x the x-coordinate of the position.
*/
default void setX(double x)
{
 getPosition().setX(x);
}

                         getRandomVector(random));
sixDoFJoint.setJointPosition(getRandomVector(random));
sixDoFJoint.getJointPose().setOrientationYawPitchRoll(random.nextDouble(), random.nextDouble(), random.nextDouble());
sixDoFJoint.setJointAcceleration(jointAcceleration);
elevator.updateFramesRecursively();
rotationMatrix.set(sixDoFJoint.getJointPose().getOrientation());

/** {@inheritDoc} */
@Override
default void setJointPosition(Tuple3DReadOnly jointTranslation)
{
 getJointPose().setPosition(jointTranslation);
}

private void setRandomPosition(FloatingJoint floatingJoint, SixDoFJoint sixDoFJoint)
{
 Point3D rootPosition = new Point3D(random.nextDouble(), random.nextDouble(), random.nextDouble());
 double yaw = random.nextDouble();
 double pitch = random.nextDouble();
 double roll = random.nextDouble();
 floatingJoint.setPosition(rootPosition);
 floatingJoint.setYawPitchRoll(yaw, pitch, roll);
 sixDoFJoint.setJointPosition(rootPosition);
 sixDoFJoint.getJointPose().setOrientationYawPitchRoll(yaw, pitch, roll);
}

/**
* Sets the orientation from the given frame orientation 2D.
*
* @param orientation the orientation with the new angle value for this. Not modified.
* @throws ReferenceFrameMismatchException if {@code this} and {@code orientation} are not expressed
*            in the same reference frame.
*/
default void setOrientation(FrameOrientation2DReadOnly orientation)
{
 checkReferenceFrameMatch(orientation);
 Pose3DBasics.super.setOrientation(orientation);
}

/**
* Sets the orientation part of this pose 3D with the given yaw, pitch, and roll angles.
* <p>
* WARNING: the Euler angles or yaw-pitch-roll representation is sensitive to gimbal lock and is
* sometimes undefined.
* </p>
*
* @param yawPitchRoll array containing the yaw-pitch-roll angles. Not modified.
* @deprecated Use {@link #setOrientation(Orientation3DReadOnly)} with {@link YawPitchRoll} instead.
*/
default void setOrientationYawPitchRoll(double[] yawPitchRoll)
{
 getOrientation().setYawPitchRoll(yawPitchRoll);
}

rootJoint.getJointPose().setPosition(translation.getX(), translation.getY(), translation.getZ());
Quaternion orientation = robotConfigurationData.getRootOrientation();
rootJoint.getJointPose().getOrientation().setQuaternion(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getS());
rootJoint.getPredecessor().updateFramesRecursively();

/**
* Sets the y-coordinate of the position.
*
* @param y the y-coordinate of the position.
*/
default void setY(double y)
{
 getPosition().setY(y);
}

/**
* Sets this pose 3D to the {@code other} pose 3D.
*
* @param other the other pose 3D. Not modified.
*/
default void set(Pose3DReadOnly other)
{
 setPosition(other.getPosition());
 setOrientation(other.getOrientation());
}

/**
* Sets the position from the given frame tuple 3D.
*
* @param position the tuple with the new position coordinates. Not modified.
* @throws ReferenceFrameMismatchException if {@code this} and {@code position} are not expressed in
*            the same reference frame.
*/
default void setPosition(FrameTuple3DReadOnly position)
{
 checkReferenceFrameMatch(position);
 Pose3DBasics.super.setPosition(position);
}

sixDoFJoint.getJointPose().setOrientationYawPitchRoll(random.nextDouble(), random.nextDouble(), random.nextDouble());

/**
* Sets the orientation from the given frame orientation.
*
* @param orientation the orientation to set the orientation part of this frame pose. Not modified.
* @throws ReferenceFrameMismatchException if {@code this} and {@code orientation} are not expressed
*            in the same reference frame.
*/
default void setOrientation(FrameOrientation3DReadOnly orientation)
{
 checkReferenceFrameMatch(orientation);
 Pose3DBasics.super.setOrientation(orientation);
}

/**
* Prepends a rotation about the y-axis to this pose 3D: Rotates the position part and prepends the
* rotation to the orientation part.
*
* @param pitch the angle to rotate about the y-axis.
*/
default void prependPitchRotation(double pitch)
{
 RotationMatrixTools.applyPitchRotation(pitch, getPosition(), getPosition());
 getOrientation().prependPitchRotation(pitch);
}