public void solve(SpatialForceVector momentumRateOfChange) {
T.reshape(SpatialMotionVector.SIZE, 0);
alpha1.reshape(T.getNumCols(), 1);
N.reshape(SpatialForceVector.SIZE, SpatialForceVector.SIZE);
beta1.reshape(N.getNumCols(), 1);
CommonOps.setIdentity(N);
momentumRateOfChange.packMatrix(beta1);
solve(T, alpha1, N, beta1);
}
/**
* Converts a vector from sample space into eigen space.
*
* @param sampleData Sample space data.
* @return Eigen space projection.
*/
public double[] sampleToEigenSpace(double[] sampleData) {
if (sampleData.length != A.getNumCols())
throw new IllegalArgumentException("Unexpected sample length");
DenseMatrix64F mean = DenseMatrix64F.wrap(A.getNumCols(), 1, this.mean);
DenseMatrix64F s = new DenseMatrix64F(A.getNumCols(), 1, true, sampleData);
DenseMatrix64F r = new DenseMatrix64F(numComponents, 1);
CommonOps.sub(s, mean, s);
CommonOps.mult(V_t, s, r);
return r.data;
}
/**
* Converts a vector from eigen space into sample space.
*
* @param eigenData Eigen space data.
* @return Sample space projection.
*/
public double[] eigenToSampleSpace(double[] eigenData) {
if (eigenData.length != numComponents)
throw new IllegalArgumentException("Unexpected sample length");
DenseMatrix64F s = new DenseMatrix64F(A.getNumCols(), 1);
DenseMatrix64F r = DenseMatrix64F.wrap(numComponents, 1, eigenData);
CommonOps.multTransA(V_t, r, s);
DenseMatrix64F mean = DenseMatrix64F.wrap(A.getNumCols(), 1, this.mean);
CommonOps.add(s, mean, s);
return s.data;
}
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++;
}
/**
* returns the joint torque vector that corresponds to the given wrench
*
* @param wrench the resulting wrench at the end effector. The wrench should be expressed in this
* Jacobian's jacobianFrame and its 'onWhat' frame should be this Jacobian's endEffectorFrame
* @return joint torques that result in the given wrench as a column vector
*/
public DenseMatrix64F computeJointTorques(Wrench wrench) {
// reference frame check
wrench.getExpressedInFrame().checkReferenceFrameMatch(this.jacobianFrame);
wrench.getMatrix(tempMatrix);
DenseMatrix64F jointTorques = new DenseMatrix64F(1, jacobian.getNumCols());
CommonOps.multTransA(tempMatrix, jacobian, jointTorques);
CommonOps.transpose(jointTorques);
return jointTorques;
}
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++);
}
}
/**
* 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 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);
}
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);
}
/** @return the number of columns in the Jacobian matrix */
public int getNumberOfColumns() {
return jacobian.getNumCols();
}
