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); } }
public CompositeRigidBodyMassMatrixCalculator(RigidBody rootBody, ArrayList<InverseDynamicsJoint> jointsToIgnore) { this.rootBody = rootBody; this.jointsToIgnore = new ArrayList<InverseDynamicsJoint>(jointsToIgnore); ArrayList<RigidBody> rigidBodiesInOrderList = new ArrayList<RigidBody>(); ArrayList<InverseDynamicsJoint> jointsInOrderList = new ArrayList<InverseDynamicsJoint>(); populateMapsAndLists(jointsInOrderList, rigidBodiesInOrderList); allRigidBodiesInOrder = rigidBodiesInOrderList.toArray(new RigidBody[0]); jointsInOrder = jointsInOrderList.toArray(new InverseDynamicsJoint[0]); crbInertiasInOrder = createCrbInertiasInOrder(allRigidBodiesInOrder.length); parentMap = ScrewTools.createParentMap(allRigidBodiesInOrder); massMatrixIndices = createMassMatrixIndices(allRigidBodiesInOrder); int size = determineSize(allRigidBodiesInOrder); massMatrix = new DenseMatrix64F(size, size); unitMomenta = createMomenta(); }
@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); } }
public MassMatrixCalculatorComparer() { joints = new ArrayList<RevoluteJoint>(); elevator = new RigidBody("elevator", worldFrame); Vector3d[] jointAxes = {X, Y, Z, Z, X, Z, Z, X, Y, Y}; ScrewTestTools.createRandomChainRobot("", joints, elevator, jointAxes, random); massMatrixCalculators.add(new DifferentialIDMassMatrixCalculator(worldFrame, elevator)); massMatrixCalculators.add(new CompositeRigidBodyMassMatrixCalculator(elevator)); }
private void buildCrbInertia(int i) { CompositeRigidBodyInertia currentBodyInertia = crbInertiasInOrder[i]; if (isValidParentIndex(parentMap[i])) { CompositeRigidBodyInertia parentBodyInertia = crbInertiasInOrder[parentMap[i]]; ReferenceFrame parentFrame = parentBodyInertia.getExpressedInFrame(); currentBodyInertia.changeFrame(parentFrame); parentBodyInertia.add(currentBodyInertia); } }
public OriginalDynamicallyConsistentNullspaceCalculator(FloatingInverseDynamicsJoint rootJoint, boolean computeSNsBar) { this.rootJoint = rootJoint; this.massMatrixCalculator = new CompositeRigidBodyMassMatrixCalculator(rootJoint.getSuccessor()); jointsInOrder = massMatrixCalculator.getJointsInOrder(); this.nDegreesOfFreedom = ScrewTools.computeDegreesOfFreedom(jointsInOrder); massMatrixSolver = LinearSolverFactory.symmPosDef(nDegreesOfFreedom); lambdaSolver = LinearSolverFactory.symmPosDef(nDegreesOfFreedom); // size of matrix is only used to choose algorithm. nDegreesOfFreedom is an upper limit pseudoInverseSolver = LinearSolverFactory.pseudoInverse(true); this.computeSNsBar = computeSNsBar; }