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