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); }
@Override public void doControl() { refFrame.update(); sixDofPelvisJoint.setJointConfiguration(robotPose); pelvisPoseHistoryCorrection.doControl(Conversions.secondsToNanoseconds(scs.getTime())); floatingJoint.setQuaternion(sixDofPelvisJoint.getJointPose().getOrientation()); floatingJoint.setPosition(sixDofPelvisJoint.getJointPose().getPosition()); } }
public static ChecksumUpdater newJointChecksumUpdater(GenericCRC32 checksum, SixDoFJoint joint) { return () -> { checksum.update(joint.getJointPose().getOrientation()); checksum.update(joint.getJointPose().getPosition()); checksum.update(joint.getJointTwist().getAngularPart()); checksum.update(joint.getJointTwist().getLinearPart()); checksum.update(joint.getJointAcceleration().getAngularPart()); checksum.update(joint.getJointAcceleration().getLinearPart()); }; }
public void get(double[] buffer, int offset) { QuaternionReadOnly rotation = inverseDynamicsJoint.getJointPose().getOrientation(); Tuple3DReadOnly translation = inverseDynamicsJoint.getJointPose().getPosition(); Tuple3DReadOnly angularVelocity = inverseDynamicsJoint.getJointTwist().getAngularPart(); Tuple3DReadOnly linearVelocity = inverseDynamicsJoint.getJointTwist().getLinearPart(); buffer[offset++] = rotation.getS(); buffer[offset++] = rotation.getX(); buffer[offset++] = rotation.getY(); buffer[offset++] = rotation.getZ(); buffer[offset++] = translation.getX(); buffer[offset++] = translation.getY(); buffer[offset++] = translation.getZ(); buffer[offset++] = angularVelocity.getX(); buffer[offset++] = angularVelocity.getY(); buffer[offset++] = angularVelocity.getZ(); buffer[offset++] = linearVelocity.getX(); buffer[offset++] = linearVelocity.getY(); buffer[offset] = linearVelocity.getZ(); }
sixDoFJoint.getJointPose().setOrientationYawPitchRoll(random.nextDouble(), random.nextDouble(), random.nextDouble());
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());