Change FT sensors names for compatibility with icub-models >= 2.0.0, …

…robots-configuration >= 2.5.0 and ergocub-software >= 0.3.4 (#720)
ami-iit · Sep 4, 2023 · 3ade8a3 · 3ade8a3
1 parent 03194d6
commit 3ade8a3
Show file tree

Hide file tree

Showing 15 changed files with 34 additions and 32 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -18,6 +18,8 @@ All notable changes to this project are documented in this file.
 - Give the possibility to set an external wrench in the `CentroidalDynamics` instead of a pure force (https://github.com/ami-iit/bipedal-locomotion-framework/pull/705)
 - Use c version of `qhull` in the Planner component. This fixes the compatibility with PCL in ubuntu 20.04 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/713)
 - Make `ICameraBridge::isValid()` virtual function (https://github.com/ami-iit/bipedal-locomotion-framework/pull/695)
+- icub-models 2.0.0 changed the name of the FT sensors in the iCub's URDF from being named `<identifier>_ft_sensor` (like `l_arm_ft_sensor`, `l_leg_ft_sensor`, ...) to `<identifier>_ft` (like `l_arm_ft`, `l_leg_ft`, ...). However, the yarprobotinterface configuration files in blf continued to refer to the sensors as `<identifier>_ft_sensor`, creating errors for software that was trying to match sensors find in URDF and sensors as exposed by the YARP's multipleanalogsensorsserver device. This PR changes all the instances of FT sensor names in iCub-related configuration files contained in blf to `<identifier>_ft`, restoring compatibility with icub-models 2.0.0, robots-configuration releases >= 2.5.0 and ergocub-software >= 0.3.4, see https://github.com/robotology/robots-configuration/pull/562 for more details (https://github.com/ami-iit/bipedal-locomotion-framework/pull/720)
+
 
 ### Fixed
 - Remove duplicated `find_package` in `BipedalLocomotionFrameworkDependencies.cmake` file (https://github.com/ami-iit/bipedal-locomotion-framework/pull/709)

diff --git a/...LoggerDevice/app/robots/ergoCubGazeboV1/blf-yarp-robot-logger-interfaces/mas-remapper.xml b/...LoggerDevice/app/robots/ergoCubGazeboV1/blf-yarp-robot-logger-interfaces/mas-remapper.xml
@@ -6,7 +6,7 @@
 <device  xmlns:xi="http://www.w3.org/2001/XInclude" name="mas-remapper" type="multipleanalogsensorsremapper">
   <param name="period">10</param>
   <param name="SixAxisForceTorqueSensorsNames">
-    (l_arm_ft_sensor, r_arm_ft_sensor, l_leg_ft_sensor, l_foot_front_ft_sensor, l_foot_rear_ft_sensor, r_leg_ft_sensor, r_foot_front_ft_sensor, r_foot_rear_ft_sensor)
+    (l_arm_ft, r_arm_ft, l_leg_ft, l_foot_front_ft, l_foot_rear_ft, r_leg_ft, r_foot_front_ft, r_foot_rear_ft)
   </param>
 
   <action phase="startup" level="5" type="attach">

diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/yarp-robot-logger.xml
@@ -70,7 +70,7 @@ BSD-3-Clause license. -->
     </group>
 
     <group name="SixAxisForceTorqueSensors">
-      <param name="sixaxis_forcetorque_sensors_list">("l_arm_ft_sensor", "r_arm_ft_sensor", "l_leg_ft_sensor", "l_foot_front_ft_sensor", "l_foot_rear_ft_sensor", "r_leg_ft_sensor", "r_foot_front_ft_sensor", "r_foot_rear_ft_sensor")</param>
+      <param name="sixaxis_forcetorque_sensors_list">("l_arm_ft", "r_arm_ft", "l_leg_ft", "l_foot_front_ft", "l_foot_rear_ft", "r_leg_ft", "r_foot_front_ft", "r_foot_rear_ft")</param>
     </group>
 
   </group>

diff --git a/...botLoggerDevice/app/robots/ergoCubSN000/blf-yarp-robot-logger-interfaces/mas-remapper.xml b/...botLoggerDevice/app/robots/ergoCubSN000/blf-yarp-robot-logger-interfaces/mas-remapper.xml
@@ -18,10 +18,10 @@
     (rfeimu_eul, l_arm_ft_eul, r_arm_ft_eul, l_foot_front_ft_eul, l_foot_rear_ft_eul, r_foot_front_ft_eul, r_foot_rear_ft_eul, l_leg_ft_eul, r_leg_ft_eul)
   </param>
   <param name="SixAxisForceTorqueSensorsNames">
-    (l_arm_ft_sensor, r_arm_ft_sensor, l_foot_front_ft_sensor, l_foot_rear_ft_sensor, r_foot_front_ft_sensor, r_foot_rear_ft_sensor, l_leg_ft_sensor, r_leg_ft_sensor)
+    (l_arm_ft, r_arm_ft, l_foot_front_ft, l_foot_rear_ft, r_foot_front_ft, r_foot_rear_ft, l_leg_ft, r_leg_ft)
   </param>
   <param name="TemperatureSensorsNames">
-    (l_arm_ft_sensor, r_arm_ft_sensor, l_foot_front_ft_sensor, l_foot_rear_ft_sensor, r_foot_front_ft_sensor, r_foot_rear_ft_sensor, l_leg_ft_sensor, r_leg_ft_sensor)
+    (l_arm_ft, r_arm_ft, l_foot_front_ft, l_foot_rear_ft, r_foot_front_ft, r_foot_rear_ft, l_leg_ft, r_leg_ft)
   </param>
 
   <action phase="startup" level="5" type="attach">

diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN000/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN000/yarp-robot-logger.xml
@@ -67,11 +67,11 @@ BSD-3-Clause license. -->
     </group>
 
     <group name="SixAxisForceTorqueSensors">
-      <param name="sixaxis_forcetorque_sensors_list">(l_arm_ft_sensor, r_arm_ft_sensor, l_foot_front_ft_sensor, l_foot_rear_ft_sensor, r_foot_front_ft_sensor, r_foot_rear_ft_sensor, l_leg_ft_sensor, r_leg_ft_sensor)</param>
+      <param name="sixaxis_forcetorque_sensors_list">(l_arm_ft, r_arm_ft, l_foot_front_ft, l_foot_rear_ft, r_foot_front_ft, r_foot_rear_ft, l_leg_ft, r_leg_ft)</param>
     </group>
 
     <group name="TemperatureSensors">
-      <param name="temperature_sensors_list">(l_arm_ft_sensor, r_arm_ft_sensor, l_foot_front_ft_sensor, l_foot_rear_ft_sensor, r_foot_front_ft_sensor, r_foot_rear_ft_sensor, l_leg_ft_sensor, r_leg_ft_sensor)</param>
+      <param name="temperature_sensors_list">(l_arm_ft, r_arm_ft, l_foot_front_ft, l_foot_rear_ft, r_foot_front_ft, r_foot_rear_ft, l_leg_ft, r_leg_ft)</param>
     </group>
 
     <group name="CartesianWrenches">

diff --git a/...erDevice/app/robots/iCubGenova02/blf-yarp-robot-logger-interfaces/ft-imu-mas-remapper.xml b/...erDevice/app/robots/iCubGenova02/blf-yarp-robot-logger-interfaces/ft-imu-mas-remapper.xml
@@ -24,10 +24,10 @@ BSD-3-Clause license. -->
            r_foot_ft_eul_3b14)
         </param>
         <param name="SixAxisForceTorqueSensorsNames">
-          (l_leg_ft_sensor
-           l_foot_ft_sensor
-           r_leg_ft_sensor
-           r_foot_ft_sensor)
+          (l_leg_ft
+           l_foot_ft
+           r_leg_ft
+           r_foot_ft)
         </param>
     <action phase="startup" level="5" type="attach">
             <paramlist name="networks">

diff --git a/devices/YarpRobotLoggerDevice/app/robots/iCubGenova02/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/iCubGenova02/yarp-robot-logger.xml
@@ -28,7 +28,7 @@ BSD-3-Clause license. -->
     </group>
 
     <group name="SixAxisForceTorqueSensors">
-      <param name="sixaxis_forcetorque_sensors_list">("l_leg_ft_sensor", "l_foot_ft_sensor", "r_leg_ft_sensor", "r_foot_ft_sensor")</param>
+      <param name="sixaxis_forcetorque_sensors_list">("l_leg_ft", "l_foot_ft", "r_leg_ft", "r_foot_ft")</param>
       <!--           <param name="sixaxis_forcetorque_sensors_list">("left_leg-eb6-j0_3-strain", "left_leg-eb7-j4_5-strain", "right_leg-eb8-j0_3-strain", "right_leg-eb9-j4_5-strain")</param>-->
     </group>
 

diff --git a/...erDevice/app/robots/iCubGenova04/blf-yarp-robot-logger-interfaces/ft-imu-mas-remapper.xml b/...erDevice/app/robots/iCubGenova04/blf-yarp-robot-logger-interfaces/ft-imu-mas-remapper.xml
@@ -24,10 +24,10 @@ BSD-3-Clause license. -->
            r_foot_ft_eul_3b14)
         </param>
         <param name="SixAxisForceTorqueSensorsNames">
-          (l_leg_ft_sensor
-           l_foot_ft_sensor
-           r_leg_ft_sensor
-           r_foot_ft_sensor)
+          (l_leg_ft
+           l_foot_ft
+           r_leg_ft
+           r_foot_ft)
         </param>
     <action phase="startup" level="5" type="attach">
             <paramlist name="networks">

diff --git a/devices/YarpRobotLoggerDevice/app/robots/iCubGenova04/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/iCubGenova04/yarp-robot-logger.xml
@@ -24,7 +24,7 @@ BSD-3-Clause license. -->
     </group>
 
     <group name="SixAxisForceTorqueSensors">
-      <param name="sixaxis_forcetorque_sensors_list">("l_leg_ft_sensor", "l_foot_ft_sensor", "r_leg_ft_sensor", "r_foot_ft_sensor")</param>
+      <param name="sixaxis_forcetorque_sensors_list">("l_leg_ft", "l_foot_ft", "r_leg_ft", "r_foot_ft")</param>
       <!--           <param name="sixaxis_forcetorque_sensors_list">("left_leg-eb6-j0_3-strain", "left_leg-eb7-j4_5-strain", "right_leg-eb8-j0_3-strain", "right_leg-eb9-j4_5-strain")</param>-->
     </group>
 

diff --git a/...botLoggerDevice/app/robots/iCubGenova09/blf-yarp-robot-logger-interfaces/mas-remapper.xml b/...botLoggerDevice/app/robots/iCubGenova09/blf-yarp-robot-logger-interfaces/mas-remapper.xml
@@ -18,10 +18,10 @@
     (rfeimu_eul, l_arm_ft_eul, r_arm_ft_eul, l_leg_ft_eul, l_foot_front_ft_eul, l_foot_rear_ft_eul, r_leg_ft_eul, r_foot_front_ft_eul, r_foot_rear_ft_eul)
   </param>
   <param name="SixAxisForceTorqueSensorsNames">
-    (l_arm_ft_sensor, r_arm_ft_sensor, l_leg_ft_sensor, l_foot_front_ft_sensor, l_foot_rear_ft_sensor, r_leg_ft_sensor, r_foot_front_ft_sensor, r_foot_rear_ft_sensor)
+    (l_arm_ft, r_arm_ft, l_leg_ft, l_foot_front_ft, l_foot_rear_ft, r_leg_ft, r_foot_front_ft, r_foot_rear_ft)
   </param>
   <param name="TemperatureSensorsNames">
-    (l_arm_ft_sensor, r_arm_ft_sensor, l_leg_ft_sensor, l_foot_front_ft_sensor, l_foot_rear_ft_sensor, r_leg_ft_sensor, r_foot_front_ft_sensor, r_foot_rear_ft_sensor)
+    (l_arm_ft, r_arm_ft, l_leg_ft, l_foot_front_ft, l_foot_rear_ft, r_leg_ft, r_foot_front_ft, r_foot_rear_ft)
   </param>
 
   <action phase="startup" level="5" type="attach">

diff --git a/devices/YarpRobotLoggerDevice/app/robots/iCubGenova09/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/iCubGenova09/yarp-robot-logger.xml
@@ -82,11 +82,11 @@ BSD-3-Clause license. -->
     </group>
 
     <group name="SixAxisForceTorqueSensors">
-      <param name="sixaxis_forcetorque_sensors_list">(l_arm_ft_sensor, r_arm_ft_sensor, l_leg_ft_sensor, l_foot_front_ft_sensor, l_foot_rear_ft_sensor, r_leg_ft_sensor, r_foot_front_ft_sensor, r_foot_rear_ft_sensor)</param>
+      <param name="sixaxis_forcetorque_sensors_list">(l_arm_ft, r_arm_ft, l_leg_ft, l_foot_front_ft, l_foot_rear_ft, r_leg_ft, r_foot_front_ft, r_foot_rear_ft)</param>
     </group>
 
     <group name="TemperatureSensors">
-      <param name="temperature_sensors_list">(l_arm_ft_sensor, r_arm_ft_sensor, l_leg_ft_sensor, l_foot_front_ft_sensor, l_foot_rear_ft_sensor, r_leg_ft_sensor, r_foot_front_ft_sensor, r_foot_rear_ft_sensor)</param>
+      <param name="temperature_sensors_list">(l_arm_ft, r_arm_ft, l_leg_ft, l_foot_front_ft, l_foot_rear_ft, r_leg_ft, r_foot_front_ft, r_foot_rear_ft)</param>
     </group>
 
     <group name="CartesianWrenches">

diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfState.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfState.h
@@ -82,7 +82,7 @@ class UkfState : public bfl::AdditiveStateModel
      * |`DYNAMICS_NAME`|          `friction_k2`         |   `vector<double>`   | Vector of double containing the coefficient k2 of the friction model of each element.          |    No     |
      * `DYNAMICS_NAME` is a placeholder for the name of the dynamics contained in the
      * `dynamics_list` list. `name` can contain only the following values ("ds", "tau_m", "tau_F",
-     * "*_ft_sensor", "*_ft_sensor_bias", "*_ft_acc_bias", "*_ftgyro_bias").
+     * "*_ft", "*_ft_bias", "*_ft_acc_bias", "*_ftgyro_bias").
      * @note The following `ini` file presents an example of the configuration that can be used to
      * build the UkfState.
      *
