private void computeTotalGroundReactionForce() { totalGroundReactionForce.setToZero(); for (int i = 0; i < feet.size(); i++) { RigidBody foot = feet.get(i); Wrench footWrench = footWrenches.get(foot); footSwitches.get(foot).computeAndPackFootWrench(footWrench); footWrench.getLinearPartIncludingFrame(tempFootForce); tempFootForce.changeFrame(worldFrame); totalGroundReactionForce.add(tempFootForce); } totalGroundReactionForce.getFrameTuple(tempCoMAcceleration); comAcceleration.set(tempCoMAcceleration); comAcceleration.setZ(comAcceleration.getZ() - robotMass.getDoubleValue() * gravitationalAcceleration); }
public void compute(Map<RigidBody, Wrench> externalWrenches) { for (int i = 0; i < contactablePlaneBodies.size(); i++) { ContactablePlaneBody contactablePlaneBody = contactablePlaneBodies.get(i); FramePoint2d cop = cops.get(contactablePlaneBody); YoFramePoint2d yoCop = yoCops.get(contactablePlaneBody); yoCop.getFrameTuple2d(cop); Wrench wrench = externalWrenches.get(contactablePlaneBody.getRigidBody()); if (wrench != null) { wrench.getLinearPartIncludingFrame(tempForce); double normalTorque = centerOfPressureResolver.resolveCenterOfPressureAndNormalTorque(cop, wrench, contactablePlaneBody.getSoleFrame()); centersOfPressure2d.get(contactablePlaneBody).set(cop); tempCoP3d.setXYIncludingFrame(cop); centersOfPressureWorld.get(contactablePlaneBody).setAndMatchFrame(tempCoP3d); groundReactionForceMagnitudes.get(contactablePlaneBody).set(tempForce.length()); normalTorques.get(contactablePlaneBody).set(normalTorque); } else { groundReactionForceMagnitudes.get(contactablePlaneBody).set(0.0); centersOfPressureWorld.get(contactablePlaneBody).setToNaN(); cop.setToNaN(); } yoCop.set(cop); desiredCenterOfPressureDataHolder.setCenterOfPressure(cop, contactablePlaneBody.getRigidBody()); } }
tempWrench.getLinearPartIncludingFrame(tempForce); tempWrench.getAngularPartIncludingFrame(tempTorque); inputForces.get(forceSensorDefinition).set(tempForce);
private void updateWristMeasuredWrench(FrameVector measuredForceToPack, FrameVector measuredTorqueToPack) { momentumBasedController.getWristMeasuredWrenchHandWeightCancelled(measuredWrench, robotSide); measuredWrench.getLinearPartIncludingFrame(tempForceVector); measuredWrench.getAngularPartIncludingFrame(tempTorqueVector); deadzoneMeasuredForce.update(tempForceVector); deadzoneMeasuredTorque.update(tempTorqueVector); filteredMeasuredForce.update(); filteredMeasuredTorque.update(); filteredMeasuredForce.getFrameTupleIncludingFrame(tempForceVector); filteredMeasuredTorque.getFrameTupleIncludingFrame(tempTorqueVector); measuredWrench.setLinearPart(tempForceVector); measuredWrench.setAngularPart(tempTorqueVector); measuredWrench.changeFrame(controlFrame); measuredWrench.getLinearPartIncludingFrame(measuredForceToPack); measuredWrench.getAngularPartIncludingFrame(measuredTorqueToPack); }
private void readWristSensorData() { if (wristForceSensors == null) return; for (RobotSide robotSide : RobotSide.values) { ForceSensorDataReadOnly wristForceSensor = wristForceSensors.get(robotSide); ReferenceFrame measurementFrame = wristForceSensor.getMeasurementFrame(); wristForceSensor.getWrench(wristTempWrench); wristTempWrench.getLinearPartIncludingFrame(tempWristForce); wristTempWrench.getAngularPartIncludingFrame(tempWristTorque); wristRawMeasuredForces.get(robotSide).setAndMatchFrame(tempWristForce); wristRawMeasuredTorques.get(robotSide).setAndMatchFrame(tempWristTorque); cancelHandWeight(robotSide, wristTempWrench, measurementFrame); wristTempWrench.getLinearPartIncludingFrame(tempWristForce); wristTempWrench.getAngularPartIncludingFrame(tempWristTorque); wristForcesHandWeightCancelled.get(robotSide).setAndMatchFrame(tempWristForce); wristTorquesHandWeightCancelled.get(robotSide).setAndMatchFrame(tempWristTorque); } }
private void calibrateFootForceSensors() { for (RobotSide robotSide : RobotSide.values) { ForceSensorDataReadOnly footForceSensor = inputForceSensorDataHolder.get(footForceSensorDefinitions.get(robotSide)); footForceSensor.getWrench(tempWrench); tempWrench.getLinearPartIncludingFrame(tempForce); tempWrench.getAngularPartIncludingFrame(tempTorque); footForceCalibrationOffsets.get(robotSide).setAndMatchFrame(tempForce); footTorqueCalibrationOffsets.get(robotSide).setAndMatchFrame(tempTorque); } calibrateFootForceSensors.set(false); }
private void calibrateWristForceSensors() { for (RobotSide robotSide : RobotSide.values) { ForceSensorDataReadOnly wristForceSensor = inputForceSensorDataHolder.get(wristForceSensorDefinitions.get(robotSide)); ReferenceFrame measurementFrame = wristForceSensor.getMeasurementFrame(); wristForceSensor.getWrench(tempWrench); cancelHandWeight(robotSide, tempWrench, measurementFrame); tempWrench.getLinearPartIncludingFrame(tempForce); tempWrench.getAngularPartIncludingFrame(tempTorque); wristForceCalibrationOffsets.get(robotSide).setAndMatchFrame(tempForce); wristTorqueCalibrationOffsets.get(robotSide).setAndMatchFrame(tempTorque); } calibrateWristForceSensors.set(false); }
residualRootJointWrench.getLinearPartIncludingFrame(residualRootJointForce); yoResidualRootJointForce.setAndMatchFrame(residualRootJointForce); yoResidualRootJointTorque.setAndMatchFrame(residualRootJointTorque);
private DenseMatrix64F computeSingleRhoCoPJacobian(FramePoint basisVectorOrigin, FrameVector basisVector) { wrenchFromRho.getLinearPartIncludingFrame(forceFromRho); forceFromRho.changeFrame(planeFrame); if (forceFromRho.getZ() > 1.0e-1) { basisVectorOrigin.changeFrame(planeFrame); basisVector.changeFrame(planeFrame); unitSpatialForceVector.setIncludingFrame(basisVector, basisVectorOrigin); singleRhoCoPJacobian.set(0, 0, -unitSpatialForceVector.getAngularPartY() / forceFromRho.getZ()); singleRhoCoPJacobian.set(1, 0, unitSpatialForceVector.getAngularPartX() / forceFromRho.getZ()); } else { singleRhoCoPJacobian.zero(); } return singleRhoCoPJacobian; }
private void updateWristForceSensorState() { if (requestWristForceSensorCalibrationSubscriber != null && requestWristForceSensorCalibrationSubscriber.checkForNewCalibrationRequest()) calibrateWristForceSensors.set(true); if (calibrateWristForceSensors.getBooleanValue()) calibrateWristForceSensors(); for (RobotSide robotSide : RobotSide.values) { ForceSensorDefinition wristForceSensorDefinition = wristForceSensorDefinitions.get(robotSide); ForceSensorDataReadOnly wristForceSensor = inputForceSensorDataHolder.get(wristForceSensorDefinition); ReferenceFrame measurementFrame = wristForceSensor.getMeasurementFrame(); RigidBody measurementLink = wristForceSensorDefinition.getRigidBody(); wristForceSensor.getWrench(tempWrench); wristForceCalibrationOffsets.get(robotSide).getFrameTupleIncludingFrame(tempForce); wristTorqueCalibrationOffsets.get(robotSide).getFrameTupleIncludingFrame(tempTorque); tempWrench.subLinearPart(tempForce); tempWrench.subAngularPart(tempTorque); outputForceSensorDataHolder.setForceSensorValue(wristForceSensorDefinition, tempWrench); tempWrench.getLinearPartIncludingFrame(tempForce); tempWrench.getAngularPartIncludingFrame(tempTorque); cancelHandWeight(robotSide, tempWrench, measurementFrame); tempWrench.getLinearPartIncludingFrame(tempForce); tempWrench.getAngularPartIncludingFrame(tempTorque); wristForcesSubtreeWeightCancelled.get(robotSide).setAndMatchFrame(tempForce); wristTorquesSubtreeWeightCancelled.get(robotSide).setAndMatchFrame(tempTorque); if (wrenches != null) wrenches.get(measurementLink).set(tempWrench); outputForceSensorDataHolderWithGravityCancelled.setForceSensorValue(wristForceSensorDefinition, tempWrench); } }
residualRootJointWrench.getLinearPartIncludingFrame(residualRootJointForce); yoResidualRootJointForce.setAndMatchFrame(residualRootJointForce); yoResidualRootJointTorque.setAndMatchFrame(residualRootJointTorque);
footWrench.getLinearPartIncludingFrame(footForceVector); footForceVector.changeFrame(ReferenceFrame.getWorldFrame());