/**
* Computes the most dominant eigen vector of A using an inverted shifted matrix. The inverted
* shifted matrix is defined as <b>B = (A - αI)<sup>-1</sup></b> and can converge faster if
* α is chosen wisely.
*
* @param A An invertible square matrix matrix.
* @param alpha Shifting factor.
* @return If it converged or not.
*/
public boolean computeShiftInvert(DenseMatrix64F A, double alpha) {
initPower(A);
LinearSolver solver = LinearSolverFactory.linear(A.numCols);
SpecializedOps.addIdentity(A, B, -alpha);
solver.setA(B);
boolean converged = false;
for (int i = 0; i < maxIterations && !converged; i++) {
solver.solve(q0, q1);
double s = NormOps.normPInf(q1);
CommonOps.divide(q1, s, q2);
converged = checkConverged(A);
}
return converged;
}
public void solve(
DenseMatrix64F accelerationSubspace,
DenseMatrix64F accelerationMultipliers,
DenseMatrix64F momentumSubspace,
DenseMatrix64F momentumMultipliers) {
if (accelerationSubspace.getNumCols() > 0) {
TaskspaceConstraintData rootJointTaskspaceConstraintData = new TaskspaceConstraintData();
DenseMatrix64F rootJointAccelerationMatrix = new DenseMatrix64F(SpatialMotionVector.SIZE, 1);
CommonOps.mult(accelerationSubspace, accelerationMultipliers, rootJointAccelerationMatrix);
SpatialAccelerationVector spatialAcceleration =
new SpatialAccelerationVector(
rootJoint.getFrameAfterJoint(),
rootJoint.getFrameBeforeJoint(),
rootJoint.getFrameAfterJoint(),
rootJointAccelerationMatrix);
spatialAcceleration.changeBodyFrameNoRelativeAcceleration(
rootJoint.getSuccessor().getBodyFixedFrame());
spatialAcceleration.changeFrameNoRelativeMotion(rootJoint.getSuccessor().getBodyFixedFrame());
DenseMatrix64F nullspaceMultipliers = new DenseMatrix64F(0, 1);
DenseMatrix64F selectionMatrix =
new DenseMatrix64F(accelerationSubspace.getNumCols(), accelerationSubspace.getNumRows());
CommonOps.transpose(accelerationSubspace, selectionMatrix);
rootJointTaskspaceConstraintData.set(
spatialAcceleration, nullspaceMultipliers, selectionMatrix);
setDesiredSpatialAcceleration(
rootJoint.getMotionSubspace(), rootJointTaskspaceConstraintData);
}
// sTranspose
sTranspose.reshape(momentumSubspace.getNumCols(), momentumSubspace.getNumRows());
CommonOps.transpose(momentumSubspace, sTranspose);
// b
b.reshape(sTranspose.getNumRows(), 1);
b.set(momentumMultipliers);
CommonOps.multAdd(-1.0, sTranspose, adotV, b);
// sTransposeA
sTransposeA.reshape(sTranspose.getNumRows(), centroidalMomentumMatrix.getMatrix().getNumCols());
CommonOps.mult(sTranspose, centroidalMomentumMatrix.getMatrix(), sTransposeA);
// assemble Jp, pp
motionConstraintHandler.compute();
DenseMatrix64F Jp = motionConstraintHandler.getJacobian();
DenseMatrix64F pp = motionConstraintHandler.getRightHandSide();
// DenseMatrix64F n = motionConstraintHandler.getNullspaceMatrixTranspose();
// DenseMatrix64F z = motionConstraintHandler.getNullspaceMultipliers();
// nullspaceMotionConstraintEnforcer.set(n, z);
// nullspaceMotionConstraintEnforcer.constrainEquation(Jp, pp);
equalityConstraintEnforcer.setConstraint(Jp, pp);
// nullspaceMotionConstraintEnforcer.constrainEquation(sTransposeA, b);
equalityConstraintEnforcer.constrainEquation(sTransposeA, b);
// APPlusbMinusAJpPluspp
vdotUnconstrained.reshape(nDegreesOfFreedom, 1);
solver.setA(sTransposeA);
solver.solve(b, vdotUnconstrained);
DenseMatrix64F vdot = equalityConstraintEnforcer.constrainResult(vdotUnconstrained);
// DenseMatrix64F vdot = nullspaceMotionConstraintEnforcer.constrainResult(vdot);
ScrewTools.setDesiredAccelerations(jointsInOrder, vdot);
CommonOps.mult(centroidalMomentumMatrix.getMatrix(), vdot, hdot);
CommonOps.addEquals(hdot, adotV);
}