@@ -107,7 +107,7 @@ class UkfState : public bfl::AdditiveStateModel
      * friction_k2                     (1.767, 5.64, 0.27, 2.0, 3.0, 0.0)
      *
      * [RIGHT_LEG_FT]
-     * name                            "r_leg_ft_sensor"
+     * name                            "r_leg_ft"
      * elements                        ("fx", "fy", "fz", "mx", "my", "mz")
      * covariance                      (1e-3, 1e-3, 1e-3, 1e-4, 1e-4, 1e-4)
      * dynamic_model                   "ZeroVelocityStateDynamics"

diff --git a/src/Estimators/tests/RobotDynamicsEstimator/ConstantMeasurementModelTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/ConstantMeasurementModelTest.cpp
@@ -32,15 +32,15 @@ TEST_CASE("Constant Measurement Model")
     REQUIRE(variableHandler.addVariable("ds", sizeVariable));
     REQUIRE(variableHandler.addVariable("tau_m", sizeVariable));
     REQUIRE(variableHandler.addVariable("tau_F", sizeVariable));
-    REQUIRE(variableHandler.addVariable("r_leg_ft_sensor", sizeVariable));
-    REQUIRE(variableHandler.addVariable("r_leg_ft_sensor_bias", sizeVariable));
-    REQUIRE(variableHandler.addVariable("r_foot_front_ft_sensor", sizeVariable));
+    REQUIRE(variableHandler.addVariable("r_leg_ft", sizeVariable));
+    REQUIRE(variableHandler.addVariable("r_leg_ft_bias", sizeVariable));
+    REQUIRE(variableHandler.addVariable("r_foot_front_ft", sizeVariable));
 
     Eigen::VectorXd state;
     state.resize(sizeVariable * variableHandler.getNumberOfVariables());
     state.setZero();
 
