private void setUnitMomenta( CompositeRigidBodyInertia currentBodyInertia, GeometricJacobian motionSubspace) { nMomentaInUse = motionSubspace.getNumberOfColumns(); for (int i = 0; i < nMomentaInUse; i++) { tempTwist.set(motionSubspace.getAllUnitTwists().get(i)); tempTwist.changeFrame(currentBodyInertia.getExpressedInFrame()); unitMomenta[i].compute(currentBodyInertia, tempTwist); } }
/** Computes the Jacobian. */ public void compute() { int column = 0; for (int i = 0; i < unitTwistList.size(); i++) { Twist twist = unitTwistList.get(i); tempTwist.set(twist); tempTwist.changeFrame(jacobianFrame); tempTwist.getMatrix(tempMatrix, 0); CommonOps.extract( tempMatrix, 0, tempMatrix.getNumRows(), 0, tempMatrix.getNumCols(), jacobian, 0, column++); } }
/** * packs the twist that corresponds to the given joint velocity vector * * @param jointVelocities the joint velocity vector; should have the same ordering as the * unitRelativeTwistsInBodyFrame that were passed in during construction, should be a column * vector * @return the twist of the end effector with respect to the base, expressed in the jacobianFrame * <p>angular velocity part of the twist is the angular velocity of end effector frame, with * respect to base frame, rotated to end effector frame linear velocity part of the twist is * the linear velocity of end effector frame, with respect to base frame, rotated to end * effector frame */ public void getTwist(Twist twistToPack, DenseMatrix64F jointVelocities) { CommonOps.mult(jacobian, jointVelocities, tempMatrix); twistToPack.set(getEndEffectorFrame(), getBaseFrame(), jacobianFrame, tempMatrix, 0); }