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);
}
/**
* Adds a new sample of the raw data to internal data structure for later processing. All the
* samples must be added before computeBasis is called.
*
* @param sampleData Sample from original raw data.
*/
public void addSample(double[] sampleData) {
if (A.getNumCols() != sampleData.length)
throw new IllegalArgumentException("Unexpected sample size");
if (sampleIndex >= A.getNumRows()) throw new IllegalArgumentException("Too many samples");
for (int i = 0; i < sampleData.length; i++) {
A.set(sampleIndex, i, sampleData[i]);
}
sampleIndex++;
}
/**
* Computes a basis (the principle components) from the most dominant eigenvectors.
*
* @param numComponents Number of vectors it will use to describe the data. Typically much smaller
* than the number of elements in the input vector.
*/
public void computeBasis(int numComponents) {
if (numComponents > A.getNumCols())
throw new IllegalArgumentException("More components requested that the data's length.");
if (sampleIndex != A.getNumRows())
throw new IllegalArgumentException("Not all the data has been added");
if (numComponents > sampleIndex)
throw new IllegalArgumentException(
"More data needed to compute the desired number of components");
this.numComponents = numComponents;
// compute the mean of all the samples
for (int i = 0; i < A.getNumRows(); i++) {
for (int j = 0; j < mean.length; j++) {
mean[j] += A.get(i, j);
}
}
for (int j = 0; j < mean.length; j++) {
mean[j] /= A.getNumRows();
}
// subtract the mean from the original data
for (int i = 0; i < A.getNumRows(); i++) {
for (int j = 0; j < mean.length; j++) {
A.set(i, j, A.get(i, j) - mean[j]);
}
}
// Compute SVD and save time by not computing U
SingularValueDecomposition<DenseMatrix64F> svd =
DecompositionFactory.svd(A.numRows, A.numCols, false, true, false);
if (!svd.decompose(A)) throw new RuntimeException("SVD failed");
V_t = svd.getV(null, true);
DenseMatrix64F W = svd.getW(null);
// Singular values are in an arbitrary order initially
SingularOps.descendingOrder(null, false, W, V_t, true);
// strip off unneeded components and find the basis
V_t.reshape(numComponents, mean.length, true);
}
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();
}
/** Computes the Jacobian. */
public void compute() {
int column = 0;
for (int i = 0; i < unitTwistList.size(); i++) {
Twist twist = unitTwistList.get(i);
tempTwist.set(twist);
tempTwist.changeFrame(jacobianFrame);
tempTwist.getMatrix(tempMatrix, 0);
CommonOps.extract(
tempMatrix,
0,
tempMatrix.getNumRows(),
0,
tempMatrix.getNumCols(),
jacobian,
0,
column++);
}
}
public int size() {
return values.getNumRows();
}
public Tuple(DenseMatrix64F values) {
// values is a column matrix
this.values = values;
this.dimensions = values.getNumRows();
}
public void setDesiredAcceleration(DenseMatrix64F qdd) {
if (qdd.getNumRows() != 6 || qdd.getNumCols() != 1)
throw new RuntimeException("Unexpected size: " + qdd);
desiredAcceleration.set(qdd);
}
public void setDesiredVelocity(DenseMatrix64F qd) {
if (qd.getNumRows() != 6 || qd.getNumCols() != 1)
throw new RuntimeException("Unexpected size: " + qd);
desiredVelocity.set(qd);
}
public void setDesiredConfiguration(DenseMatrix64F q) {
if (q.getNumRows() != 7 || q.getNumCols() != 1)
throw new RuntimeException("Unexpected size: " + q);
desiredConfiguration.set(q);
}
@Override
public boolean hasDesiredAcceleration() {
return desiredAcceleration.getNumRows() != 0;
}
@Override
public boolean hasDesiredVelocity() {
return desiredVelocity.getNumRows() != 0;
}
@Override
public boolean hasDesiredConfiguration() {
return desiredConfiguration.getNumRows() != 0;
}
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);
}
public void setSelectionMatrix(DenseMatrix64F selectionMatrix) {
this.selectionMatrix.reshape(selectionMatrix.getNumRows(), selectionMatrix.getNumCols());
this.selectionMatrix.set(selectionMatrix);
}