-    const std::string name = "r_leg_ft_sensor";
+    const std::string name = "r_leg_ft";
     Eigen::VectorXd covariance(6);
     covariance << 1e-7, 1e-2, 5e0, 5e-3, 5e-1, 5e-10;
     const std::string model = "ConstantMeasurementModel";
@@ -65,13 +65,13 @@ TEST_CASE("Constant Measurement Model")
         bias(index) = GENERATE(take(1, random(-100, 100)));
     }
 
-    state.segment(variableHandler.getVariable("r_leg_ft_sensor_bias").offset, variableHandler.getVariable("r_leg_ft_sensor_bias").size) = bias;
+    state.segment(variableHandler.getVariable("r_leg_ft_bias").offset, variableHandler.getVariable("r_leg_ft_bias").size) = bias;
 
     ft.setState(state);
     REQUIRE(ft.update());
 
     Eigen::VectorXd updatedVariable = ft.getUpdatedVariable();
-    Eigen::VectorXd ftPre = state.segment(variableHandler.getVariable("r_leg_ft_sensor").offset, variableHandler.getVariable("r_leg_ft_sensor").size);
+    Eigen::VectorXd ftPre = state.segment(variableHandler.getVariable("r_leg_ft").offset, variableHandler.getVariable("r_leg_ft").size);
 
     REQUIRE((ftPre+bias).isApprox(updatedVariable));
 }
