private static LinkedHashMap<InverseDynamicsJoint, List<Twist>> extractTwistsFromJoints(InverseDynamicsJoint[] joints) { checkJointOrder(joints); LinkedHashMap<InverseDynamicsJoint, List<Twist>> ret = new LinkedHashMap<InverseDynamicsJoint, List<Twist>>(); for (int i = 0; i < joints.length; i++) { InverseDynamicsJoint joint = joints[i]; ret.put(joint, joint.getMotionSubspace().unitTwistMap.get(joint)); } return ret; }
/** * 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); }
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); }
private void setOffDiagonalTerms(int i) { int parentIndex; int j = i; while (isValidParentIndex(parentIndex = parentMap[j])) { ReferenceFrame parentFrame = allRigidBodiesInOrder[parentIndex].getInertia().getExpressedInFrame(); changeFrameOfMomenta(parentFrame); j = parentIndex; GeometricJacobian motionSubspace = allRigidBodiesInOrder[j].getParentJoint().getMotionSubspace(); setMassMatrixPart(i, j, motionSubspace); } }
GeometricJacobian motionSubspace = inverseDynamicsJoint.getMotionSubspace(); for (Twist twist : motionSubspace.getAllUnitTwists())
private void storeJointState() { ScrewTools.getDesiredJointAccelerationsMatrix(jointsInOrder, storedJointDesiredAccelerations); ScrewTools.getJointVelocitiesMatrix(jointsInOrder, storedJointVelocities); for (InverseDynamicsJoint joint : jointsInOrder) { DenseMatrix64F tauMatrix = new DenseMatrix64F(joint.getDegreesOfFreedom(), 1); joint.getTauMatrix(tauMatrix); DenseMatrix64F spatialForce = new DenseMatrix64F(SpatialForceVector.SIZE, 1); CommonOps.mult(joint.getMotionSubspace().getJacobianMatrix(), tauMatrix, spatialForce); Wrench jointWrench = storedJointWrenches.get(joint); jointWrench.set(joint.getFrameAfterJoint(), spatialForce); jointWrench.changeFrame(joint.getSuccessor().getBodyFixedFrame()); } }
@Override public void compute() { MatrixTools.setToZero(massMatrix); for (int i = 0; i < allRigidBodiesInOrder.length; i++) { crbInertiasInOrder[i].set(allRigidBodiesInOrder[i].getInertia()); } for (int i = allRigidBodiesInOrder.length - 1; i >= 0; i--) { RigidBody currentBody = allRigidBodiesInOrder[i]; InverseDynamicsJoint parentJoint = currentBody.getParentJoint(); CompositeRigidBodyInertia currentBodyInertia = crbInertiasInOrder[i]; GeometricJacobian motionSubspace = parentJoint.getMotionSubspace(); setUnitMomenta(currentBodyInertia, motionSubspace); setDiagonalTerm(i, motionSubspace); setOffDiagonalTerms(i); buildCrbInertia(i); } }
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());