Understand why the colors are not correctly parsed from urdf

[GiulioRomualdi]

When I try to load the talos robot stored in example-robot-data the robot link colors are not set

The same is valid for the meshcatvisualizer

However, the urdf file contains the colors.

On the other hand using gepetto-visualizer the colors are correctly loaded

I try to debug the problem and I noticed that iDynTree XMLParser is able to find the <color> attribute

[INFO] XMLParser :: parserCallbackStartDocument : Start document
[INFO] XMLParser :: parserCallbackStartTag : Start of tag <robot>
[INFO] XMLParser :: parserCallbackStartTag : Attribute found: name="talos"
Ch:(3) __
  __
[INFO] XMLParser :: parserCallbackStartTag : Start of tag <material>
[INFO] XMLParser :: parserCallbackStartTag : Attribute found: name="FlatBlack"
Ch:(5) __
    __
[INFO] XMLParser :: parserCallbackStartTag : Start of tag <color>
[INFO] XMLParser :: parserCallbackStartTag : Attribute found: rgba="0.1 0.1 0.1 1"
[INFO] XMLParser :: parserCallbackEndTag : Content of tag: 
[INFO] XMLParser :: parserCallbackEndTag : End of tag <color>
Ch:(3) __
  __
[INFO] XMLParser :: parserCallbackEndTag : Content of tag: 

[INFO] XMLParser :: parserCallbackEndTag : End of tag <material>

however visual.m_material seems to be not initialized indeed if I add

std::cerr << linkName  << " " <<  visual.m_material << std::endl;

here:

https://github.com/robotology/idyntree/blob/db312bfe9f490f991250603f05cea84a8c156899/src/model_io/urdf/src/URDFDocument.cpp#L345-L346

I got

leg_right_4_link 0

for all robot's links

The very same problem happens also for icub

[GiulioRomualdi]

I also noticed that when the tag is <color>

https://github.com/robotology/idyntree/blob/845f53aade73a171b39b60b914beaa1fc8177df9/src/model_io/xml/src/XMLParser.cpp#L235-L237

is actually this:

https://github.com/robotology/idyntree/blob/845f53aade73a171b39b60b914beaa1fc8177df9/src/model_io/xml/src/XMLElement.cpp#L118

Indeed for a MaterialElement if the tag is color the setAttributeCallback is called with a lambda function

https://github.com/robotology/idyntree/blob/845f53aade73a171b39b60b914beaa1fc8177df9/src/model_io/urdf/src/MaterialElement.cpp?_pjax=%23js-repo-pjax-container%2C%20div%5Bitemtype%3D%22http%3A%2F%2Fschema.org%2FSoftwareSourceCode%22%5D%20main%2C%20%5Bdata-pjax-container%5D#L37-L55

So in theory m_info->m_rgba should be correctly initialized and it should contain the color of the link

[traversaro]

Probably the cirrect support for parsing color needs to be implemented in the URDF parser, as you noted. Furthermore, the visualizer(s) need to be modified to properly take into account this. Some notes to keep into account while working on this:

• URDF spec is not super-clear on this (see http://wiki.ros.org/urdf/XML/link) but tipically the color tag is used only for mesh format that cannot handle internally colors (such as .stl).
• If I remember correctly the same "bug" of the URDF color tag being ignored is presented in Classic Gazebo (but not in Ignition Gazebo, see https://github.com/ignitionrobotics/sdformat/pull/526). This does not mean that we should ignore the color tag also in iDynTree, just something that we should remember once the the same model is render in a different way in iDynTree-based visualizers or in Classic Gazebo. However, if someone has any problems with the colors provided in the URDF model, it is quite simple to create a function that just loops over all the visual elements of the model and assign them the white color.

[traversaro]

This may be related to this TODO: https://github.com/robotology/idyntree/blob/cbb0bf5e593b7add489bccb7dd3ba5054b200573/src/model_io/urdf/src/RobotElement.cpp#L55 . Perhaps @francesco-romano may recall something on this.

[francesco-romano]

I would say I did not expect it to be used (or it was not implemented in the old parser and I did not add it in the new)

[traversaro]

A first fix (for material colors defined in visual --> material --> color elements, not in robot --> color) is provided in https://github.com/robotology/idyntree/tree/fixURDFMaterials .