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); } } }
public ReferenceFrame getMeasurementFrame() { return forceSensorData.getMeasurementFrame(); }
public WrenchAndContactSensorFusedFootSwitch(String namePrefix, ForceSensorDataReadOnly forceSensorData, ContactSensor contactSensor, double footSwitchCoPThresholdFraction, double robotTotalWeight, ContactablePlaneBody contactablePlaneBody, YoGraphicsListRegistry yoGraphicsListRegistry, double contactThresholdForce, YoVariableRegistry parentRegistry) { this.registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName()); if (!forceSensorData.getMeasurementLink().equals(contactSensor.getRigidBody())) { throw new RuntimeException("The force sensor and the contact sensor are not on the same link."); } this.wrenchBasedFootSwitch = new WrenchBasedFootSwitch(namePrefix + "WrenchBasedFootSwitch", forceSensorData, footSwitchCoPThresholdFraction, robotTotalWeight, contactablePlaneBody, yoGraphicsListRegistry, contactThresholdForce, registry); this.contactSensor = contactSensor; this.inContact = new BooleanYoVariable(namePrefix + "InContact", registry); parentRegistry.addChild(registry); }
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); }
ReferenceFrame measurementFrame = wristForceSensor.getMeasurementFrame(); RigidBody measurementLink = wristForceSensor.getMeasurementLink(); wristForceSensorMeasurementFrames.put(robotSide, measurementFrame);
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 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); } }
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.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);
ReferenceFrame measurementFrame = forceSensorData.getMeasurementFrame(); forceSensorData.getWrench(footWrench); FrameVector footForce = footWrench.getLinearPartAsFrameVectorCopy(); FrameVector footTorque = footWrench.getAngularPartAsFrameVectorCopy();
@Override public void update() { foreSensorData.getWrench(wrenchToPack); wrenchToPack.getLinearPart(vectorToPack); touchdownDetected.set(vectorToPack.length() > touchdownForceThreshold.getDoubleValue()); touchdownDetectedFiltered.update(); } }
this.momentumBasedController = momentumBasedController; ReferenceFrame forceSensorMeasurementFrame = momentumBasedController.getWristForceSensor(robotSide).getMeasurementFrame();
ReferenceFrame measurementFrame = forceSensorData.getMeasurementFrame(); forceSensorData.getWrench(footWrench); FrameVector3D footForce = new FrameVector3D(footWrench.getLinearPart()); FrameVector3D footTorque = new FrameVector3D(footWrench.getAngularPart());
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 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); } }
public void packEstimatorJoints(long timestamp, long sensorHeadPPSTimestamp, RobotConfigurationData jointConfigurationData) { if (jointConfigurationData == null) { return; } rootJoint.getTranslation(rootTranslation); rootJoint.getRotation(rootOrientation); rootJoint.getAngularVelocity(rootAngularVelocity); rootJoint.getLinearVelocity(rootLinearVelocity); rootJoint.getLinearAcceleration(rootLinearAcceleration); jointConfigurationData.setPelvisAngularVelocity(rootAngularVelocity); jointConfigurationData.setPelvisLinearVelocity(rootLinearVelocity); jointConfigurationData.setPelvisLinearAcceleration(rootLinearAcceleration); jointConfigurationData.setRootTranslation(rootTranslation); jointConfigurationData.setRootOrientation(rootOrientation); jointConfigurationData.setJointState(joints); jointConfigurationData.setTimestamp(timestamp); jointConfigurationData.setSensorHeadPPSTimestamp(sensorHeadPPSTimestamp); for (int sensorNumber = 0; sensorNumber < getNumberOfForceSensors(); sensorNumber++) { float[] forceAndMomentVector = jointConfigurationData.getMomentAndForceVectorForSensor(sensorNumber); forceSensorDataList.get(sensorNumber).getWrench(forceAndMomentVector); } }
private void readSensorData(Wrench footWrenchToPack) { forceSensorData.getWrench(footWrenchToPack); // First in measurement frame for all the frames... footForce.setToZero(footWrenchToPack.getExpressedInFrame()); footWrenchToPack.getLinearPart(footForce); yoFootForce.set(footForce); footTorque.setToZero(footWrenchToPack.getExpressedInFrame()); footWrenchToPack.getAngularPart(footTorque); yoFootTorque.set(footTorque); // magnitude of force part is independent of frame footForceMagnitude.set(footForce.length()); // Now change to frame after the parent joint (ankle or wrist for example): footWrenchInBodyFixedFrame.set(footWrenchToPack); footWrenchInBodyFixedFrame.changeFrame(contactablePlaneBody.getRigidBody().getBodyFixedFrame()); footForce.setToZero(footWrenchInBodyFixedFrame.getExpressedInFrame()); footWrenchInBodyFixedFrame.getLinearPart(footForce); footTorque.setToZero(footWrenchInBodyFixedFrame.getExpressedInFrame()); footWrenchInBodyFixedFrame.getAngularPart(footTorque); footForce.changeFrame(contactablePlaneBody.getFrameAfterParentJoint()); yoFootForceInFoot.set(footForce); footTorque.changeFrame(contactablePlaneBody.getFrameAfterParentJoint()); yoFootTorqueInFoot.set(footTorque); footForce.changeFrame(ReferenceFrame.getWorldFrame()); footTorque.changeFrame(ReferenceFrame.getWorldFrame()); yoFootForceInWorld.set(footForce); yoFootTorqueInWorld.set(footTorque); updateSensorVisualizer(); }