Bipedal-locomotion-framework: Discuss some possible modifications of the Yarp implementation of the SensorBridge

[GiulioRomualdi]

This week I discussed with @prashanthr05 possible modification of the YARP implementation of the SensorBridge. I noticed that they may simplify the usage of the class while implementing an application that required the SensorBridge.

1. If for any chance the user does not set check_for_nan the result is undetermined.
   https://github.com/dic-iit/bipedal-locomotion-framework/blob/a03502a65ec8ce9fd62181fccb13653818e43bf3/src/RobotInterface/YarpImplementation/src/YarpSensorBridge.cpp#L33-L35
   Furthermore, the parameter handler can handle booleans. so in theory the code may be changed as

   if (!ptr->getParameter("check_for_nan", m_pimpl->checkForNAN))
   {
       return false;
   }
   

   Then the configuration file has to contain true / false (0, 1 don't work). Regarding this last sentence, I'm pretty sure on the ini files but I don't know for XML files.

2. The joints_list vector is required as a parameter by the configuration of the control board remapper interfaces in the SensorBridge
   https://github.com/dic-iit/bipedal-locomotion-framework/blob/c055c48edb86aa9f23fb7cc572a4ea005f1c626b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridgeImpl.h#L195-L211
   This is required because the sensor bridge can handle the joints ordered in a different manner respect to the polydriver. I like this feature but it would be nice if joints_list becomes an optional parameter. If passed the new order is used otherwise the polydriver joints order is used.

3. The SensorBridge does not have the ownership of the polydrivers. This may cause problems in case the polydrivers are deallocated (by mistake) in the consumer library/application. In the IRobotControl class, we solved this problem by saving the shared ptr to the polydriver
   https://github.com/dic-iit/bipedal-locomotion-framework/blob/c055c48edb86aa9f23fb7cc572a4ea005f1c626b/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp#L41
   https://github.com/dic-iit/bipedal-locomotion-framework/blob/c055c48edb86aa9f23fb7cc572a4ea005f1c626b/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp#L287
   Unfortunately, in the SensorBridge we used yarp::dev::PolyDriverList that contains only the raw pointer to the polydrivers.
   https://github.com/dic-iit/bipedal-locomotion-framework/blob/c055c48edb86aa9f23fb7cc572a4ea005f1c626b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridge.h#L104
   I would suggest implementing our custom PolyDriverList where the raw pointers are converted to shared_ptr. Another possible solution is to propose the modification in yarp but I don't know if it's a good idea or not (we may break some already existing code)

4. It would be nice to change the getJointPositions function signature from
   https://github.com/dic-iit/bipedal-locomotion-framework/blob/c055c48edb86aa9f23fb7cc572a4ea005f1c626b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridge.h#L229-L230
   into
   cpp Eigen::Ref<const Eigen::VectorXd> getJointPositions(double* receiveTimeInSeconds);
   It simplifies a lot the usage in the consumer library. For instance you can do
   cpp myMagicFunction(m_sensorBridge.getJointPositions());
   instead of
   cpp Eigen::VectorXd joints; m_sensorBridge.getJointPositions(joints); myMagicFunction(joints);
   The main concern here is how to handle the default implementation in the SensorBridge class.

@S-Dafarra @prashanthr05 If you agree on some of these modifications I can already propose some PR.

[prashanthr05]

I agree with all the pointers.

Regarding pointer 1, I chose to handle them as integers from xml configuration file because they were not recognised as booleans. This was also the case with the legacy YarpUtilities/Helper methods that we were previously using.

Aside, in addition to all the pointers, there was one feature I did not implement in the YarpSensorBridge while porting from the legacy RobotSensorBridge which basically runs a dry reading of sensors at the end of setDriversList phase once all the drivers are attached. This dry read polls for sensor measurements upto a timeout period (similar to the recent fix in IRobotControl) helping to identify early failures at the initialisation phase itself, in case there are no measurements available at the lower level.
Should we consider to reimplement it here as well?

[S-Dafarra]

3. Unfortunately, in the SensorBridge we used yarp::dev::PolyDriverList that contains only the raw pointer to the polydrivers.
https://github.com/dic-iit/bipedal-locomotion-framework/blob/c055c48edb86aa9f23fb7cc572a4ea005f1c626b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridge.h#L104

I would suggest implementing our custom `PolyDriverList` where the `raw pointers` are converted to `shared_ptr`. Another possible solution is to propose the modification in yarp but I don't know if it's a good idea or not (we may break some already existing code)

On this part we need to be careful. If the raw pointer was not created shared, then it needs to be clear who has the ownership. In fact, in principle, multiple shared_ptr could be created from the same raw pointer, but the use_count of each one would remain 1 since they do not know about each other. Hence, the first that gets deleted, delete the content for everyone. If we need to pass these pointers around, without leaving the user to deallocate them, it would be better to create a specific class meant to deal with these pointers. Then, we could pass a shared_ptr to this class around.

4. It would be nice to change the getJointPositions function signature from
https://github.com/dic-iit/bipedal-locomotion-framework/blob/c055c48edb86aa9f23fb7cc572a4ea005f1c626b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridge.h#L229-L230

into
```c++
Eigen::Ref<const Eigen::VectorXd>  getJointPositions(double* receiveTimeInSeconds);
```


It simplifies a lot the usage in the consumer library. For instance you can do
```c++
 myMagicFunction(m_sensorBridge.getJointPositions());
```


instead of
```c++
Eigen::VectorXd joints;
m_sensorBridge.getJointPositions(joints);
myMagicFunction(joints);
```


The main concern here is how to handle the default implementation in the `SensorBridge` class.

I like the idea. On the other hand, the interfaces so far, including the original Yarp one, assume that no memory is own. If we want to change this, then I would suggest creating a struct containing all the possible data that can be retrieved by the bridge. Each one of the get* would return some reference to the underlying structure. The nice point of this is that it would be possible to write an advance method filling up this struct. The problem is that this struct should be implementation dependent. It would require a lot of copies if we define it with Eigen objects. Maybe it would be possible to do that with the Vector::Ref, but I am not sure.

[traversaro]

I would suggest implementing our custom PolyDriverList where the raw pointers are converted to shared_ptr. Another possible solution is to propose the modification in yarp but I don't know if it's a good idea or not (we may break some already existing code)

If we need to pass these pointers around, without leaving the user to deallocate them, it would be better to create a specific class meant to deal with these pointers. Then, we could pass a shared_ptr to this class around.

Sincerely, I think that this may impose sever constraints on the use of the resulting classes that adopt this API. If setDriversList takes a vector of shared ptr, then I can't use it inside a YARP device in which the devices are passed via the attachAll method.

[GiulioRomualdi]

I agree with all the pointers.

Regarding pointer 1, I chose to handle them as integers from xml configuration file because they were not recognised as booleans. This was also the case with the legacy YarpUtilities/Helper methods that we were previously using.

Hi @prashanthr05 regarding this point I tried to write a test where an xml file is loaded using the ResourceFinder however I was not able to have a working application. Could you point me a link with an example of usage of the resource finder with xml?

<?xml version="1.0" encoding="UTF-8" ?>

<param name="answer_to_the_ultimate_question_of_life">42</param>
<param name="pi">3.14</param>
<param name="Jhon">Smith</param>
<param name="Fibonacci Numbers">(1, 1, 2, 3, 5, 8, 13, 21)</param>

<group name="CARTOONS">
  <param name="Donald's nephews">(Huey, Dewey, Louie)</param>
  <param name="Jhon">Smith</param>
  <param name="Fibonacci Numbers">(1, 1, 2, 3, 5, 8, 13, 21)</param>
</group>

But when I try to print the content of the ResourceFinder I got this:

("</group>") ("<?xml" "version=\"1.0\"" "encoding=\"UTF-8\"" "?>") ("<group" "name=\"CARTOONS\">") ("<param" "name=\"Fibonacci Numbers\">(1, 1, 2, 3, 5, 8, 13, 21)</param>") (from "/home/gromualdi/robot-code/bipedal-locomotion-framework/src/ParametersHandler/tests/config.xml")

There seems that the file is not correctly loaded.

[traversaro]

Could you point me a link with an example of usage of the resource finder with xml?

Unfortunately the parsing from XML files to property is tightly coupled with the yarprobotinterface/libYARP_robotinterface logic, and it can't be used independently from that. You can find example of its use in https://github.com/robotology/yarp/blob/master/tests/libYARP_robotinterface/RobotinterfaceTest.cpp .

[prashanthr05]

Could you point me a link with an example of usage of the resource finder with xml?

Unfortunately the parsing from XML files to property is tightly coupled with the yarprobotinterface/libYARP_robotinterface logic, and it can't be used independently from that. You can find example of its use in https://github.com/robotology/yarp/blob/master/tests/libYARP_robotinterface/RobotinterfaceTest.cpp .

I run the YARP device through the yarprobotinterface application. You could try running the Yarp Test Sensor Bridge device that I had created. Please see https://github.com/prashanthr05/bipedal-locomotion-framework/commit/f310861b9d6931ae3fdf11f354c436b508d907d9.

To run the device, after installation

• run yarpserver
• run Gazebo, load the robot
• run wholeBodyDynamics
• run the testdevice using yarprobotinterface --config launch-test-sensor-bridge.xml

[GiulioRomualdi]

@traversaro and @S-Dafarra

Sincerely, I think that this may impose sever constraints on the use of the resulting classes that adopt this API. If setDriversList takes a vector of shared ptr, then I can't use it inside a YARP device in which the devices are passed via the attachAll method.

What I would like to have is instead of passing yarp::dev::PolyDriverList to the following function:
https://github.com/dic-iit/bipedal-locomotion-framework/blob/c055c48edb86aa9f23fb7cc572a4ea005f1c626b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridge.h#L104

is to pass

bool setDriversList(const std::vector<std::pair<std::string, std::shared_ptr<yarp:::dev::PolyDriver>>& deviceDriversList); 

so the SensorBridge can save the vector and own the memory of the polydrivers. Then if inside the SensorBridge we need to call the attachAll we can implement a function to convert the std::vector<std::pair<std::string, std::shared_ptr<yarp:::dev::PolyDriver>> into a yarp:::dev::PolyDriverList.

Note: instead of having std::vector<std::pair<std::string, std::shared_ptr<yarp:::dev::PolyDriver>> we can create a custom class

[prashanthr05]

@traversaro and @S-Dafarra

Sincerely, I think that this may impose sever constraints on the use of the resulting classes that adopt this API. If setDriversList takes a vector of shared ptr, then I can't use it inside a YARP device in which the devices are passed via the attachAll method.

What I would like to have is instead of passing yarp::dev::PolyDriverList to the following function:
https://github.com/dic-iit/bipedal-locomotion-framework/blob/c055c48edb86aa9f23fb7cc572a4ea005f1c626b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridge.h#L104

is to pass

bool setDriversList(const std::vector<std::pair<std::string, std::shared_ptr<yarp:::dev::PolyDriver>>& deviceDriversList); 

so the SensorBridge can save the vector and own the memory of the polydrivers. Then if inside the SensorBridge we need to call the attachAll we can implement a function to convert the std::vector<std::pair<std::string, std::shared_ptr<yarp:::dev::PolyDriver>> into a yarp:::dev::PolyDriverList.

Note: instead of having std::vector<std::pair<std::string, std::shared_ptr<yarp:::dev::PolyDriver>> we can create a custom class

Please note that YARP device implementation becomes a wrapper using the YarpSensorBridge class.

In this case, the YarpSensorBridge setDriverList() is called within the attachAll() of the YARP device implementation.

I am thinking, If we are creating a custom class, we might just need to create a helper function that moves back and forth between Yarp PolyDriverList and the custom class implementation. (This was what Giulio also wrote above!)

[traversaro]

What I would like to have is instead of passing yarp::dev::PolyDriverList to the following function:

https://github.com/dic-iit/bipedal-locomotion-framework/blob/c055c48edb86aa9f23fb7cc572a4ea005f1c626b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridge.h#L104

The main problem if is that is the only way of passing devices to the SensorBridge, then you can't pass to it the devices created by the libYARP_robotinterface, because the devices created by the yarp::robotinterface::experimental::Robot are owned exclusively by the yarp::robotinterface::experimental::Robot, and as an external user of this API you can't change how yarp::robotinterface::experimental::Robot handle ownership of YARP devices.

Indeed the attachAll is not the problem, but rather anyone that is creating YARP devices without creating them explicitly from the beginning as a shared_ptr, being it libYARP_robotinterface or the gazebo-yarp-plugins.

[traversaro]

Related PRs/discussion on yarp::dev::PolyDriverList and ownership of YARP devices:

• https://github.com/robotology/yarp/pull/2319
• https://github.com/robotology/yarp/pull/2304

[GiulioRomualdi]

Regarding the Point 1 of https://github.com/dic-iit/bipedal-locomotion-framework/issues/121#issue-708357726

Could you point me a link with an example of usage of the resource finder with xml?

Unfortunately the parsing from XML files to property is tightly coupled with the yarprobotinterface/libYARP_robotinterface logic, and it can't be used independently from that. You can find example of its use in https://github.com/robotology/yarp/blob/master/tests/libYARP_robotinterface/RobotinterfaceTest.cpp .

I run the YARP device through the yarprobotinterface application. You could try running the Yarp Test Sensor Bridge device that I had created. Please see prashanthr05@f310861.

To run the device, after installation

• run yarpserver
• run Gazebo, load the robot
• run wholeBodyDynamics
• run the testdevice using yarprobotinterface --config launch-test-sensor-bridge.xml

I followed the instructions described by @prashanthr05 and I was able with some small modification to run the device where the int are substituted with the booleans.

The code can be found:

• https://github.com/GiulioRomualdi/bipedal-locomotion-controllers/commit/0671570bcad2338cf1e451780a286cd61083e9ba for the modification in the code
• https://github.com/GiulioRomualdi/bipedal-locomotion-controllers/commit/b8e21bc20ff84c571cd6f46d60f41a95abd4df51 for the modification in the configuration file.

I will open a PR in the meantime @prashanthr05 will test the code on his personal laptop

[GiulioRomualdi]

@traversaro

What I would like to have is instead of passing yarp::dev::PolyDriverList to the following function:
https://github.com/dic-iit/bipedal-locomotion-framework/blob/c055c48edb86aa9f23fb7cc572a4ea005f1c626b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridge.h#L104

The main problem if is that is the only way of passing devices to the SensorBridge, then you can't pass to it the devices created by the libYARP_robotinterface, because the devices created by the yarp::robotinterface::experimental::Robot are owned exclusively by the yarp::robotinterface::experimental::Robot, and as an external user of this API you can't change how yarp::robotinterface::experimental::Robot handle ownership of YARP devices.

Indeed the attachAll is not the problem, but rather anyone that is creating YARP devices without creating them explicitly from the beginning as a shared_ptr, being it libYARP_robotinterface or the gazebo-yarp-plugins.

Got it. So it will remain as it is.

[prashanthr05]

Regarding the Point 1 of #121 (comment)

Could you point me a link with an example of usage of the resource finder with xml?

Unfortunately the parsing from XML files to property is tightly coupled with the yarprobotinterface/libYARP_robotinterface logic, and it can't be used independently from that. You can find example of its use in https://github.com/robotology/yarp/blob/master/tests/libYARP_robotinterface/RobotinterfaceTest.cpp .

I run the YARP device through the yarprobotinterface application. You could try running the Yarp Test Sensor Bridge device that I had created. Please see prashanthr05@f310861.
To run the device, after installation

• run yarpserver
• run Gazebo, load the robot
• run wholeBodyDynamics
• run the testdevice using yarprobotinterface --config launch-test-sensor-bridge.xml

I followed the instructions described by @prashanthr05 and I was able with some small modification to run the device where the int are substituted with the booleans.

The code can be found:

• GiulioRomualdi@0671570 for the modification in the code
• GiulioRomualdi@b8e21bc for the modification in the configuration file.

I will open a PR in the meantime @prashanthr05 will test the code on his personal laptop

Tested with modifications introduced by @GiulioRomualdi in https://github.com/GiulioRomualdi/bipedal-locomotion-controllers/commit/0671570bcad2338cf1e451780a286cd61083e9ba and https://github.com/GiulioRomualdi/bipedal-locomotion-controllers/commit/b8e21bc20ff84c571cd6f46d60f41a95abd4df51
It happens to work in expected behavior.

Now, I am not sure why I was getting previously strange behavior while dealing with bools.

Anyways, thanks for the fix @GiulioRomualdi !!

[prashanthr05]

Should we add the TestSensorBridgeDevice to BLF master?

[GiulioRomualdi]

Should we add the TestSensorBridge device to BLF master?

you can open another PR for that one

[GiulioRomualdi]

I'm going to close this issue because:

• Point 1 of https://github.com/dic-iit/bipedal-locomotion-framework/issues/121#issue-708357726 has been addressed in #122.
• Point 2 of https://github.com/dic-iit/bipedal-locomotion-framework/issues/121#issue-708357726 fixed in #123
• Point 3 of https://github.com/dic-iit/bipedal-locomotion-framework/issues/121#issue-708357726 as @traversaro and @prashanthr05 cannot be handled in a simple way so for the time being, we can drop it.
• Point 4 of https://github.com/dic-iit/bipedal-locomotion-framework/issues/121#issue-708357726 I move the discussion in https://github.com/dic-iit/bipedal-locomotion-framework/issues/124

[prashanthr05]

In this https://github.com/dic-iit/bipedal-locomotion-framework/issues/121#issuecomment-698519020, I wrote

Aside, in addition to all the pointers, there was one feature I did not implement in the YarpSensorBridge while porting from the legacy RobotSensorBridge which basically runs a dry reading of sensors at the end of setDriversList phase once all the drivers are attached. This dry read polls for sensor measurements upto a timeout period (similar to the recent fix in IRobotControl) helping to identify early failures at the initialisation phase itself, in case there are no measurements available at the lower level.
Should we consider to reimplement it here as well?

I copied this to a separate issue for relevant discussion. https://github.com/dic-iit/bipedal-locomotion-framework/issues/125