Merge pull request #194 from robotology/cleanupftsensorsconfig

Support specifying the FT sensors to use in the estimation via multipleAnalogSensorsNames::SixAxisForceTorqueSensorsNames without the need to modify the loaded URDF

devices/wholeBodyDynamics/README.md

@@ -61,6 +61,26 @@ For an overview on `wholeBodyDynamics` and to understand how to run the device,
   The axes contained in the `axesNames` parameter are then mapped to the wrapped `controlboard` in the `attachAll` method, using `controlBoardRemapper` class.
   Furthermore are also used to match the yarp axes to the joint names found in the passed URDF file.
 
+#### Specifying the Force Torque Sensors to use
+
+By default, the `wholebodydynamics` device takes the list of forcetorque sensors from the  `multipleAnalogSensorsNames` group that signals which FT sensors are used.
+
+In particular, the `SixAxisForceTorqueSensorsNames` parameter from the `multipleAnalogSensorsNames` group specify the sensor names that are used.
+
+Example:
+```
+<group name="multipleAnalogSensorsNames">
+    <param name="SixAxisForceTorqueSensorsNames">(l_leg_ft,r_leg_ft,l_foot_ft,r_foot_ft)</param>
+</group>
+```
+
+The sensors specified in this way need to be:
+* Available in the URDF sensor used for the robot, using the format supported by iDynTree to specify FT sensors (see https://github.com/robotology/idyntree/blob/master/doc/model_loading.md#force_torque)
+* Exposed as YARP devices that expose the [`yarp::dev::ISixAxisForceTorqueSensors`](https://www.yarp.it/latest/classyarp_1_1dev_1_1ISixAxisForceTorqueSensors.html) YARP interface in one of the devices passed to the device via the `attach` xml list or the `attachAll` C++ method.
+
+If the `FT_TEMPERATURE_COEFFICIENTS` parameter is specified, then it is assumed that for each FT sensors there is a temperature sensor with the same name the measure of which is available via the [`yarp::dev::ITemperatureSensors`](https://www.yarp.it/latest/classyarp_1_1dev_1_1ITemperatureSensors.html) interface. If that is not the case, an error is raised. To use the temperature compensation, all used FT sensors must support the temperature compensation.
+
+
 #### Gravity Compensation
   This device also provides gravity compensation torques (using the `IImpedanceControl::setImpedanceOffset` method) for axis that are in compliant interaction mode and in position/position direct/velocity control mode.
 
@@ -149,7 +169,6 @@ Typically this estimates are provided only for the upper joints (arms and torso)
        </group>
   ```
 
-For a detailed explanation on their usage, please see the document [Using temperature coefficients and pre-estimated FT offsets](../../doc/howto/useTemperatureCoefficientsAndOffsetCompensationInWholeBodyDynamics.md).
 
   #### Filters
 

devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h

@@ -366,6 +366,8 @@ class WholeBodyDynamicsDevice :  public yarp::dev::DeviceDriver,
     bool loadSettingsFromConfig(yarp::os::Searchable& config);
     bool loadSecondaryCalibrationSettingsFromConfig(yarp::os::Searchable& config);
     bool loadGravityCompensationSettingsFromConfig(yarp::os::Searchable & config);
+    bool m_temperatureCompensationEnabled{false};
+    bool checkIfTemperatureCompensationIsSetFromConfig(yarp::os::Searchable & config);
     bool loadTemperatureCoefficientsSettingsFromConfig(yarp::os::Searchable& config);
     bool loadFTSensorOffsetFromConfig(yarp::os::Searchable& config);
     bool applyLPFSettingsFromConfig(const yarp::os::Property& config, const std::string& setting_name);
@@ -446,11 +448,9 @@ class WholeBodyDynamicsDevice :  public yarp::dev::DeviceDriver,
     std::vector<wholeBodyDynamics::SixAxisForceTorqueMeasureProcessor> ftProcessors;
 
     /**
-     * Vector of Analog FT Sensor names
+     * Vector of FT Sensor names considered for estimation
      */
-    std::vector<std::string>     ftAnalogSensorNames;
-    std::vector<std::string>     ftMultipleAnalogSensorNames;
-    std::vector<int>  ftMultipleAnalogSensorIdxMapping;
+    std::vector<std::string> ftSensorNames;
 
     /***
      * RPC Calibration related methods

devices/wholeBodyDynamics/test/ergocub_simulated_wholebodydynamics.xml

@@ -13,6 +13,7 @@
   <param name="forceTorqueEstimateConfidence">2</param>
   <param name="useJointVelocity">true</param>
   <param name="useJointAcceleration">false</param>
+  <param name="estimateJointVelocityAcceleration">false</param>
   <param name="streamFilteredFT">true</param>
   <param name="imuFilterCutoffInHz">3.0</param>
   <param name="forceTorqueFilterCutoffInHz">3.0</param>