private void setUnitMomenta(
CompositeRigidBodyInertia currentBodyInertia, GeometricJacobian motionSubspace) {
nMomentaInUse = motionSubspace.getNumberOfColumns();
for (int i = 0; i < nMomentaInUse; i++) {
tempTwist.set(motionSubspace.getAllUnitTwists().get(i));
tempTwist.changeFrame(currentBodyInertia.getExpressedInFrame());
unitMomenta[i].compute(currentBodyInertia, tempTwist);
}
}
private void setMassMatrixPart(int i, int j, GeometricJacobian motionSubspace) {
int rowStart = massMatrixIndices[i];
int colStart = massMatrixIndices[j];
for (int m = 0; m < nMomentaInUse; m++) {
Momentum unitMomentum = unitMomenta[m];
int massMatrixRow = rowStart + m;
for (int n = 0; n < motionSubspace.getNumberOfColumns(); n++) {
Twist unitRelativeTwist = motionSubspace.getAllUnitTwists().get(n);
unitRelativeTwist.changeFrame(unitMomentum.getExpressedInFrame());
double entry = unitMomentum.computeKineticCoEnergy(unitRelativeTwist);
int massMatrixCol = colStart + n;
setMassMatrixSymmetrically(massMatrixRow, massMatrixCol, entry);
}
}
}
/** 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++);
}
}
/**
* packs the twist that corresponds to the given joint velocity vector
*
* @param jointVelocities the joint velocity vector; should have the same ordering as the
* unitRelativeTwistsInBodyFrame that were passed in during construction, should be a column
* vector
* @return the twist of the end effector with respect to the base, expressed in the jacobianFrame
* <p>angular velocity part of the twist is the angular velocity of end effector frame, with
* respect to base frame, rotated to end effector frame linear velocity part of the twist is
* the linear velocity of end effector frame, with respect to base frame, rotated to end
* effector frame
*/
public void getTwist(Twist twistToPack, DenseMatrix64F jointVelocities) {
CommonOps.mult(jacobian, jointVelocities, tempMatrix);
twistToPack.set(getEndEffectorFrame(), getBaseFrame(), jacobianFrame, tempMatrix, 0);
}
