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 CentroidalMomentumHandler(
RigidBody rootBody, ReferenceFrame centerOfMassFrame, YoVariableRegistry parentRegistry) {
this.jointsInOrder = ScrewTools.computeSupportAndSubtreeJoints(rootBody);
this.centroidalMomentumMatrix = new CentroidalMomentumMatrix(rootBody, centerOfMassFrame);
this.previousCentroidalMomentumMatrix =
new DenseMatrix64F(
centroidalMomentumMatrix.getMatrix().getNumRows(),
centroidalMomentumMatrix.getMatrix().getNumCols());
yoPreviousCentroidalMomentumMatrix =
new DoubleYoVariable[previousCentroidalMomentumMatrix.getNumRows()]
[previousCentroidalMomentumMatrix.getNumCols()];
MatrixYoVariableConversionTools.populateYoVariables(
yoPreviousCentroidalMomentumMatrix, "previousCMMatrix", registry);
int nDegreesOfFreedom = ScrewTools.computeDegreesOfFreedom(jointsInOrder);
this.v = new DenseMatrix64F(nDegreesOfFreedom, 1);
for (InverseDynamicsJoint joint : jointsInOrder) {
TIntArrayList listToPackIndices = new TIntArrayList();
ScrewTools.computeIndexForJoint(jointsInOrder, listToPackIndices, joint);
int[] indices = listToPackIndices.toArray();
columnsForJoints.put(joint, indices);
}
centroidalMomentumRate = new SpatialForceVector(centerOfMassFrame);
this.centerOfMassFrame = centerOfMassFrame;
parentRegistry.addChild(registry);
double robotMass = TotalMassCalculator.computeSubTreeMass(rootBody);
this.centroidalMomentumRateTermCalculator =
new CentroidalMomentumRateTermCalculator(rootBody, centerOfMassFrame, v, robotMass);
}
public MomentumSolver3(
SixDoFJoint rootJoint,
RigidBody elevator,
ReferenceFrame centerOfMassFrame,
TwistCalculator twistCalculator,
LinearSolver<DenseMatrix64F> jacobianSolver,
double controlDT,
YoVariableRegistry parentRegistry) {
this.rootJoint = rootJoint;
this.jointsInOrder = ScrewTools.computeSupportAndSubtreeJoints(rootJoint.getSuccessor());
this.motionConstraintHandler =
new MotionConstraintHandler("solver3", jointsInOrder, twistCalculator, null, registry);
this.centroidalMomentumMatrix = new CentroidalMomentumMatrix(elevator, centerOfMassFrame);
this.previousCentroidalMomentumMatrix =
new DenseMatrix64F(
centroidalMomentumMatrix.getMatrix().getNumRows(),
centroidalMomentumMatrix.getMatrix().getNumCols());
this.centroidalMomentumMatrixDerivative =
new DenseMatrix64F(
centroidalMomentumMatrix.getMatrix().getNumRows(),
centroidalMomentumMatrix.getMatrix().getNumCols());
yoPreviousCentroidalMomentumMatrix =
new DoubleYoVariable[previousCentroidalMomentumMatrix.getNumRows()]
[previousCentroidalMomentumMatrix.getNumCols()];
MatrixYoVariableConversionTools.populateYoVariables(
yoPreviousCentroidalMomentumMatrix, "previousCMMatrix", registry);
this.controlDT = controlDT;
nDegreesOfFreedom = ScrewTools.computeDegreesOfFreedom(jointsInOrder);
this.b = new DenseMatrix64F(SpatialMotionVector.SIZE, 1);
this.v = new DenseMatrix64F(nDegreesOfFreedom, 1);
// nullspaceMotionConstraintEnforcer = new
// EqualityConstraintEnforcer(LinearSolverFactory.pseudoInverse(true));
equalityConstraintEnforcer =
new EqualityConstraintEnforcer(LinearSolverFactory.pseudoInverse(true));
solver = LinearSolverFactory.pseudoInverse(true);
parentRegistry.addChild(registry);
reset();
}
public void initialize() {
centroidalMomentumMatrix.compute();
previousCentroidalMomentumMatrix.set(centroidalMomentumMatrix.getMatrix());
MatrixYoVariableConversionTools.storeInYoVariables(
previousCentroidalMomentumMatrix, yoPreviousCentroidalMomentumMatrix);
}
