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