Fix FT sensors name after some tests

[martinaxgloria]

Today I tried to import both iCubV2.5 and iCubV3 models in gazebo for the first time after updating icub-models to v2.1.0 release (hence after #239) and I obtained the same errors for both, resulting in gazebo crashing:

[ERROR] |yarp.device.multipleanalogsensorsremapper| Impossible to find sensor name l_leg_ft_sensor , exiting.
[ERROR] |yarp.device.multipleanalogsensorsremapper|     Names of available sensors are:
[ERROR] Device left_leg-FT_remapper cannot execute attach
[ERROR] Cannot run attach action on device left_leg-FT_remapper
[INFO] Executing attach action, level 5 on device right_leg-FT_remapper with parameters [("networks" = "(right_leg_ft_sensor right_foot_ft_sensor)"), ("right_leg_ft_sensor" = "icub_right_leg_ft"), ("right_foot_ft_sensor" = "icub_right_foot_ft")]
[ERROR] |yarp.device.multipleanalogsensorsremapper| Impossible to find sensor name r_leg_ft_sensor , exiting.
[ERROR] |yarp.device.multipleanalogsensorsremapper|     Names of available sensors are:
[ERROR] Device right_leg-FT_remapper cannot execute attach
[ERROR] Cannot run attach action on device right_leg-FT_remapper

Inspecting the changes introduced after the refactoring of FT sensors, I found out that sensorName does not contain the suffix _sensor for both the left and right legs and the FT does not stream temperature data.
After the changes introduced with this PR, I managed to make the yarprobotinterface run.

cc @Nicogene @traversaro @pattacini

[traversaro]

Thanks a lot for catching this error! Unfortunatly, I am afraid the current proposed fix may be problematic.

TL;DR

The sensor name should be the same between simulated and real robots, and on the real robots the name of the sensor are *_leg_ft_sensor, *_foot_rear_ft_sensor, and *_foot_front_ft_sensor, see for example https://github.com/robotology/robots-configuration/blob/d106aacf66104e14fc93a14a3e17e0311fdd6b12/iCubGenova09/wrappers/FT/right_leg-FT_remapper.xml#L9 . So if those names are not working we should change something else to make sure that they can work, not changing where they are used to use other wrong names.

Long explanation:

Why the name of the sensor must be the same between simulated and real robots? The main reason is that the sensor name is used in downstream code to select the sensor you want to read, for example:

// Snippet of code to read just the two sensor on the left foot of iCub
// Note: this would be drastically simplified if using the `multipleanalogsensorsclientremapper` device proposed in https://github.com/robotology/yarp/pull/2881 was 

// Create client
std::string robotPortPrefix = "icub"; // or icubSim for simulated robots, this is tipically passed via an option
std::string localPortPrefix = "localCode"
yarp::os::Property client_options;
client_options.put("device","multipleanalogsensorsclient");
client_options.put("remote","/" + robotPortPrefix + "/left_leg/FT");
client_options.put("local", "/" + localCode + "/" + robotPortPrefix + "/left_leg/FT");
yarp::os::PolyDriver clientDevice;
bool ok = clientDevice.open(client_options);
// Handle if ok is not true <..>

// Create remapper
yarp::os::Property remapper_options;
remapper_options.put("device","multipleanalogsensorsremapper");
yarp::os::Bottle sixAxisForceTorqueSensorsNames;
yarp::os::Bottle& sixAxisForceTorqueSensorsList = sixAxisForceTorqueSensorsNames.addList();
sixAxisForceTorqueSensorsList.addString("l_foot_rear_ft_sensor");
sixAxisForceTorqueSensorsList.addString("l_foot_front_ft_sensor");
remapper_options.put("SixAxisForceTorqueSensorsNames",sixAxisForceTorqueSensorsNames.get(0));
yarp::os::PolyDriver remapperDevice;
ok = remapperDevice.open(remapper_options);
// Handle if ok is not true <..>

// Attach remapper to the client
yarp::dev::IMultipleWrapper* imultiwrap=nullptr;
ok = remapperDevice.view(imultiwrap);
// Handle if ok is not true <..>

yarp::dev::PolyDriverList driverList;
driverList.push(&clientDevice, "left_legs_fts");
ok = imultiwrap->attachAll(driverList);
// Handle if ok is not true <..>

// At this point, you can read FT sensors
yarp::dev::ISixAxisForceTorqueSensors* isixaxis=nullptr;
ok = remapperDevice.view(isixaxis);
// Handle if ok is not true <..>

yarp::sig::Vector measure(6).
for(int ftIdx=0; ftIdx < isixaxis->getNrOfSixAxisForceTorqueSensors(); ftIdx++)
{
    std::string sensorName;
    isixaxis->getSixAxisForceTorqueSensorName(ftIdx, sensorName);
    isixaxis->getSixAxisForceTorqueSensorMeasure(ftIdx, measure);
    std::string << "The measure of FT sensor " << sensorName << " is " << measure.toString() << std::endl;
}

In this snippet of code, what identifies the sensors you want to read are the identifiers specified in:

sixAxisForceTorqueSensorsList.addString("l_foot_rear_ft_sensor");
sixAxisForceTorqueSensorsList.addString("l_foot_front_ft_sensor");

if this identifiers are different between real robot and simulated robot, it is impossible (or quite complex) to write code that works with minimal modifications in both the real robot and the simulated robot.

[traversaro]

This may be also interesting for @mfussi66 that is working on documenting how to access iCub's sensors.

[traversaro]

To be more clear, what I think we should fix is to align the sensor name between real and simulated robot for example in https://github.com/robotology/icub-models/blob/b209ab84411c6a64e297a3bc98146a2f576d7984/iCub/robots/iCubGazeboV3/model.urdf#L1670 . At the moment, I think we are not specifying the sensorName in the simmechanics-to-urdf configuration, and so the sensor name is defaulting to be the joint one, see https://github.com/robotology/simmechanics-to-urdf#forcetorque-sensors-parameters-keys-of-elements-of-forcetorquesensors for the documenation on this.

Another option is to align the real robot sensor name to the model sensor name. The important thing is that there is a match between the two names.

[Nicogene]

I agree w/ @traversaro analysis, part of the work of the refactoring was for aligning simulated and real robots configurations, unfortunately for the models we are forced to put as sensorName the name w/o the _sensor suffix, because when exporting the corresponding frame, using the _sensor suffix will clash w/ the (fake) joint name representing the ft sensor.

For this reason also on the real robot we specified the frame name w/o the suffix:

• https://github.com/robotology/robots-configuration/blob/d106aacf66104e14fc93a14a3e17e0311fdd6b12/iCubGenova11/hardware/FT/left_leg-eb6-j0_3-strain.xml#L33-L34

From my understanding for the urdf models (both of iCub and ergoCub) we have to use the name w/o the suffix _sensor, otherwise it will be impossible to export the corresponding frame.
For example ergoCub models right now uses the same names of the real robot BUT this is possible because the exportFrameInURDF flag is not set, then is false by default. If we enable it we fall in this very same problem.

• https://github.com/icub-tech-iit/ergocub-software/blob/9d56991cb4c5d24b935ddd64a34de2a2d8188947/urdf/simmechanics/data/ergocub/ERGOCUB_all_options.yaml#L708-L716

For this reason, probably we should:

• merge this PR
• make the same changes to ergocub-software
• change the names in the xml of the real robots for not using the suffix _sensor.

This could be annoying for the users because probably would have to change their code, I don't know if you have a better solution.

[traversaro]

For a long time I have not be a big fan of using a different sensor name and frame name, but if this is what we are doing on the real robot, can't we do exactly the same on the simulated robot? (I found the answer myself: we can do that, but we need to modify gazebo-yarp-plugins: https://github.com/robotology/gazebo-yarp-plugins/blob/725ebe4687fced370766b0024b7929fdceebf2b6/plugins/forcetorque/src/ForceTorqueDriver.cpp#L68 ).

using the _sensor suffix will clash w/ the (fake) joint name representing the ft sensor.
From my understanding for the urdf models (both of iCub and ergoCub) we have to use the name w/o the suffix _sensor, otherwise it will be impossible to export the corresponding frame.

I think we already discussed this but I can't recall, why we can't have both a sensor and a joint called l_arm_ft_sensor ? I think there is nothing in the URDF+SDF specification that prevents that.

[traversaro]

Apparently the sensor names were changed in #228, but I do not find any related discussion, probably I did not looked carefully to the changes.

[Nicogene]

I think we already discussed this but I can't recall, why we can't have both a sensor and a joint called l_arm_ft_sensor ? I think there is nothing in the URDF+SDF specification that prevents that.

I recall that I had issues in the past when I tried to visualized the ft on rviz, I don't recall if it was just a problem for computing the tf or the model was not loaded in gazebo for this clash, but this test can be done quickly.

This problem was the reason why I had to fix the frame exportation in the URDF in simmechanics_to_urdf:

• Add frameName and frame parameters in FT sensor simmechanics-to-urdf#45
• Export ft sensor frames simmechanics-to-urdf#53

[traversaro]

I think we already discussed this but I can't recall, why we can't have both a sensor and a joint called l_arm_ft_sensor ? I think there is nothing in the URDF+SDF specification that prevents that.

I recall that I had issues in the past when I tried to visualized the ft on rviz, I don't recall if it was just a problem for computing the tf or the model was not loaded in gazebo for this clash, but this test can be done quickly.

This problem was the reason why I had to fix the frame exportation in the URDF in simmechanics_to_urdf

If it does not take a long time, I would try to understand why we can't just change sensorName to l_arm_ft_sensor, as that would be by far the easiest solution if it works.

[traversaro]

I think we already discussed this but I can't recall, why we can't have both a sensor and a joint called l_arm_ft_sensor ? I think there is nothing in the URDF+SDF specification that prevents that.

I recall that I had issues in the past when I tried to visualized the ft on rviz, I don't recall if it was just a problem for computing the tf or the model was not loaded in gazebo for this clash, but this test can be done quickly.
This problem was the reason why I had to fix the frame exportation in the URDF in simmechanics_to_urdf

If it does not take a long time, I would try to understand why we can't just change sensorName to l_arm_ft_sensor, as that would be by far the easiest solution if it works.

Looking into internal discussion, I think I remember now. The problem is not in the URDF spec by itself, but rather by the Gazebo/SDF extension to URDF. Specifically, at the moment it is perfectly fine in URDF itself to have:

<link name="link1" />
<joint name="ft_sensor" type="fixed">
  <parent link="link1"/>
  <child link="link2"/>
</joint>
<link name="ft_sensor" />

However, when you write a Gazebo extension you extend the content of a link or joint with Gazebo-specific information with a tag called <gazebo> formed as in the following:

<gazebo reference="ft_sensor">
  <mu1>0.2</mu1>
  <mu2>0.2</mu2>
  <material>Gazebo/Black</material>
</gazebo>

the attribute reference of the gazebo element can refer either to a joint or a link. If a joint and a link has the same name, it is possible that the parser tries to insert in a joint an extension that is meant for a link, or viceversa. So indeed, having a joint and a link in URDF that share the name is not a great idea, so the best solution is to get rid of this difference.

[traversaro]

change the names in the xml of the real robots for not using the suffix _sensor.

If that is the best solution, let's go for that, even if I prefer to avoid the change of sensor name of FT from l_arm_ft_sensor to l_arm_ft as we used l_arm_ft_sensor for a long time in wholebodydynamics (see https://github.com/search?q=org%253Arobotology+l_arm_ft_sensor and https://github.com/search?q=org%3Aami-iit+l_arm_ft_sensor&type=code&p=2).

Just to recap the different options, we have:

Option	Pros	Cons
O1: Change sensorName from l_arm_ft_sensor to l_arm_ft on real robots, and keep l_arm_ft as frameName	sensorName and frameName match again	Requires changes in icub-model-generator, ergocub-software, robots-configuration. If the changes are not done together, wholebodydynamics from whole-body-estimators stop working.
02: Change sensorName in simulated robots back from from l_arm_ft to l_arm_ft_sensor, and keep l_arm_ft as frameName	Does not change established sensorName	Requires changes in icub-model-generator and in gazebo-yarp-plugins to ensure we can expose the frameName, sensorName and frameName will remain different. If the changes are not done together, ROS2 visualization of FT sensors stop working.

[traversaro]

Anyhow, I agree that this PR just discovered a problem that was already there, so I do not think that there is anything blocking it from being merged. @martinaxgloria can you just update the changelog to describe this change? Thanks!

[traversaro]

FT does not stream temperature data.

For this I opened robotology/gazebo-yarp-plugins#654 .

[traversaro]

@martinaxgloria can you just update the changelog to describe this change? Thanks!

As discussed in chat, this repo does not have a CHANGELOG. Merging.