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++); } }