diff --git a/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp
@@ -128,7 +128,7 @@ TEST_CASE("Gyroscope Measurement Dynamics")
 
     auto accGroup = std::make_shared<StdImplementation>();
     std::vector<std::string> accNameList = {"r_leg_ft_gyro"};
-    std::vector<std::string> accFrameList = {"r_leg_ft_sensor"};
+    std::vector<std::string> accFrameList = {"r_leg_ft"};
     accGroup->setParameter("names", accNameList);
     accGroup->setParameter("frames", accFrameList);
     REQUIRE(modelParamHandler->setGroup("GYROSCOPE", accGroup));

diff --git a/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityStateDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityStateDynamicsTest.cpp
@@ -44,9 +44,9 @@ TEST_CASE("Zero Velocity Dynamics")
     REQUIRE(variableHandler.addVariable("ds", sizeVariable));
     REQUIRE(variableHandler.addVariable("tau_m", sizeVariable));
     REQUIRE(variableHandler.addVariable("tau_F", sizeVariable));
-    REQUIRE(variableHandler.addVariable("r_leg_ft_sensor", sizeVariable));
-    REQUIRE(variableHandler.addVariable("r_leg_ft_sensor_bias", sizeVariable));
-    REQUIRE(variableHandler.addVariable("r_foot_front_ft_sensor", sizeVariable));
+    REQUIRE(variableHandler.addVariable("r_leg_ft", sizeVariable));
+    REQUIRE(variableHandler.addVariable("r_leg_ft_bias", sizeVariable));
+    REQUIRE(variableHandler.addVariable("r_foot_front_ft", sizeVariable));
 
     ZeroVelocityStateDynamics tau_m;