/** * Creates a new Jacobian representing one joint. * * @param joint the joint that this Jacobian considers. * @param jacobianFrame the frame in which the resulting twist of the end effector with respect to the base frame will be expressed. * @throws RuntimeException if the joint ordering is incorrect. */ public GeometricJacobian(InverseDynamicsJoint joint, ReferenceFrame jacobianFrame) { this(joint, joint.getMotionSubspace().getAllUnitTwists(), jacobianFrame); }
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); } } }
public void compute() { int column = 0; for (int i = 0; i < rigidBodyList.size(); i++) { comScaledByMassMapIsUpdated.get(rigidBodyList.get(i)).setValue(false); } for (InverseDynamicsJoint joint : joints) { RigidBody childBody = joint.getSuccessor(); FramePoint comPositionScaledByMass = getCoMScaledByMass(childBody); double subTreeMass = getSubTreeMass(childBody); GeometricJacobian motionSubspace = joint.getMotionSubspace(); final List<Twist> allTwists = motionSubspace.getAllUnitTwists(); for(int i = 0; i < allTwists.size(); i++) { tempUnitTwist.set(allTwists.get(i)); tempUnitTwist.changeFrame(rootFrame); setColumn(tempUnitTwist, comPositionScaledByMass, subTreeMass, column); column++; } } CommonOps.scale(inverseTotalMass, jacobianMatrix); }
for (Twist twist : motionSubspace.getAllUnitTwists())
tempTwist.set(columnJoint.getMotionSubspace().getAllUnitTwists().get(k)); tempTwist.changeFrame(inertia.getExpressedInFrame()); tempMomentum.compute(inertia, tempTwist);
tempTwist.set(jointList[j].getMotionSubspace().getAllUnitTwists().get(k)); tempTwist.changeFrame(rigidBodies[i].getInertia().getExpressedInFrame());