/** {@inheritDoc} */ @Override default void setJointAngularVelocity(Vector3DReadOnly jointAngularVelocity) { getJointTwist().getAngularPart().set(jointAngularVelocity); }
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()); }; }
Point3DReadOnly initialPosition = initialPose.getPosition(); FixedFrameVector3DBasics finalAngularVelocity = finalTwist.getAngularPart(); FixedFrameVector3DBasics finalLinearVelocity = finalTwist.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(); }
linearVelocity.changeFrame(ReferenceFrame.getWorldFrame()); floatingJoint.setVelocity(linearVelocity); floatingJoint.setAngularVelocityInBody(sixDoFJoint.getJointTwist().getAngularPart());