public static RigidBody[] computeSupportAndSubtreeSuccessors(RigidBody... bodies) { return computeSuccessors(computeSupportAndSubtreeJoints(bodies)); }
public static InverseDynamicsJoint[] computeJointsToOptimizeFor(FullHumanoidRobotModel fullRobotModel, InverseDynamicsJoint... jointsToRemove) { List<InverseDynamicsJoint> joints = new ArrayList<InverseDynamicsJoint>(); InverseDynamicsJoint[] allJoints = ScrewTools.computeSupportAndSubtreeJoints(fullRobotModel.getRootJoint().getSuccessor()); joints.addAll(Arrays.asList(allJoints)); for (RobotSide robotSide : RobotSide.values) { RigidBody hand = fullRobotModel.getHand(robotSide); if (hand != null) { List<InverseDynamicsJoint> fingerJoints = Arrays.asList(ScrewTools.computeSubtreeJoints(hand)); joints.removeAll(fingerJoints); } } if (jointsToRemove != null) { for (InverseDynamicsJoint joint : jointsToRemove) { joints.remove(joint); } } return joints.toArray(new InverseDynamicsJoint[joints.size()]); }
public JointStateFullRobotModelUpdater(ControlFlowGraph controlFlowGraph, Map<OneDoFJoint, ControlFlowOutputPort<double[]>> jointPositionSensors, Map<OneDoFJoint, ControlFlowOutputPort<double[]>> jointVelocitySensors, FullInverseDynamicsStructure inverseDynamicsStructure) { InverseDynamicsJoint[] joints = ScrewTools.computeSupportAndSubtreeJoints(inverseDynamicsStructure.getRootJoint().getSuccessor()); this.oneDoFJoints = ScrewTools.filterJoints(joints, OneDoFJoint.class); this.inverseDynamicsStructureOutputPort = createOutputPort("inverseDynamicsStructureOutputPort"); inverseDynamicsStructureOutputPort.setData(inverseDynamicsStructure); for (OneDoFJoint oneDoFJoint : oneDoFJoints) { if (jointPositionSensors.get(oneDoFJoint) == null) { throw new RuntimeException("positionSensorPorts.get(oneDoFJoint) == null. oneDoFJoint = " + oneDoFJoint); } } for (OneDoFJoint oneDoFJoint : oneDoFJoints) { ControlFlowOutputPort<double[]> positionSensorOutputPort = jointPositionSensors.get(oneDoFJoint); ControlFlowInputPort<double[]> positionSensorInputPort = createInputPort("positionSensorInputPort"); positionSensorInputPorts.put(oneDoFJoint, positionSensorInputPort); controlFlowGraph.connectElements(positionSensorOutputPort, positionSensorInputPort); ControlFlowOutputPort<double[]> velocitySensorOutputPort = jointVelocitySensors.get(oneDoFJoint); ControlFlowInputPort<double[]> velocitySensorInputPort = createInputPort("velocitySensorInputPort"); velocitySensorInputPorts.put(oneDoFJoint, velocitySensorInputPort); controlFlowGraph.connectElements(velocitySensorOutputPort, velocitySensorInputPort); } }
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 JointStateUpdater(FullInverseDynamicsStructure inverseDynamicsStructure, SensorOutputMapReadOnly sensorOutputMapReadOnly, StateEstimatorParameters stateEstimatorParameters, YoVariableRegistry parentRegistry) { twistCalculator = inverseDynamicsStructure.getTwistCalculator(); spatialAccelerationCalculator = inverseDynamicsStructure.getSpatialAccelerationCalculator(); rootBody = twistCalculator.getRootBody(); this.sensorMap = sensorOutputMapReadOnly; InverseDynamicsJoint[] joints = ScrewTools.computeSupportAndSubtreeJoints(inverseDynamicsStructure.getRootJoint().getSuccessor()); this.oneDoFJoints = ScrewTools.filterJoints(joints, OneDoFJoint.class); iMUBasedJointVelocityEstimator = createIMUBasedJointVelocityEstimator(sensorOutputMapReadOnly, stateEstimatorParameters, registry); parentRegistry.addChild(registry); }
public SideDependentList<ContactablePlaneBody> createHandContactableBodies(RigidBody rootBody) { if (namesOfJointsBeforeHands == null) return null; InverseDynamicsJoint[] allJoints = ScrewTools.computeSupportAndSubtreeJoints(rootBody); SideDependentList<ContactablePlaneBody> handContactableBodies = new SideDependentList<>(); for (RobotSide robotSide : RobotSide.values) { InverseDynamicsJoint[] jointBeforeHandArray = ScrewTools.findJointsWithNames(allJoints, namesOfJointsBeforeHands.get(robotSide)); if (jointBeforeHandArray.length != 1) throw new RuntimeException("Incorrect number of joints before hand found: " + jointBeforeHandArray.length); RigidBody hand = jointBeforeHandArray[0].getSuccessor(); String name = robotSide.getCamelCaseNameForStartOfExpression() + "HandContact"; ListOfPointsContactablePlaneBody handContactableBody = createListOfPointsContactablePlaneBody(name, hand, handContactPointTransforms.get(robotSide), handContactPoints.get(robotSide)); handContactableBodies.put(robotSide, handContactableBody); } return handContactableBodies; }
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); }
public CentroidalMomentumRateADotVTerm(RigidBody rootBody, ReferenceFrame centerOfMassFrame, CentroidalMomentumMatrix centroidalMomentumMatrix, double robotMass, DenseMatrix64F v) { this.jointsInOrder = ScrewTools.computeSupportAndSubtreeJoints(rootBody); this.robotMass = robotMass; this.centerOfMassFrame = centerOfMassFrame; this.rootBody = rootBody; this.v = v; this.twistCalculator = new TwistCalculator(ReferenceFrame.getWorldFrame(), rootBody); this.centroidalMomentumMatrix = centroidalMomentumMatrix; this.rootAcceleration = new SpatialAccelerationVector(rootBody.getBodyFixedFrame(), ReferenceFrame.getWorldFrame(), rootBody.getBodyFixedFrame()); this.spatialAccelerationCalculator = new SpatialAccelerationCalculator(rootBody, ReferenceFrame.getWorldFrame(), this.rootAcceleration, this.twistCalculator, true, false, false); this.comTwist = new Twist(centerOfMassFrame, rootBody.getBodyFixedFrame(), centerOfMassFrame); comTwist.setToZero(); this.comVelocityVector = new Vector3d(); this.aDotV = new DenseMatrix64F(6, 1); this.tempSpatialAcceleration = new SpatialAccelerationVector(); this.tempMomentum = new Momentum(); this.tempTwist = new Twist(); this.tempCoMTwist = new Twist(); this.tempVector = new Vector3d(0, 0, 0); this.leftSide = new Momentum(centerOfMassFrame); }
allJoints = ScrewTools.computeSupportAndSubtreeJoints(fullRobotModel.getRootJoint().getSuccessor()); v = new DenseMatrix64F(ScrewTools.computeDegreesOfFreedom(allJoints), 1);
public CentroidalMomentumRateTermCalculator(RigidBody rootBody, ReferenceFrame centerOfMassFrame,DenseMatrix64F v,double robotMass) this.jointList = ScrewTools.computeSupportAndSubtreeJoints(rootBody); this.centerOfMassFrame = centerOfMassFrame; int nDegreesOfFreedom = ScrewTools.computeDegreesOfFreedom(jointList);