public ForceSensorData(ForceSensorDefinition forceSensorDefinition) { measurementFrame = forceSensorDefinition.getSensorFrame(); measurementLink = forceSensorDefinition.getRigidBody(); }
public ForceSensorData(ForceSensorDefinition forceSensorDefinition) { measurementFrame = forceSensorDefinition.getSensorFrame(); measurementLink = forceSensorDefinition.getRigidBody(); }
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); } }
RigidBody measurementLink = forceSensorDefinition.getRigidBody(); RigidBody measurementLink = forceSensorDefinition.getRigidBody(); wrenches.put(measurementLink, new Wrench());
public ForceSensorDistalMassCompensator(ForceSensorDefinition forceSensorDefinition, double dtForLowpassFilter, YoVariableRegistry registry) { String sensorName = forceSensorDefinition.getSensorName(); InverseDynamicsJoint parentJointOfSensorBody = forceSensorDefinition.getRigidBody().getParentJoint(); sensorFrame = forceSensorDefinition.getSensorFrame(); sensorPose = new FramePose(world); yoSensorPositionInWorld = new YoFramePoint(sensorName + "Position", world, registry); distalMassCalc = new CenterOfMassCalculator(ScrewTools.computeRigidBodiesAfterThisJoint(parentJointOfSensorBody), world); distalMass = new DoubleYoVariable(sensorName + "DistalMass", registry); lowPassSensorForceZ = new AlphaFilteredYoVariable(sensorName + "LowPassFz", registry, AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(0.0001, dtForLowpassFilter)); distalMassForceInWorld = new YoFrameVector(sensorName + "DistalWeight", world, registry); distalCoMInWorld = new YoFramePoint(sensorName + "DistalCoM", world, registry); yoSensorToDistalCoMvectorInWorld = new YoFrameVector(sensorName + "ToDistalCoM", world, registry); distalMassWrench = new Wrench(sensorFrame, world); // Put sensor values in world frame since it's easy to interpret from looking at GUI yoSensorForce = new YoFrameVector(sensorName + "Force", world, registry); yoSensorTorque = new YoFrameVector(sensorName + "Torque", world, registry); yoSensorForceFromDistalMass = new YoFrameVector(sensorName + "ForceDueToDistalMass", world, registry); yoSensorTorqueFromDistalMass = new YoFrameVector(sensorName + "TorqueDueToDistalMass", world, registry); yoSensorForceMassCompensated = new YoFrameVector(sensorName + "ForceMassCompensated", world, registry); yoSensorTorqueMassCompensated = new YoFrameVector(sensorName + "TorqueMassCompensated", world, registry); addSimulatedSensorNoise = new BooleanYoVariable(sensorName + "AddSimulatedNoise", registry); addSimulatedSensorNoise.set(false); }