/** {@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(); }
/** {@inheritDoc} */ @Override default void setJointAngularVelocity(Vector3DReadOnly jointAngularVelocity) { getJointTwist().getAngularPart().set(jointAngularVelocity); }
/** {@inheritDoc} */ @Override default int setJointVelocity(int rowStart, DenseMatrix64F matrix) { getJointTwist().set(rowStart, matrix); 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()); }; }
spatialAcceleration.checkExpressedInFrameMatch(spatialAcceleration.getBodyFrame()); if (finalTwist != initialTwist) finalTwist.checkReferenceFrameMatch(initialTwist); Point3DReadOnly initialPosition = initialPose.getPosition(); FixedFrameVector3DBasics finalAngularVelocity = finalTwist.getAngularPart(); FixedFrameVector3DBasics finalLinearVelocity = finalTwist.getLinearPart();
/** {@inheritDoc} */ @Override default void setJointLinearVelocity(Vector3DReadOnly jointLinearVelocity) { getJointTwist().getLinearPart().set(jointLinearVelocity); }
public static ChecksumUpdater newJointChecksumUpdater(GenericCRC32 checksum, PlanarJoint joint) { return () -> { checksum.update(joint.getJointPose().getOrientation().getPitch()); checksum.update(joint.getJointPose().getPosition().getX()); checksum.update(joint.getJointPose().getPosition().getY()); checksum.update(joint.getJointTwist().getAngularPartY()); checksum.update(joint.getJointTwist().getLinearPartX()); checksum.update(joint.getJointTwist().getLinearPartZ()); checksum.update(joint.getJointAcceleration().getAngularPartY()); checksum.update(joint.getJointAcceleration().getLinearPartX()); checksum.update(joint.getJointAcceleration().getLinearPartZ()); }; }
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.set(rootJoint.getJointTwist().getLinearPart());
linearVelocity.setIncludingFrame(pelvisFrame, sixDoFJoint.getJointTwist().getLinearPart()); linearVelocity.changeFrame(ReferenceFrame.getWorldFrame()); floatingJoint.setVelocity(linearVelocity); floatingJoint.setAngularVelocityInBody(sixDoFJoint.getJointTwist().getAngularPart());
private void getFloatingJointStateFromSCS() { FloatingJoint scsJoint = (FloatingJoint) robot.getRootJoints().get(0); RigidBodyTransform jointTransform3D = scsJoint.getJointTransform3D(); floatingJoint.getJointPose().set(jointTransform3D); floatingJoint.getFrameAfterJoint().update(); FrameVector3D linearVelocity = new FrameVector3D(); scsJoint.getVelocity(linearVelocity); linearVelocity.changeFrame(floatingJoint.getFrameAfterJoint()); floatingJoint.getJointTwist().set(scsJoint.getAngularVelocityInBody(), linearVelocity); }