How to use

setOrientationYawPitchRoll

method

in

us.ihmc.euclid.geometry.interfaces.Pose3DBasics

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

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

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

public void setFullRobotModelStateRandomly(Random random, double maxJointVelocity, double maxRootJointLinearAndAngularVelocity)
{
 FloatingJointBasics rootJoint = fullRobotModel.getRootJoint();
 ReferenceFrame elevatorFrame = rootJoint.getFrameBeforeJoint();
 ReferenceFrame bodyFrame = rootJoint.getFrameAfterJoint();
 Twist bodyTwist = new Twist(bodyFrame, elevatorFrame, bodyFrame, RandomGeometry.nextVector3D(random, maxRootJointLinearAndAngularVelocity),
    RandomGeometry.nextVector3D(random, maxRootJointLinearAndAngularVelocity));
 rootJoint.setJointTwist(bodyTwist);
 rootJoint.setJointPosition(RandomGeometry.nextVector3D(random));
 double yaw = RandomNumbers.nextDouble(random, Math.PI / 20.0);
 double pitch = RandomNumbers.nextDouble(random, Math.PI / 20.0);
 double roll = RandomNumbers.nextDouble(random, Math.PI / 20.0);
 rootJoint.getJointPose().setOrientationYawPitchRoll(yaw, pitch, roll);
 ArrayList<OneDoFJointBasics> oneDoFJoints = new ArrayList<OneDoFJointBasics>();
 fullRobotModel.getOneDoFJoints(oneDoFJoints);
 for (OneDoFJointBasics oneDoFJoint : oneDoFJoints)
 {
   double lowerLimit = oneDoFJoint.getJointLimitLower();
   double upperLimit = oneDoFJoint.getJointLimitUpper();
   double delta = upperLimit - lowerLimit;
   lowerLimit = lowerLimit + 0.05 * delta;
   upperLimit = upperLimit - 0.05 * delta;
   oneDoFJoint.setQ(RandomNumbers.nextDouble(random, lowerLimit, upperLimit));
   oneDoFJoint.setQd(RandomNumbers.nextDouble(random, maxJointVelocity));
 }
}

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