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