/** {@inheritDoc} */ @Override default void setJointLinearVelocity(Vector3DReadOnly jointLinearVelocity) { getJointTwist().getLinearPart().set(jointLinearVelocity); }
/** {@inheritDoc} */ @Override default int setJointVelocity(int rowStart, DenseMatrix64F matrix) { int index = rowStart; double qdRot = matrix.get(index++, 0); double xd = matrix.get(index++, 0); double zd = matrix.get(index++, 0); getJointTwist().setToZero(); getJointTwist().setAngularPartY(qdRot); getJointTwist().getLinearPart().set(xd, 0.0, zd); return rowStart + getDegreesOfFreedom(); }
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()); }; }
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.setIncludingFrame(pelvisFrame, sixDoFJoint.getJointTwist().getLinearPart()); linearVelocity.changeFrame(ReferenceFrame.getWorldFrame()); floatingJoint.setVelocity(linearVelocity);
linearVelocity.set(rootJoint.getJointTwist().getLinearPart());