public static void integrateVelocities(SixDoFJoint sixDoFJoint, double dt) { Twist twist = new Twist(); sixDoFJoint.getJointTwist(twist); Matrix3d rotation = new Matrix3d(); sixDoFJoint.getRotation(rotation); Vector3d position = new Vector3d(); sixDoFJoint.getTranslation(position); integrate(rotation, position, dt, twist); sixDoFJoint.setRotation(rotation); sixDoFJoint.setPosition(position); }
public void get(double[] buffer, int offset) { inverseDynamicsJoint.getRotation(rotation); inverseDynamicsJoint.getTranslation(translation); inverseDynamicsJoint.getJointTwist(twist); buffer[offset + 0] = rotation.getW(); buffer[offset + 1] = rotation.getX(); buffer[offset + 2] = rotation.getY(); buffer[offset + 3] = rotation.getZ(); buffer[offset + 4] = translation.getX(); buffer[offset + 5] = translation.getY(); buffer[offset + 6] = translation.getZ(); twist.getArray(buffer, offset + 7); } }
public void get(double[] buffer, int offset) { inverseDynamicsJoint.getRotation(rotation); inverseDynamicsJoint.getTranslation(translation); inverseDynamicsJoint.getJointTwist(twist); buffer[offset + 0] = rotation.getW(); buffer[offset + 1] = rotation.getX(); buffer[offset + 2] = rotation.getY(); buffer[offset + 3] = rotation.getZ(); buffer[offset + 4] = translation.getX(); buffer[offset + 5] = translation.getY(); buffer[offset + 6] = translation.getZ(); twist.getArray(buffer, offset + 7); } }
joint.getTranslation(previousTranslation); joint.getLinearVelocity(previousLinearVelocity); joint.getAngularVelocity(previousAngularVelocity);
joint.getTranslation(previousTranslation);