tempWrench.getAngularPartIncludingFrame(tempTorque); inputForces.get(forceSensorDefinition).set(tempForce); inputTorques.get(forceSensorDefinition).set(tempTorque);
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 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 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 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.getAngularPartIncludingFrame(residualRootJointTorque); residualRootJointWrench.getLinearPartIncludingFrame(residualRootJointForce); yoResidualRootJointForce.setAndMatchFrame(residualRootJointForce);
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.getAngularPartIncludingFrame(residualRootJointTorque); residualRootJointWrench.getLinearPartIncludingFrame(residualRootJointForce); yoResidualRootJointForce.setAndMatchFrame(residualRootJointForce);