Gazebo YARP plugin for iCub skin

[maxymczech]

Good day. This is an attempt at a pull request to solve issue #55. I apologize for dragging this out for so long. I will describe the idea behind the plugin, and will kindly ask for a review, I have a feeling this will not be accepted at a first attempt.

• This is a model plugin. Upon initialization it subscribes to ~/physics/contacts Gazebo topic and filters out contacts on skin parts, using regular expressions to check for a certain naming convention in collision bodies to determine if this was a contact with skin. I went this road instead of sensor plugin recommended in Gazebo tutorials for performance reasons. As the skin will have hundreds of collision elements, I found model plugin to perform orders of magnitude better than sensor plugin.
• The plugin forwards filtered contact information to corresponding YARP ports at 10 Hz frequency.

As you can see in the header file, I have hard coded body parts and port names configuration. This worked fine for my thesis project, but for the pull to be accepted I guess this has to be configurable via YARP ini files? But I am not sure what is the proper way to do this, can you please point me in a direction for documentation on this?

Other than that, I have studied the code for other YARP Gazebo plugins, but I am not sure if my coding style and conventions are up to your standard.

Once again, I apologize for dragging this issue out.

[CLAassistant]

All committers have signed the CLA.

[traversaro]

Thanks a lot @maxymczech ! I will check it in next days!

[matejhof]

Hi guys, what is the state of this please? @traversaro ?

[traversaro]

Hi @matejhof, sorry this was in the backlog, I will try to check it in the next weeks.

[traversaro]

Hi @maxymczech, thanks a lot for the PR and sorry for the late review!

This worked fine for my thesis project, but for the pull to be accepted I guess this has to be configurable via YARP ini files? But I am not sure what is the proper way to do this, can you please point me in a direction for documentation on this?

Yes, ideally we should do something like that. I guess the best introduction to YARP ini configuration file is https://www.yarp.it//git-master/yarp_config_files.html . Tipically then a .ini file is passed to the plugin via a yarpConfigurationFile child element of a plugin tag, see for example https://github.com/robotology/icub-models/blob/4c911ad591f9362d100d89cee1e282e7fe91384c/iCub_manual/robots/iCubGazeboV2_5_visuomanip/model.urdf#L2680 . To read that plugin, we have a dedicated function loadConfigModelPlugin, see for example

gazebo-yarp-plugins/plugins/linkattacher/src/linkattacher.cc

Line 48 in 40ef333

bool configuration_loaded = GazeboYarpPlugins::loadConfigModelPlugin(_model, _sdf, m_parameters);

for how it is used. Once you loaded the .ini file, you can read it using the functions described in https://www.yarp.it//git-master/yarp_config_files.html . Probably to start working on this you may want to first think how the information you need could be written in a .ini config file.

