public static double computeSubTreeMass(RigidBody rootBody) { RigidBodyInertia inertia = rootBody.getInertia(); double ret = inertia == null ? 0.0 : inertia.getMass(); for (InverseDynamicsJoint childJoint : rootBody.getChildrenJoints()) { ret += computeSubTreeMass(childJoint.getSuccessor()); } return ret; }
public static double computeSubTreeMass(RigidBodyBasics rootBody) { SpatialInertiaBasics inertia = rootBody.getInertia(); double ret = inertia == null ? 0.0 : inertia.getMass(); for (JointBasics childJoint : rootBody.getChildrenJoints()) { ret += computeSubTreeMass(childJoint.getSuccessor()); } return ret; }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputeSubTreeMass() { Random random = new Random(100L); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); RigidBodyBasics elevator = new RigidBody("body", worldFrame); int numberOfJoints = 100; double addedMass = createRandomRigidBodyTreeAndReturnTotalMass(worldFrame, elevator, numberOfJoints, random); assertEquals(addedMass, TotalMassCalculator.computeSubTreeMass(elevator), 0.00001); }
totalMass = TotalMassCalculator.computeSubTreeMass(elevator);
double totalRobotWeight = TotalMassCalculator.computeSubTreeMass(estimatorFullRobotModel.getElevator()) * gravityMagnitude;
double totalRobotWeight = TotalMassCalculator.computeSubTreeMass(estimatorFullRobotModel.getElevator()) * gravityMagnitude;
double totalRobotWeight = TotalMassCalculator.computeSubTreeMass(fullRobotModel.getElevator()) * gravityMagnitude;
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); }
totalRobotMass = TotalMassCalculator.computeSubTreeMass(fullRobotModel.getElevator()); else totalRobotMass = TotalMassCalculator.computeSubTreeMass(ScrewTools.getRootBody(controlledJoints[0].getSuccessor()));
String[] imuSensorsToUseInStateEstimator = sensorInformation.getIMUSensorsToUseInStateEstimator(); double gravitationalAcceleration = 9.81; double totalRobotWeight = TotalMassCalculator.computeSubTreeMass(fullRobotModel.getElevator()) * gravitationalAcceleration;
String[] imuSensorsToUseInStateEstimator = sensorInformation.getIMUSensorsToUseInStateEstimator(); double gravitationalAcceleration = 9.81; double totalRobotWeight = TotalMassCalculator.computeSubTreeMass(fullRobotModel.getElevator()) * gravitationalAcceleration;
String[] imuSensorsToUseInStateEstimator = sensorInformation.getIMUSensorsToUseInStateEstimator(); double gravitationalAcceleration = 9.81; double totalRobotWeight = TotalMassCalculator.computeSubTreeMass(fullRobotModel.getElevator()) * gravitationalAcceleration;
contactableBodies.addAll(additionalContacts); double totalRobotWeight = TotalMassCalculator.computeSubTreeMass(fullRobotModel.getElevator()) * gravityZ; SideDependentList<FootSwitchInterface> footSwitches = createFootSwitches(feet, totalRobotWeight, referenceFrames.getSoleZUpFrames());
double totalMass = TotalMassCalculator.computeSubTreeMass(fullRobotModel.getElevator()); double totalRobotWeight = totalMass * gravityZ;
robotMass.set(TotalMassCalculator.computeSubTreeMass(elevator)); setupBunchOfVariables();
double subtreeMass = TotalMassCalculator.computeSubTreeMass(highLevelControllerOutputJoint.getSuccessor()); jointController.setProportionalGain(15.0 * subtreeMass); jointController.setIntegralGain(35.0 * subtreeMass);
double subtreeMass = TotalMassCalculator.computeSubTreeMass(highLevelControllerOutputJoint.getSuccessor()); jointPositionController.setProportionalGain(15.0 * subtreeMass); jointPositionController.setIntegralGain(35.0 * subtreeMass); double subtreeMass = TotalMassCalculator.computeSubTreeMass(highLevelControllerOutputJoint.getSuccessor()); jointPositionController.setProportionalGain(145.0 * subtreeMass); jointPositionController.setIntegralGain(9.0 * 50.0 * subtreeMass);
double subtreeMass = TotalMassCalculator.computeSubTreeMass(highLevelControllerOutputJoint.getSuccessor()); jointController.setProportionalGain(15.0 * subtreeMass); jointController.setIntegralGain(35.0 * subtreeMass);
aTermCalculator.reshape(6,numberOfDoFs); double totalMass = TotalMassCalculator.computeSubTreeMass(elevator);
double totalMass = TotalMassCalculator.computeSubTreeMass(fullRobotModel.getElevator()); double minimumSwingTimeForDisturbanceRecovery = walkingControllerParameters.getMinimumSwingTimeForDisturbanceRecovery();