/**
* Computes inverse coefficients
*
* @param border
* @param forward Forward coefficients.
* @param inverse Inverse used in the inner portion of the data stream.
* @return
*/
private static WlBorderCoef<WlCoef_F32> computeBorderCoefficients(
BorderIndex1D border, WlCoef_F32 forward, WlCoef_F32 inverse) {
int N = Math.max(forward.getScalingLength(), forward.getWaveletLength());
N += N % 2;
N *= 2;
border.setLength(N);
// Because the wavelet transform is a linear invertible system the inverse coefficients
// can be found by creating a matrix and inverting the matrix. Boundary conditions are then
// extracted from this inverted matrix.
DenseMatrix64F A = new DenseMatrix64F(N, N);
for (int i = 0; i < N; i += 2) {
for (int j = 0; j < forward.scaling.length; j++) {
int index = border.getIndex(j + i + forward.offsetScaling);
A.add(i, index, forward.scaling[j]);
}
for (int j = 0; j < forward.wavelet.length; j++) {
int index = border.getIndex(j + i + forward.offsetWavelet);
A.add(i + 1, index, forward.wavelet[j]);
}
}
LinearSolver<DenseMatrix64F> solver = LinearSolverFactory.linear(N);
if (!solver.setA(A) || solver.quality() < 1e-5) {
throw new IllegalArgumentException("Can't invert matrix");
}
DenseMatrix64F A_inv = new DenseMatrix64F(N, N);
solver.invert(A_inv);
int numBorder = UtilWavelet.borderForwardLower(inverse) / 2;
WlBorderCoefFixed<WlCoef_F32> ret = new WlBorderCoefFixed<>(numBorder, numBorder + 1);
ret.setInnerCoef(inverse);
// add the lower coefficients first
for (int i = 0; i < ret.getLowerLength(); i++) {
computeLowerCoef(inverse, A_inv, ret, i * 2);
}
// add upper coefficients
for (int i = 0; i < ret.getUpperLength(); i++) {
computeUpperCoef(inverse, N, A_inv, ret, i * 2);
}
return ret;
}
/**
* 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);
}