Once the configuration are loaded from .ini files, I think that the plugin may be merged, even if further improvements could be done, such as decomposing the "simulation part" in a "YARP DEVICE", and using existing YARP Device Network Wrapper Servers (see https://www.yarp.it//latest/nws_and_nwc_architecture.html). However, those further improvements could be future PRs.

However, the other question that I have is what kind of changes we need to do on the URDF/SDF of the iCub to actually use this plugin? I guess we need to have some kind of collision sensor for each skin patch?

Thanks again!

[traversaro]

Anyhow can be of interested of other people working on skin: @xEnVrE @lornat75 (feel free to tag more people, I may not know everyone involved).

[xEnVrE]

However, the other question that I have is what kind of changes we need to do on the URDF/SDF of the iCub to actually use this plugin? I guess we need to have some kind of collision sensor for each skin patch?

Hi @traversaro,

in this respect, do you think these changes might be inserted in the manually modified URDF of the robot, i.e. https://github.com/robotology/icub-models/blob/master/iCub_manual/robots/iCubGazeboV2_5_visuomanip/model.urdf? I saw that the author of the plugin is already using it, as per this video.

Or, it would be better to include these changes using the machinery provided by https://github.com/robotology/icub-models-generator?

Thanks

[traversaro]

However, the other question that I have is what kind of changes we need to do on the URDF/SDF of the iCub to actually use this plugin? I guess we need to have some kind of collision sensor for each skin patch?

Hi @traversaro,

in this respect, do you think these changes might be inserted in the manually modified URDF of the robot, i.e. https://github.com/robotology/icub-models/blob/master/iCub_manual/robots/iCubGazeboV2_5_visuomanip/model.urdf? I saw that the author of the plugin is already using it, as per this video.

To be perfectly honest, I would strongly prefer to have some way to automatically the necessary bits to any model. Note that it does not need to re-use what the existing pipeline is using, if it simplifies the implementation it can be anything (C++, Python, whatever) as long it can run automatically. If there is no other option we can add it manually, but adding it manually to the model is basically shifting effort from the initial implementation to the mantainance (see for example robotology/icub-models#124).

[maxymczech]

@traversaro Thank you for your comments regarding usage of ini files, I am going over the documentation now and will update the PR shortly.

@xEnVrE Yes, I have written a script to manually generate sensors and place them under respective body parts of the robot model. The proper way should be to integrate this process into icub-models-generator.

[matejhof]

What is the state here? Is the ball on @maxymczech side or @traversaro ? @2103simon is interested in this, so maybe he can also help here that we merge to robotology....

[traversaro]

What is the state here? Is the ball on @maxymczech side or @traversaro ? @2103simon is interested in this, so maybe he can also help here that we merge to robotology....

From what I intended it was on the @maxymczech side, but I may be biased. :)
If you want we can align on Teams or similar next week.

[2103simon]

I would be happy to join in. I will have a look at the code soon and try to implement the skin of the hand. I haven't looked at it yet, but from my knowledge the iCub model has changed, not sure how this effects this special project. I can update you as soon as I had a closer look.

[diogofbsilva]

However, the other question that I have is what kind of changes we need to do on the URDF/SDF of the iCub to actually use this plugin? I guess we need to have some kind of collision sensor for each skin patch?

Hi @traversaro,

in this respect, do you think these changes might be inserted in the manually modified URDF of the robot, i.e. https://github.com/robotology/icub-models/blob/master/iCub_manual/robots/iCubGazeboV2_5_visuomanip/model.urdf? I saw that the author of the plugin is already using it, as per this video.

Or, it would be better to include these changes using the machinery provided by https://github.com/robotology/icub-models-generator?

Thanks

Hello everyone, I am working with the tactile sensing of the robot iCub in ISR and I am trying to run some simulations of the tactile sensors, I see that the plugins are available but I was wondering if there is a modified version of the urdf of the robot that I could use also in order to be able to run some simulations as @xEnVrE asked.

If I did not explain myself well please let me know and if it exists any other way to be possible to do the simulations please let me know!

Thank you!

[plinioMoreno]

Hi all,

@diogofbsilva wants to use the plugin developed by @maxymczech, and we wonder if @maxymczech can send to @diogofbsilva the script he mentioned here, to be able to use his plugin in the iCub gazebo model.

[maxymczech]

@plinioMoreno Give me a couple hours, I need to refresh my memory, will send you the script and some instructions.

[maxymczech]

@plinioMoreno Ok, I didn't forget everything completely yet 😅 The code is available here, the repo seems to be public:

https://gitlab.fel.cvut.cz/body-schema/code-icub-gazebo-skin

Skin generation script is in skin-generation folder. It's a Node.js script, you can run it with command node make-skin.js > output.urdf. Some notes:

• It reads skin taxel coordinates from file specified in positionsPath variable, and robot model from modelFileName. I have copied the versions I had been working with locally to the same directory, you can try overwriting them with more current versions, or just specify the absolute path to files.

• The script works by attaching "skin" links to arms, legs and torso, and adding multiple collision geometries + visuals inside those links. The list of affected body parts and all required configuration is in config.json file. You can remove bodyParts selectively, or you can add head if you want, but you will have to figure out origin coordinates for the head skin link.

• The collisions are processed by gazebo_contactsensor_plugin, sources for which are in same repository. It is basically a bridge that reads collision from Gazebo and redirects them to relevant iCub YARP ports.

I hope this will be helpful.

[plinioMoreno]

Many thanks @maxymczech! @diogofbsilva will try and let you know how it goes.

[traversaro]

Thanks a lot everyone! If there is anyone interested in getting this plugin merged in the repo, feel free to write to me, so if useful we can also do a meeting to understand what is necessary to be done, thanks!

[diogofbsilva55]

Hi! Over the last months, among other developments of my master thesis regarding Real-Time Control of Dexterous Robot Hands using Tactile and Force Sensors, using the robot iCub, I have been using a grasping simulation which is available in https://github.com/robotology/icub-gazebo-grasping-sandbox. I had the simulation working well however did not have the plugins for tactile sensors (which are very important to my thesis). So, I decided to use the plugin developped by @maxymczech. However, I faced some problems primarly regarding the pysics engine. In the simulation world I am working on it is used DART. However, when using the plugin and the model (urdf) of the iCub updated with the tactile sensors the following error is present:

ERRO: gzserver: ./dart/dynamics/detail/GenericJoint.hpp:1778: void dart::dynamics::GenericJoint::addChildArtInertiaToDynamic(Eigen::Matrix6d&, const Matrix6d&) [with ConfigSpaceT = dart::math::RealVectorSpace<1>; Eigen::Matrix6d = Eigen::Matrix<double, 6, 6>]: Assertion `!math::isNan(PI)' failed.
Aborted

It is important to note as well that when changing the physics engine of the simulator from DART to ODE, this error did not appear however the iCub was not able to successfully grasp the objects of the world (objects would slip).

So, i would like to get recommendation in what modification I can and should be doing, mainly from @maxymczech (of course).

Thank you!
Diogo Silva

[maxymczech]

@diogofbsilva55 I will be able to take a look at your request this weekend. When working on my thesis I did test different physical engines that come with Gazebo and ODE was the only one that worked reliably for me.

[diogofbsilva55]

@maxymczech thank you so much for your fast response and for the help provided!
I look forward to your response over the next days!

Thank you!

[pattacini]

Hi, @diogofbsilva

Happy to know that you found the sandbox useful for your research!
Please, consider contributing back by filling out the table at https://robotology.github.io/icub-gazebo-grasping-sandbox/building-on-sandbox.html.

Also, consider the disclaimer at https://github.com/robotology/icub-gazebo-grasping-sandbox?tab=readme-ov-file#-cite-the-sandbox.

[diogofbsilva]

Hello!

@maxymczech did you by any chance had the opportunity to investigate my request?

I would like also to ask something else: When reading your READme file on gitlab: https://gitlab.fel.cvut.cz/body-schema/code-icub-gazebo-skin. I saw your launch and manual control section on that file and I did exactly the same, for only one exception as I launch with gzserver and added "-e ode" or else the iCub kinda appears dead (arms down and head down). The rest of the instructions I replicated using a model of the iCub generated (like you explained above) and I moved both hands of the robot to collide (palm and fingers). However, when reading the ports of both hands: yarp read ... /icubSim/skin/left_hand_comp and the same for the right hand of course, I only obtain zeros.

Is this a problem still related to the physics engines or am I missing something??

Thank you so much for your help!
Diogo Silva

[maxymczech]

@diogofbsilva sorry, still looking into the engine issue.

Regarding your other question, YARP ports showing 0 does not sound like something connected to phys engine. I tried to write the readme to be as concise and easy to replicate as possible, in fact I tested it step by step on a "blank" system to make sure of that.

[diogofbsilva]

Maybe is something that I changed in the model that is impacting! I will try to test it in a blank system as you said to test it more precisely!

Thank you!

[maxymczech]

No problem, I will let you know once I've tested the DART engine compilafion error.

[diogofbsilva]

Hey @maxymczech ! Got any updates?

[diogofbsilva]

Hello @maxymczech !
Did you by any chance had the opportunity to look into the DART engine compilation error?