public void getLinearVelocity(Vector3d linearVelocityToPack) { linearVelocityToPack.set(jointTwist.getLinearPartX(), 0.0, jointTwist.getLinearPartZ()); }
@Override public void getVelocityMatrix(DenseMatrix64F matrix, int rowStart) { matrix.set(rowStart + 0, 0, jointTwist.getAngularPartY()); matrix.set(rowStart + 1, 0, jointTwist.getLinearPartX()); matrix.set(rowStart + 2, 0, jointTwist.getLinearPartZ()); }
protected void getYoValuesFromTwist() { linearPart.set(twist.getExpressedInFrame(), twist.getLinearPartX(), twist.getLinearPartY(), twist.getLinearPartZ()); angularPart.set(twist.getExpressedInFrame(), twist.getAngularPartX(), twist.getAngularPartY(), twist.getAngularPartZ()); }
@Override public void getJointTwist(Twist twistToPack) { twistToPack.setToZero(jointTwist.getBodyFrame(), jointTwist.getBaseFrame(), jointTwist.getExpressedInFrame()); twistToPack.setAngularPartY(jointTwist.getAngularPartY()); twistToPack.setLinearPartX(jointTwist.getLinearPartX()); twistToPack.setLinearPartZ(jointTwist.getLinearPartZ()); }
public void setJointTwist(Twist jointTwist) { this.jointTwist.checkReferenceFramesMatch(jointTwist.getBodyFrame(), jointTwist.getBaseFrame(), jointTwist.getExpressedInFrame()); this.jointTwist.setAngularPartY(jointTwist.getAngularPartY()); this.jointTwist.setLinearPartX(jointTwist.getLinearPartX()); this.jointTwist.setLinearPartZ(jointTwist.getLinearPartZ()); }
@Override public void calculateJointStateChecksum(GenericCRC32 checksum) { RotationTools.convertQuaternionToYawPitchRoll(jointRotation, yawPitchRoll); checksum.update(yawPitchRoll[1]); checksum.update(jointTwist.getAngularPartY()); checksum.update(jointAcceleration.getAngularPartY()); checksum.update(jointTranslation.getX()); checksum.update(jointTranslation.getY()); checksum.update(jointTwist.getLinearPartX()); checksum.update(jointTwist.getLinearPartZ()); checksum.update(jointAcceleration.getLinearPartX()); checksum.update(jointAcceleration.getLinearPartZ()); }