public void set(ForceSensorDataHolderReadOnly otherForceSensorDataHolder) { for (int i = 0; i < forceSensorDefinitions.size(); i++) { final ForceSensorDefinition forceSensorDefinition = forceSensorDefinitions.get(i); ForceSensorDataReadOnly otherForceSensorData = otherForceSensorDataHolder.get(forceSensorDefinition); if (otherForceSensorData == null) { if (firstException) { firstException = false; System.err.println(getClass().getSimpleName() + " Could not find the force sensor: " + forceSensorDefinition.getSensorName()); } } else { otherForceSensorData.getWrench(tempWrench); forceSensors.get(forceSensorDefinition).setWrench(tempWrench); } } }
private SideDependentList<ForceSensorDataReadOnly> createWristForceSensors(ForceSensorDataHolderReadOnly forceSensorDataHolder) { if (wristSensorNames == null) return null; SideDependentList<ForceSensorDataReadOnly> wristForceSensors = new SideDependentList<>(); for (RobotSide robotSide : RobotSide.values) { if (wristSensorNames.get(robotSide) == null) { return null; } ForceSensorDataReadOnly wristForceSensor = forceSensorDataHolder.getByName(wristSensorNames.get(robotSide)); wristForceSensors.put(robotSide, wristForceSensor); } return wristForceSensors; }
/** * The estimated state of the whole robot is packed and sent to the GUI using the DRCJointConfigurationData packet. * Hackish shit going on around here: the lidar and finger joints are not packed in the packet. * @param estimatorModel * @param forceSensorDataHolderForEstimator */ public JointConfigurationGatherer(FullHumanoidRobotModel estimatorModel, ForceSensorDataHolderReadOnly forceSensorDataHolderForEstimator) { this.rootJoint = estimatorModel.getRootJoint(); FullRobotModelUtils.getAllJointsExcludingHands(joints, estimatorModel); forceSensorDefinitions = forceSensorDataHolderForEstimator.getForceSensorDefinitions().toArray(new ForceSensorDefinition[forceSensorDataHolderForEstimator.getForceSensorDefinitions().size()]); for (ForceSensorDefinition definition : forceSensorDefinitions) { String sensorName = definition.getSensorName(); forceSensorNameList.add(sensorName); ForceSensorDataReadOnly forceSensorData = forceSensorDataHolderForEstimator.get(definition); forceSensorDataList.add(forceSensorData); } }
this.forceSensorDataHolderToUpdate = forceSensorDataHolderToUpdate; outputForceSensorDataHolder = new ForceSensorDataHolder(inputForceSensorDataHolder.getForceSensorDefinitions()); outputForceSensorDataHolderWithGravityCancelled = new ForceSensorDataHolder(inputForceSensorDataHolder.getForceSensorDefinitions()); ForceSensorDefinition forceSensorDefinition = inputForceSensorDataHolder.findForceSensorDefinition(footForceSensorNames.get(robotSide)); footForceSensorDefinitions.put(robotSide, forceSensorDefinition); ForceSensorDefinition forceSensorDefinition = inputForceSensorDataHolder.findForceSensorDefinition(wristForceSensorNames.get(robotSide)); wristForceSensorDefinitions.put(robotSide, forceSensorDefinition); ForceSensorDefinition forceSensorDefinition = inputForceSensorDataHolder.findForceSensorDefinition(wristForceSensorNames.get(robotSide)); RigidBody measurementLink = forceSensorDefinition.getRigidBody(); wrenches.put(measurementLink, new Wrench());
private boolean checkIfWristForceSensorsExist(StateEstimatorParameters stateEstimatorParameters) { SideDependentList<String> wristForceSensorNames = stateEstimatorParameters.getWristForceSensorNames(); if (wristForceSensorNames == null || wristForceSensorNames.isEmpty()) { return false; } // Make sure that both sensors actually exist if (inputForceSensorDataHolder.findForceSensorDefinition(wristForceSensorNames.get(RobotSide.LEFT)) == null || inputForceSensorDataHolder.findForceSensorDefinition(wristForceSensorNames.get(RobotSide.RIGHT)) == null) { return false; } return true; }
private boolean checkIfFootForceSensorsExist(StateEstimatorParameters stateEstimatorParameters) { SideDependentList<String> footForceSensorNames = stateEstimatorParameters.getFootForceSensorNames(); if (footForceSensorNames == null || footForceSensorNames.isEmpty()) { return false; } // Make sure that both sensors actually exist if (inputForceSensorDataHolder.findForceSensorDefinition(footForceSensorNames.get(RobotSide.LEFT)) == null || inputForceSensorDataHolder.findForceSensorDefinition(footForceSensorNames.get(RobotSide.RIGHT)) == null) { return false; } return true; }
public void set(ForceSensorDataHolderReadOnly otherForceSensorDataHolder) { for (int i = 0; i < forceSensorDefinitions.size(); i++) { final ForceSensorDefinition forceSensorDefinition = forceSensorDefinitions.get(i); ForceSensorDataReadOnly otherForceSensorData = otherForceSensorDataHolder.get(forceSensorDefinition); if (otherForceSensorData == null) { if (firstException) { firstException = false; System.err.println(getClass().getSimpleName() + " Could not find the force sensor: " + forceSensorDefinition.getSensorName()); } } else { otherForceSensorData.getWrench(tempWrench); forceSensors.get(forceSensorDefinition).setWrench(tempWrench); } } }
this.forceSensorDataHolder = forceSensorDataHolder; ForceSensorDataReadOnly leftAnkleForceSensor = forceSensorDataHolder.getByName("LeftAnkle"); ForceSensorDataReadOnly rightAnkleForceSensor = forceSensorDataHolder.getByName("RightAnkle"); ankleForceSensors.put(RobotSide.LEFT, leftAnkleForceSensor); ankleForceSensors.put(RobotSide.RIGHT, rightAnkleForceSensor);
private void updateFootForceSensorState() { if (calibrateFootForceSensorsAtomic.getAndSet(false)) { calibrateFootForceSensors.set(false); calibrateFootForceSensors(); } for (RobotSide robotSide : RobotSide.values) { ForceSensorDefinition footForceSensorDefinition = footForceSensorDefinitions.get(robotSide); ForceSensorDataReadOnly footForceSensor = inputForceSensorDataHolder.get(footForceSensorDefinition); footForceSensor.getWrench(tempWrench); footForceCalibrationOffsets.get(robotSide).getFrameTupleIncludingFrame(tempForce); footTorqueCalibrationOffsets.get(robotSide).getFrameTupleIncludingFrame(tempTorque); tempWrench.subLinearPart(tempForce); tempWrench.subAngularPart(tempTorque); outputForceSensorDataHolder.setForceSensorValue(footForceSensorDefinition, tempWrench); } }
this.forceSensorDataHolder = forceSensorDataHolder; ForceSensorDataReadOnly leftAnkleForceSensor = forceSensorDataHolder.getByName("LeftAnkle"); ForceSensorDataReadOnly rightAnkleForceSensor = forceSensorDataHolder.getByName("RightAnkle"); ankleForceSensors.put(RobotSide.LEFT, leftAnkleForceSensor); ankleForceSensors.put(RobotSide.RIGHT, rightAnkleForceSensor);
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); }
ForceSensorDataReadOnly footForceSensor = forceSensorDataHolder.getByName(footSensorNames.get(robotSide)); double contactThresholdForce = walkingControllerParameters.getContactThresholdForce(); double footSwitchCoPThresholdFraction = walkingControllerParameters.getCoPThresholdFraction();
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); }
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); } }