private static DenseMatrix64F createJacobianMatrix(LinkedHashMap<InverseDynamicsJoint, List<Twist>> unitTwists) { return new DenseMatrix64F(SpatialMotionVector.SIZE, ScrewTools.computeDegreesOfFreedom(unitTwists.keySet())); }
public DesiredJointAccelerationCalculator(GeometricJacobian jacobian, LinearSolver<DenseMatrix64F> jacobianSolver) { this.jointAccelerations = new DenseMatrix64F(ScrewTools.computeDegreesOfFreedom(jacobian.getJointsInOrder()), 1); this.jacobian = jacobian; this.jacobianSolver = jacobianSolver; }
public JointIndexHandler(InverseDynamicsJoint[] jointsToIndex) { indexedJoints = jointsToIndex; indexedOneDoFJoints = ScrewTools.filterJoints(indexedJoints, OneDoFJoint.class); numberOfDoFs = ScrewTools.computeDegreesOfFreedom(jointsToIndex); for (InverseDynamicsJoint joint : jointsToIndex) { TIntArrayList listToPackIndices = new TIntArrayList(); ScrewTools.computeIndexForJoint(jointsToIndex, listToPackIndices, joint); int[] indices = listToPackIndices.toArray(); columnsForJoints.put(joint, indices); } }
public CenterOfMassJacobian(RigidBody[] rigidBodies, InverseDynamicsJoint[] joints, ReferenceFrame rootFrame) { this.rigidBodyList.addAll(Arrays.asList(rigidBodies)); this.rootFrame = rootFrame; this.joints = joints; int size = ScrewTools.computeDegreesOfFreedom(this.joints); jacobianMatrix = new DenseMatrix64F(3, size); tempJointVelocitiesMatrix = new DenseMatrix64F(size, 1); centerOfMassVelocityMatrix = new DenseMatrix64F(jacobianMatrix.getNumRows(), 1); for (int i = 0; i < rigidBodyList.size(); i++) { RigidBody rigidBody = rigidBodyList.get(i); comScaledByMassMap.put(rigidBody, new FramePoint(rootFrame)); comScaledByMassMapIsUpdated.put(rigidBody, new MutableBoolean(false)); subTreeMassMap.put(rigidBody, new MutableDouble(-1.0)); } recomputeSubtreeMassesAndTotalMass(); }
public DenseMatrix64F getCentroidalMomentumMatrixPart(InverseDynamicsJoint[] joints) { int partDegreesOfFreedom = ScrewTools.computeDegreesOfFreedom(joints); centroidalMomentumMatrixPart.reshape(Momentum.SIZE, partDegreesOfFreedom); centroidalMomentumMatrixPart.zero(); int startColumn = 0; for (InverseDynamicsJoint joint : joints) { int[] columnsForJoint = columnsForJoints.get(joint); MatrixTools.extractColumns(centroidalMomentumRateTermCalculator.getCentroidalMomentumMatrix(), columnsForJoint, centroidalMomentumMatrixPart, startColumn); startColumn += columnsForJoint.length; } return centroidalMomentumMatrixPart; }
public DesiredJointAccelerationCalculatorOld(GeometricJacobian jacobian, JacobianSolver jacobianSolver) { this.jointAccelerations = new DenseMatrix64F(ScrewTools.computeDegreesOfFreedom(jacobian.getJointsInOrder()), 1); this.sign = ScrewTools.isAncestor(jacobian.getEndEffector(), jacobian.getBase()) ? 1 : -1; // because the sign of a joint acceleration changes when you switch a joint's 'direction' in the chain this.jacobian = jacobian; this.jacobianSolver = jacobianSolver; }
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; }
public DifferentialIDMassMatrixCalculator(ReferenceFrame inertialFrame, RigidBody rootBody) { twistCalculator = new TwistCalculator(inertialFrame, rootBody); LinkedHashMap<RigidBody, Wrench> zeroExternalWrench = new LinkedHashMap<RigidBody, Wrench>(); ArrayList<InverseDynamicsJoint> zeroJointToIgnore = new ArrayList<InverseDynamicsJoint>(); SpatialAccelerationVector zeroRootAcceleration = ScrewTools.createGravitationalSpatialAcceleration(rootBody, 0.0); idCalculator = new InverseDynamicsCalculator(inertialFrame, zeroRootAcceleration, zeroExternalWrench, zeroJointToIgnore, false, true, twistCalculator); jointsInOrder = ScrewTools.computeSubtreeJoints(rootBody); totalNumberOfDoFs = ScrewTools.computeDegreesOfFreedom(jointsInOrder); massMatrix = new DenseMatrix64F(totalNumberOfDoFs, totalNumberOfDoFs); storedJointDesiredAccelerations = new DenseMatrix64F(totalNumberOfDoFs, 1); storedJointVelocities = new DenseMatrix64F(totalNumberOfDoFs, 1); tmpDesiredJointAccelerationsMatrix = new DenseMatrix64F(totalNumberOfDoFs, 1); tmpTauMatrix = new DenseMatrix64F(totalNumberOfDoFs, 1); for (InverseDynamicsJoint joint : jointsInOrder) { ReferenceFrame bodyFixedFrame = joint.getSuccessor().getBodyFixedFrame(); Wrench jointWrench = new Wrench(bodyFixedFrame, bodyFixedFrame); storedJointWrenches.put(joint, jointWrench); } }
public CentroidalMomentumMatrix(RigidBody rootBody, ReferenceFrame centerOfMassFrame) { this.jointList = ScrewTools.computeSupportAndSubtreeJoints(rootBody); this.centerOfMassFrame = centerOfMassFrame; int nDegreesOfFreedom = ScrewTools.computeDegreesOfFreedom(jointList); this.centroidalMomentumMatrix = new DenseMatrix64F(6, nDegreesOfFreedom); this.unitMomenta = new Momentum[nDegreesOfFreedom]; for (int i = 0; i < nDegreesOfFreedom; i++) unitMomenta[i] = new Momentum(centerOfMassFrame); isAncestorMapping = new boolean[jointList.length][jointList.length]; for (int j = 0; j < jointList.length; j++) { RigidBody columnRigidBody = jointList[j].getSuccessor(); for (int i = 0; i < jointList.length; i++) { RigidBody rowRigidBody = jointList[i].getSuccessor(); isAncestorMapping[i][j] = ScrewTools.isAncestor(rowRigidBody, columnRigidBody); } } }
public ReNumericalInverseKinematicsCalculator(GeometricJacobian jacobian, double tolerance, int maxIterations, double maxStepSize, double minRandomSearchScalar, double maxRandomSearchScalar) { if (jacobian.getJacobianFrame() != jacobian.getEndEffectorFrame()) throw new RuntimeException("jacobian.getJacobianFrame() != jacobian.getEndEffectorFrame()"); this.jacobian = jacobian; this.solver = LinearSolverFactory.leastSquares(SpatialMotionVector.SIZE, jacobian.getNumberOfColumns()); // new DampedLeastSquaresSolver(jacobian.getNumberOfColumns()); this.oneDoFJoints = ScrewTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJoint.class); oneDoFJointsSeed = oneDoFJoints.clone(); if (oneDoFJoints.length != jacobian.getJointsInOrder().length) throw new RuntimeException("Can currently only handle OneDoFJoints"); int nDoF = ScrewTools.computeDegreesOfFreedom(oneDoFJoints); correction = new DenseMatrix64F(nDoF, 1); current = new DenseMatrix64F(nDoF, 1); best = new DenseMatrix64F(nDoF, 1); this.tolerance = tolerance; this.maxIterations = maxIterations; this.maxStepSize = maxStepSize; this.minRandomSearchScalar = minRandomSearchScalar; this.maxRandomSearchScalar = maxRandomSearchScalar; counter = 0; }
numberOfDoFs = ScrewTools.computeDegreesOfFreedom(oneDoFJoints);
public KinematicSolver(GeometricJacobian jacobian, double tolerance, double maxIterations) { this.jacobian = jacobian; this.tolerance = tolerance; this.maxIterations = maxIterations; this.oneDoFJoints = ScrewTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJoint.class); nDoF = ScrewTools.computeDegreesOfFreedom(oneDoFJoints); jacobianMethod = new DenseMatrix64F(nDoF, nDoF); jacobianTranspose = new DenseMatrix64F(nDoF, nDoF); jacobianJacobianTranspose = new DenseMatrix64F(nDoF, nDoF); jJTe = new DenseMatrix64F(nDoF, 1); correction = new DenseMatrix64F(nDoF, 1); spatialError = new DenseMatrix64F(SpatialMotionVector.SIZE, 1); dampingSquaredDiagonal = new DenseMatrix64F(nDoF, nDoF); inverseTerm = new DenseMatrix64F(nDoF, nDoF); convergeRate = 1E-8; solveMethod = JacobianMethod.JACOBIAN_TRANSPOSE; dampingConstant = 0.3; }
public InverseKinematicsOptimizationControlModule(WholeBodyControlCoreToolbox toolbox, YoVariableRegistry parentRegistry) { jointIndexHandler = toolbox.getJointIndexHandler(); jointsToOptimizeFor = jointIndexHandler.getIndexedJoints(); oneDoFJoints = jointIndexHandler.getIndexedOneDoFJoints(); ReferenceFrame centerOfMassFrame = toolbox.getCenterOfMassFrame(); numberOfDoFs = ScrewTools.computeDegreesOfFreedom(jointsToOptimizeFor); motionQPInput = new MotionQPInput(numberOfDoFs); double controlDT = toolbox.getControlDT(); GeometricJacobianHolder geometricJacobianHolder = toolbox.getGeometricJacobianHolder(); TwistCalculator twistCalculator = toolbox.getTwistCalculator(); motionQPInputCalculator = new MotionQPInputCalculator(centerOfMassFrame, geometricJacobianHolder, twistCalculator, jointIndexHandler, toolbox.getJointPrivilegedConfigurationParameters(), registry); boundCalculator = new InverseDynamicsQPBoundCalculator(jointIndexHandler, controlDT, registry); qDotMinMatrix = new DenseMatrix64F(numberOfDoFs, 1); qDotMaxMatrix = new DenseMatrix64F(numberOfDoFs, 1); for (int i = 0; i < oneDoFJoints.length; i++) { OneDoFJoint joint = oneDoFJoints[i]; jointMaximumVelocities.put(joint, new DoubleYoVariable("qd_max_qp_" + joint.getName(), registry)); jointMinimumVelocities.put(joint, new DoubleYoVariable("qd_min_qp_" + joint.getName(), registry)); } qpSolver = new InverseKinematicsQPSolver(numberOfDoFs, registry); parentRegistry.addChild(registry); }
public CentroidalMomentumHandler(RigidBody rootBody, ReferenceFrame centerOfMassFrame, YoVariableRegistry parentRegistry) { this.jointsInOrder = ScrewTools.computeSupportAndSubtreeJoints(rootBody); this.centroidalMomentumMatrix = new CentroidalMomentumMatrix(rootBody, centerOfMassFrame); this.previousCentroidalMomentumMatrix = new DenseMatrix64F(centroidalMomentumMatrix.getMatrix().getNumRows(), centroidalMomentumMatrix.getMatrix().getNumCols()); yoPreviousCentroidalMomentumMatrix = new DoubleYoVariable[previousCentroidalMomentumMatrix.getNumRows()][previousCentroidalMomentumMatrix.getNumCols()]; MatrixYoVariableConversionTools.populateYoVariables(yoPreviousCentroidalMomentumMatrix, "previousCMMatrix", registry); int nDegreesOfFreedom = ScrewTools.computeDegreesOfFreedom(jointsInOrder); this.v = new DenseMatrix64F(nDegreesOfFreedom, 1); for (InverseDynamicsJoint joint : jointsInOrder) { TIntArrayList listToPackIndices = new TIntArrayList(); ScrewTools.computeIndexForJoint(jointsInOrder, listToPackIndices, joint); int[] indices = listToPackIndices.toArray(); columnsForJoints.put(joint, indices); } centroidalMomentumRate = new SpatialForceVector(centerOfMassFrame); this.centerOfMassFrame = centerOfMassFrame; parentRegistry.addChild(registry); double robotMass = TotalMassCalculator.computeSubTreeMass(rootBody); this.centroidalMomentumRateTermCalculator = new CentroidalMomentumRateTermCalculator(rootBody, centerOfMassFrame, v, robotMass); }
int taskSize = ScrewTools.computeDegreesOfFreedom(commandToConvert.getJoints());
int taskSize = ScrewTools.computeDegreesOfFreedom(commandToConvert.getJoints());
ReferenceFrame centerOfMassFrame = toolbox.getCenterOfMassFrame(); numberOfDoFs = ScrewTools.computeDegreesOfFreedom(jointsToOptimizeFor); int rhoSize = toolbox.getRhoSize();
this.jointList = ScrewTools.computeSupportAndSubtreeJoints(rootBody); this.centerOfMassFrame = centerOfMassFrame; int nDegreesOfFreedom = ScrewTools.computeDegreesOfFreedom(jointList); this.v = v; this.centroidalMomentumMatrix = new DenseMatrix64F(6, nDegreesOfFreedom);
private void resizeMatrices() { int nActuatedDegreesOfFreedom = ScrewTools.computeDegreesOfFreedom(actuatedJoints); S.reshape(nActuatedDegreesOfFreedom, nDegreesOfFreedom); Js.reshape(nConstraints, nDegreesOfFreedom); AInverse.reshape(nDegreesOfFreedom, nDegreesOfFreedom); AInverseJSTranspose.reshape(AInverse.getNumRows(), Js.getNumRows()); LambdasInverse.reshape(Js.getNumRows(), AInverseJSTranspose.getNumCols()); Lambdas.reshape(LambdasInverse.getNumRows(), LambdasInverse.getNumCols()); JsBar.reshape(AInverseJSTranspose.getNumRows(), LambdasInverse.getNumCols()); Ns.reshape(nDegreesOfFreedom, nDegreesOfFreedom); SNs.reshape(S.getNumRows(), Ns.getNumCols()); AInverseSNsTranspose.reshape(AInverse.getNumRows(), SNs.getNumRows()); PhiStar.reshape(SNs.getNumRows(), AInverseSNsTranspose.getNumCols()); PhiStarInverse.reshape(PhiStar.getNumRows(), PhiStar.getNumCols()); SNsBar.reshape(nDegreesOfFreedom, nActuatedDegreesOfFreedom); }
int numberOfDoFs = ScrewTools.computeDegreesOfFreedom(joints); jointVelocities.reshape(numberOfDoFs, 1); jointAccelerations.reshape(numberOfDoFs, 1);