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 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; }
public ForceSensorHysteresisCreator(double totalRobotMass, String parentJointName, WrenchCalculatorInterface wrenchCalculatorInterface) { super(wrenchCalculatorInterface.getName() + "HysteresisCreator"); this.wrenchCalculatorInterface = wrenchCalculatorInterface; this.wrenchCalculatorInterface.setDoWrenchCorruption(true); this.totalRobotWeightInNewtons = totalRobotMass * 9.81; this.normalForceThreshold = totalRobotWeightInNewtons * PERCENT_OF_FULL_WEIGHT_TO_TRIGGER_HYSTERESIS/100; hysteresisInZDirection = new YoDouble(parentJointName + "ForceSensorZHysteresis", registry); hysteresisInZDirection.set(0); this.isForcePastThresholdFiltered = new GlitchFilteredYoBoolean(parentJointName + "ForceSensorZHysteresisIsForcePastThreshold", registry, ITERS_BEFORE_HYSTERESIS_TRIGGERS); }
public ArrayList<RobotController> getFootForceSensorHysteresisCreators() { SideDependentList<ArrayList<WrenchCalculatorInterface>> footForceSensors = new SideDependentList<ArrayList<WrenchCalculatorInterface>>(); packFootForceSensors(footForceSensors); ArrayList<RobotController> footForceSensorSignalCorruptors = new ArrayList<RobotController>(); for (RobotSide robotSide : RobotSide.values) { for (int i = 0; i < footForceSensors.get(robotSide).size(); i++) { ForceSensorHysteresisCreator forceSensorSignalCorruptor = new ForceSensorHysteresisCreator(sdfRobot.computeCenterOfMass(new Point3D()), footForceSensors.get(robotSide).get(i).getName(), footForceSensors.get(robotSide).get(i)); footForceSensorSignalCorruptors.add(forceSensorSignalCorruptor); } } return footForceSensorSignalCorruptors; }