public void compute() {
centroidalMomentumMatrix.compute();
MatrixYoVariableConversionTools.getFromYoVariables(
previousCentroidalMomentumMatrix, yoPreviousCentroidalMomentumMatrix);
MatrixTools.numericallyDifferentiate(
centroidalMomentumMatrixDerivative,
previousCentroidalMomentumMatrix,
centroidalMomentumMatrix.getMatrix(),
controlDT);
MatrixYoVariableConversionTools.storeInYoVariables(
previousCentroidalMomentumMatrix, yoPreviousCentroidalMomentumMatrix);
ScrewTools.packJointVelocitiesMatrix(jointsInOrder, v);
CommonOps.mult(centroidalMomentumMatrixDerivative, v, adotV);
}
public void initialize() {
centroidalMomentumMatrix.compute();
previousCentroidalMomentumMatrix.set(centroidalMomentumMatrix.getMatrix());
MatrixYoVariableConversionTools.storeInYoVariables(
previousCentroidalMomentumMatrix, yoPreviousCentroidalMomentumMatrix);
}