private int getForceSensorIndex(String forceSensorName, ForceSensorDefinition[] forceSensorDefinitions) { for(int i = 0; i < forceSensorDefinitions.length; i++) { if(forceSensorDefinitions[i].getSensorName().equals(forceSensorName)) { return i; } } return -1; }
public ForceSensorData(ForceSensorDefinition forceSensorDefinition) { measurementFrame = forceSensorDefinition.getSensorFrame(); measurementLink = forceSensorDefinition.getRigidBody(); }
@Override public boolean equals(Object other) { if(other instanceof ForceSensorDefinition) { ForceSensorDefinition otherSensor = (ForceSensorDefinition) other; return otherSensor.getSensorName().equals(getSensorName()) && otherSensor.getParentJointName().equals(getParentJointName()); } return false; } }
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); }
private void setupForceSensorMassCompensators(FullRobotModel estimatorModel, SideDependentList<String> wristForceSensorNames) { ForceSensorDefinition[] forceSensorDefinitions = estimatorModel.getForceSensorDefinitions(); for (int i = 0; i < forceSensorDefinitions.length; i++) { ForceSensorDefinition sensorDef = forceSensorDefinitions[i]; String forceSensorName = sensorDef.getSensorName(); for (RobotSide robotSide : RobotSide.values) { if (forceSensorName.equals(wristForceSensorNames.get(robotSide))) { ForceSensorDistalMassCompensator massComp = new ForceSensorDistalMassCompensator(sensorDef, WORKER_SLEEP_TIME_MILLIS, registry); wristForceSensorDistalMassCompensators.put(robotSide, massComp); ReferenceFrame sensorFrame = sensorDef.getSensorFrame(); wristForceSensorFrames.put(robotSide, sensorFrame); } } } }
private LinkedHashMap<WrenchCalculatorInterface, ForceSensorDefinition> generateForceSensorDefinitions( ArrayList<WrenchCalculatorInterface> groundContactPointBasedWrenchCalculators) { LinkedHashMap<WrenchCalculatorInterface, ForceSensorDefinition> forceSensorDefinitions = new LinkedHashMap<WrenchCalculatorInterface, ForceSensorDefinition>(); for (WrenchCalculatorInterface groundContactPointBasedWrenchCalculator : groundContactPointBasedWrenchCalculators) { OneDegreeOfFreedomJoint forceTorqueSensorJoint = groundContactPointBasedWrenchCalculator.getJoint(); OneDoFJoint sensorParentJoint = scsToInverseDynamicsJointMap.getInverseDynamicsOneDoFJoint(forceTorqueSensorJoint); RigidBodyTransform transformFromSensorToParentJoint = new RigidBodyTransform(); groundContactPointBasedWrenchCalculator.getTransformToParentJoint(transformFromSensorToParentJoint); ForceSensorDefinition sensorDefinition = new ForceSensorDefinition(groundContactPointBasedWrenchCalculator.getName(), sensorParentJoint.getSuccessor(), transformFromSensorToParentJoint); forceSensorDefinitions.put(groundContactPointBasedWrenchCalculator, sensorDefinition); } return forceSensorDefinitions; }
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); } }
String sensorName = forceSensorDefinition.getSensorName(); allForceSensorNames.add(sensorName); ReferenceFrame sensorFrame = forceSensorDefinition.getSensorFrame();
private LinkedHashMap<WrenchCalculatorInterface, ForceSensorDefinition> generateForceSensorDefinitions( ArrayList<WrenchCalculatorInterface> groundContactPointBasedWrenchCalculators) { LinkedHashMap<WrenchCalculatorInterface, ForceSensorDefinition> forceSensorDefinitions = new LinkedHashMap<WrenchCalculatorInterface, ForceSensorDefinition>(); for (WrenchCalculatorInterface groundContactPointBasedWrenchCalculator : groundContactPointBasedWrenchCalculators) { Joint forceTorqueSensorJoint = groundContactPointBasedWrenchCalculator.getJoint(); OneDoFJointBasics sensorParentJoint; if (forceTorqueSensorJoint instanceof OneDegreeOfFreedomJoint) sensorParentJoint = scsToInverseDynamicsJointMap.getInverseDynamicsOneDoFJoint((OneDegreeOfFreedomJoint) forceTorqueSensorJoint); else throw new RuntimeException("Force sensor is only supported for OneDegreeOfFreedomJoint."); RigidBodyTransform transformFromSensorToParentJoint = new RigidBodyTransform(); groundContactPointBasedWrenchCalculator.getTransformToParentJoint(transformFromSensorToParentJoint); ForceSensorDefinition sensorDefinition = new ForceSensorDefinition(groundContactPointBasedWrenchCalculator.getName(), sensorParentJoint.getSuccessor(), transformFromSensorToParentJoint); forceSensorDefinitions.put(groundContactPointBasedWrenchCalculator, sensorDefinition); } return forceSensorDefinitions; }
private int getForceSensorIndex(String forceSensorName, ForceSensorDefinition[] forceSensorDefinitions) { for(int i = 0; i < forceSensorDefinitions.length; i++) { if(forceSensorDefinitions[i].getSensorName().equals(forceSensorName)) { return i; } } return -1; }
@Override public boolean equals(Object other) { if(other instanceof ForceSensorDefinition) { ForceSensorDefinition otherSensor = (ForceSensorDefinition) other; return otherSensor.getSensorName().equals(getSensorName()) && otherSensor.getParentJointName().equals(getParentJointName()); } return false; } }
public ForceSensorData(ForceSensorDefinition forceSensorDefinition) { measurementFrame = forceSensorDefinition.getSensorFrame(); measurementLink = forceSensorDefinition.getRigidBody(); }
ForceSensorDefinition forceSensorDefinition = new ForceSensorDefinition(sdfForceSensor.getName(), inverseDynamicsJoint.getSuccessor(), sdfForceSensor.getTransformToJoint()); forceSensorDefinitions.add(forceSensorDefinition);
private int getForceSensorIndex(String forceSensorName, ForceSensorDefinition[] forceSensorDefinitions) { for (int i = 0; i < forceSensorDefinitions.length; i++) { if (forceSensorDefinitions[i].getSensorName().equals(forceSensorName)) { return i; } } return -1; }
ReferenceFrame measurementFrame = forceSensorDefinition.getSensorFrame(); ReferenceFrame measurementFrame = forceSensorDefinition.getSensorFrame(); RigidBody measurementLink = forceSensorDefinition.getRigidBody(); RigidBody measurementLink = forceSensorDefinition.getRigidBody(); wrenches.put(measurementLink, new Wrench());
public ForceSensorDataHolder(List<ForceSensorDefinition> forceSensors) { for (ForceSensorDefinition forceSensorDefinition : forceSensors) { ForceSensorData forceSensor = new ForceSensorData(forceSensorDefinition); forceSensorDefinitions.add(forceSensorDefinition); this.forceSensors.put(forceSensorDefinition, forceSensor); sensorNameToDefintionMap.put(forceSensorDefinition.getSensorName(), forceSensorDefinition); } }
public ForceSensorDataHolder(List<ForceSensorDefinition> forceSensors) { for (ForceSensorDefinition forceSensorDefinition : forceSensors) { ForceSensorData forceSensor = new ForceSensorData(forceSensorDefinition); forceSensorDefinitions.add(forceSensorDefinition); this.forceSensors.put(forceSensorDefinition, forceSensor); sensorNameToDefintionMap.put(forceSensorDefinition.getSensorName(), forceSensorDefinition); } }
public static int calculateJointNameHash(OneDoFJointBasics[] joints, ForceSensorDefinition[] forceSensorDefinitions, IMUDefinition[] imuDefinitions) { CRC32 crc = new CRC32(); for (OneDoFJointBasics joint : joints) { crc.update(joint.getName().getBytes()); } for (ForceSensorDefinition forceSensorDefinition : forceSensorDefinitions) { crc.update(forceSensorDefinition.getSensorName().getBytes()); } for (IMUDefinition imuDefinition : imuDefinitions) { crc.update(imuDefinition.getName().getBytes()); } return (int) crc.getValue(); }
public static int calculateJointNameHash(OneDoFJoint[] joints, ForceSensorDefinition[] forceSensorDefinitions, IMUDefinition[] imuDefinitions) { CRC32 crc = new CRC32(); for (OneDoFJoint joint : joints) { crc.update(joint.getName().getBytes()); } for (ForceSensorDefinition forceSensorDefinition : forceSensorDefinitions) { crc.update(forceSensorDefinition.getSensorName().getBytes()); } for (IMUDefinition imuDefinition : imuDefinitions) { crc.update(imuDefinition.getName().getBytes()); } return (int) crc.getValue(); }
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); } } }