public void packFootForceSensors(SideDependentList<ArrayList<WrenchCalculatorInterface>> footForceSensors) { ArrayList<WrenchCalculatorInterface> forceSensors = new ArrayList<WrenchCalculatorInterface>(); sdfRobot.getForceSensors(forceSensors); SideDependentList<String> jointNamesBeforeFeet = sdfRobot.getJointNamesBeforeFeet(); for (RobotSide robotSide : RobotSide.values) { footForceSensors.put(robotSide, new ArrayList<WrenchCalculatorInterface>()); for (int i = 0; i < forceSensors.size(); i++) { if (forceSensors.get(i).getJoint().getName().equals(jointNamesBeforeFeet.get(robotSide))) { footForceSensors.get(robotSide).add(forceSensors.get(i)); } } } }
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; }
Joint joint = groundContactPointBasedWrenchCalculator.getJoint(); OneDoFJointBasics oneDoFJoint; if (joint instanceof OneDegreeOfFreedomJoint)