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