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); }