public RigidBody(String name, RigidBodyInertia inertia, InverseDynamicsJoint parentJoint) { nameBasedHashCode = NameBasedHashCodeTools.combineHashCodes(name, parentJoint); inertia.getBodyFrame().checkReferenceFrameMatch(inertia.getExpressedInFrame()); // inertia should be expressed in body frame, otherwise it will change this.name = name; this.inertia = inertia; this.bodyFixedFrame = inertia.getBodyFrame(); this.parentJoint = parentJoint; this.parentJoint.setSuccessor(this); }
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); } }
/** * Precomputes Ad^{T} * I since it is used twice in Adot*v and once in computing A. */ private void precomputeAdjointTimesInertia() { tempAdjoint.zero(); for(int i = 0; i<jointList.length; i++) { rigidBodies[i].getInertia().getExpressedInFrame().getTransformToDesiredFrame(tempTransform, centerOfMassFrame); tempTransform.get(tempMatrix3d,tempVector); set3By3MatrixBlock(tempAdjoint, 0, 0, tempMatrix3d); set3By3MatrixBlock(tempAdjoint, 3, 3, tempMatrix3d); MatrixTools.toTildeForm(tempPTilde, tempVector); tempPTilde.mul(tempMatrix3d); set3By3MatrixBlock(tempAdjoint, 0, 3, tempPTilde); rigidBodies[i].getInertia().getMatrix(tempInertiaMatrix); CommonOps.mult(tempAdjoint, tempInertiaMatrix, denseAdjTimesI[i]); } }
RigidBodyInertia inertia = rowRigidBody.getInertia(); tempTwist.set(columnJoint.getMotionSubspace().getAllUnitTwists().get(k)); tempTwist.changeFrame(inertia.getExpressedInFrame()); tempMomentum.compute(inertia, tempTwist); tempMomentum.changeFrame(centerOfMassFrame);
tempTwist.changeFrame(rigidBodies[i].getInertia().getExpressedInFrame()); tempTwist.changeFrame(jointList[j].getSuccessor().getInertia().getExpressedInFrame());
tempTwist.changeFrame(inertia.getExpressedInFrame());