public DenseMatrix64F getCentroidalMomentumMatrixPart(InverseDynamicsJoint[] joints) {
int partDegreesOfFreedom = ScrewTools.computeDegreesOfFreedom(joints);
centroidalMomentumMatrixPart.reshape(Momentum.SIZE, partDegreesOfFreedom);
centroidalMomentumMatrixPart.zero();
int startColumn = 0;
for (InverseDynamicsJoint joint : joints) {
int[] columnsForJoint = columnsForJoints.get(joint);
MatrixTools.extractColumns(
centroidalMomentumRateTermCalculator.getCentroidalMomentumMatrix(),
columnsForJoint,
centroidalMomentumMatrixPart,
startColumn);
startColumn += columnsForJoint.length;
}
return centroidalMomentumMatrixPart;
}
public void compute() {
ScrewTools.getJointVelocitiesMatrix(jointsInOrder, v);
centroidalMomentumRateTermCalculator.compute();
adotV.set(centroidalMomentumRateTermCalculator.getADotVTerm());
}