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 void recomputeSubtreeMassesAndTotalMass() { for (int i = 0; i < rigidBodyList.size(); i++) subTreeMassMap.get(rigidBodyList.get(i)).setValue(-1.0); inverseTotalMass = 1.0 / TotalMassCalculator.computeMass(rigidBodyList); }
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; }
public ExternalWrenchHandler(double gravityZ, ReferenceFrame centerOfMassFrame, InverseDynamicsJoint rootJoint, List<? extends ContactablePlaneBody> contactablePlaneBodies) { this.centerOfMassFrame = centerOfMassFrame; MathTools.checkIfInRange(gravityZ, 0.0, Double.POSITIVE_INFINITY); this.contactablePlaneBodies = new ArrayList<>(contactablePlaneBodies); gravitationalWrench = new SpatialForceVector(centerOfMassFrame); double totalMass = TotalMassCalculator.computeMass(ScrewTools.computeSupportAndSubtreeSuccessors(rootJoint.getSuccessor())); gravitationalWrench.setLinearPartZ(-gravityZ * totalMass); totalWrenchAlreadyApplied = new SpatialForceVector(centerOfMassFrame); for (int i = 0; i < contactablePlaneBodies.size(); i++) { RigidBody rigidBody = this.contactablePlaneBodies.get(i).getRigidBody(); externalWrenches.put(rigidBody, new Wrench(rigidBody.getBodyFixedFrame(), rigidBody.getBodyFixedFrame())); } }
@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;
DoubleYoVariable handMass = new DoubleYoVariable(namePrefix + "SubtreeMass", registry); wristSubtreeMass.put(robotSide, handMass); handMass.set(TotalMassCalculator.computeSubTreeMass(measurementLink));
double totalMass = TotalMassCalculator.computeSubTreeMass(elevator); DoubleYoVariable handMass = new DoubleYoVariable(sidePrefix + "HandTotalMass", registry); handsMass.put(robotSide, handMass); handMass.set(TotalMassCalculator.computeSubTreeMass(measurementLink));
robotMass.set(TotalMassCalculator.computeSubTreeMass(elevator)); setupBunchOfVariables();
double subtreeMass = TotalMassCalculator.computeSubTreeMass(highLevelControllerOutputJoint.getSuccessor()); jointController.setProportionalGain(15.0 * subtreeMass); jointController.setIntegralGain(35.0 * subtreeMass);