public ReferenceFrame getMeasurementFrame() { return forceSensorData.getMeasurementFrame(); }
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 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); } }
this.secondContactThresholdForce.set(Double.POSITIVE_INFINITY); yoFootForce = new YoFrameVector(namePrefix + "Force", forceSensorData.getMeasurementFrame(), registry); yoFootTorque = new YoFrameVector(namePrefix + "Torque", forceSensorData.getMeasurementFrame(), registry); yoFootForceInFoot = new YoFrameVector(namePrefix + "ForceFootFrame", contactablePlaneBody.getFrameAfterParentJoint(), registry); yoFootTorqueInFoot = new YoFrameVector(namePrefix + "TorqueFootFrame", contactablePlaneBody.getFrameAfterParentJoint(), registry); this.footSwitchCoPThresholdFraction.set(footSwitchCoPThresholdFraction); this.footWrench = new Wrench(forceSensorData.getMeasurementFrame(), null);
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); } }
ReferenceFrame measurementFrame = wristForceSensor.getMeasurementFrame(); RigidBody measurementLink = wristForceSensor.getMeasurementLink(); wristForceSensorMeasurementFrames.put(robotSide, measurementFrame);
this.momentumBasedController = momentumBasedController; ReferenceFrame forceSensorMeasurementFrame = momentumBasedController.getWristForceSensor(robotSide).getMeasurementFrame();
ReferenceFrame measurementFrame = forceSensorData.getMeasurementFrame(); forceSensorData.getWrench(footWrench); FrameVector footForce = footWrench.getLinearPartAsFrameVectorCopy();
ReferenceFrame measurementFrame = forceSensorData.getMeasurementFrame(); forceSensorData.getWrench(footWrench); FrameVector3D footForce = new FrameVector3D(footWrench.getLinearPart());