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 void compactBlockToFullBlockIgnoreUnindexedJoints(InverseDynamicsJoint[] joints, DenseMatrix64F compactMatrix, DenseMatrix64F fullMatrix) { fullMatrix.zero(); for (int index = 0; index < joints.length; index++) { InverseDynamicsJoint joint = joints[index]; indicesIntoCompactBlock.reset(); ScrewTools.computeIndexForJoint(joints, indicesIntoCompactBlock, joint); int[] indicesIntoFullBlock = columnsForJoints.get(joint); if (indicesIntoFullBlock == null) // don't do anything for joints that are not in the list continue; for (int i = 0; i < indicesIntoCompactBlock.size(); i++) { int compactBlockIndex = indicesIntoCompactBlock.get(i); int fullBlockIndex = indicesIntoFullBlock[i]; CommonOps.extract(compactMatrix, 0, compactMatrix.getNumRows(), compactBlockIndex, compactBlockIndex + 1, fullMatrix, 0, fullBlockIndex); } } }
public boolean compactBlockToFullBlock(InverseDynamicsJoint[] joints, DenseMatrix64F compactMatrix, DenseMatrix64F fullMatrix) { fullMatrix.zero(); for (int index = 0; index < joints.length; index++) { InverseDynamicsJoint joint = joints[index]; indicesIntoCompactBlock.reset(); ScrewTools.computeIndexForJoint(joints, indicesIntoCompactBlock, joint); int[] indicesIntoFullBlock = columnsForJoints.get(joint); if (indicesIntoFullBlock == null) // don't do anything for joints that are not in the list return false; for (int i = 0; i < indicesIntoCompactBlock.size(); i++) { int compactBlockIndex = indicesIntoCompactBlock.get(i); int fullBlockIndex = indicesIntoFullBlock[i]; CommonOps.extract(compactMatrix, 0, compactMatrix.getNumRows(), compactBlockIndex, compactBlockIndex + 1, fullMatrix, 0, fullBlockIndex); } } return true; }
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); }
ScrewTools.computeIndexForJoint(jointsUsedInTask, tempJointIndices, joint); for (int j = 0; j < tempJointIndices.size(); j++) MatrixTools.scaleColumn(secondaryTaskJointsWeight.getDoubleValue(), tempJointIndices.get(j), tempTaskJacobian);