public void setRobotStateRandomly(Random random, double maxJointVelocity, double maxRootJointLinearAndAngularVelocity)
{
FloatingJoint rootJoint = robot.getRootJoint();
rootJoint.setVelocity(RandomGeometry.nextVector3D(random, maxRootJointLinearAndAngularVelocity));
rootJoint.setAngularVelocityInBody(RandomGeometry.nextVector3D(random, maxRootJointLinearAndAngularVelocity));
rootJoint.setPosition(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.setYawPitchRoll(yaw, pitch, roll);
ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>();
robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints);
for (OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint : oneDegreeOfFreedomJoints)
{
double lowerLimit = oneDegreeOfFreedomJoint.getJointLowerLimit();
double upperLimit = oneDegreeOfFreedomJoint.getJointUpperLimit();
double delta = upperLimit - lowerLimit;
lowerLimit = lowerLimit + 0.05 * delta;
upperLimit = upperLimit - 0.05 * delta;
oneDegreeOfFreedomJoint.setQ(RandomNumbers.nextDouble(random, lowerLimit, upperLimit));
oneDegreeOfFreedomJoint.setQd(RandomNumbers.nextDouble(random, maxJointVelocity));
}
}