Idyntree: Load meshes that are specified using the `package://` URDF URI

[traversaro]

We need to correctly process meshes of generated URDF models.

One of the problems to solve is that the URDF format [1] does not describe the semantics of the filename attribute, that is used to specify external meshes in Collada or STL format.
In practice, most URDF released as part of ROS packages specify the meshes location using the package:// URI, that are resolved to absolute filenames using ROS' resource_retriever.

Clearly this is not a viable strategy for libraries that do not depend on ROS, and want to load arbitrary URDF files. Unfortunately mesh handling without using ROS is not uniform across different libraries.
Some (such as Google's Bullet ) just support meshes specified w.r.t to the model subdirectory using the format filename="meshes/mesh.dae", but I guess a model of that kind would not be supported by ROS tools.

For iDynTree, I guess the most reasonable strategy is to add a class that is able to resolve package:// URIs, with some strategy.
A possible basic strategy is to manually specify to the class to (to be completed)

[1] : http://wiki.ros.org/urdf/XML/link

See https://github.com/robotology-playground/icub-model-generator/issues/33 , https://github.com/robotology-playground/icub-model-generator/issues/28 for more info.

[traversaro]

Related urdf issue: https://github.com/ros/urdf/issues/30 .

[traversaro]

SDF is evaluating the support for specifying meshes using path relative to the model location: https://bitbucket.org/osrf/sdformat/pull-requests/558/accept-relative-path-in/diff .

[traversaro]

Related PR: https://github.com/leggedrobotics/raisimOgre/pull/25 .

[traversaro]

According to the ROS specification we need to look in ROS_PACKAGE_PATH or AMENT_PREFIX_PATH . I think pinocchio has some nice compact code for that, see:

• https://github.com/stack-of-tasks/pinocchio/blob/17c479e2fe91905122713643f489feb8ddf135a3/src/parsers/urdf/geometry.cpp#L470
• https://github.com/stack-of-tasks/pinocchio/blob/9389400a6018e97ec3822796d465a4b801630a1f/src/utils/file-explorer.cpp#L45

And given Pinocchio license we can just take that code, as long as we keep the existing copyright header.

cc @S-Dafarra @GiulioRomualdi

[S-Dafarra]

According to the ROS specification we need to look in ROS_PACKAGE_PATH or AMENT_PREFIX_PATH . I think pinocchio has some nice compact code for that, see:

• https://github.com/stack-of-tasks/pinocchio/blob/17c479e2fe91905122713643f489feb8ddf135a3/src/parsers/urdf/geometry.cpp#L470
• https://github.com/stack-of-tasks/pinocchio/blob/9389400a6018e97ec3822796d465a4b801630a1f/src/utils/file-explorer.cpp#L45

And given Pinocchio license we can just take that code, as long as we keep the existing copyright header.

cc @S-Dafarra @GiulioRomualdi

I have implemented some code that seems to do the same of https://github.com/stack-of-tasks/pinocchio/blob/9389400a6018e97ec3822796d465a4b801630a1f/src/utils/file-explorer.cpp#L45 in https://github.com/S-Dafarra/dyn-visualizer/blob/master/src/main.cpp#L29-L68 using Qt. The first part does not really need Qt. We can get the environmental variable using std::getenv and split the string using the : separator as in https://stackoverflow.com/a/10058725

At this point, we have a list of paths. Then, we can try to open the mesh file by basically trying all the addresses :thinking:

[traversaro]

We can get the environmental variable using std::getenv and split the string using the : separator as in https://stackoverflow.com/a/10058725

Note that on Windows the PATH separator is ; , but yes, that is correct.

At this point, we have a list of paths. Then, we can try to open the mesh file by basically trying all the addresses 🤔

In theory it should be possible to also understand which folder to use by inspecting the package.xml files, but probably that is an overkill that is not strictly necessary. In any case, other similar classes in other projects are:

• DART https://github.com/dartsim/dart/blob/46abc668dbbcdc58fb63e07f7d8ffb15e834da0a/dart/utils/PackageResourceRetriever.hpp
• Drake https://github.com/RobotLocomotion/drake/blob/4e5b1133d89c8499ed3aed0fb3905672b9bdf175/multibody/parsing/package_map.cc (related issue https://github.com/RobotLocomotion/drake-external-examples/issues/170 )

[S-Dafarra]

With https://github.com/S-Dafarra/dyn-visualizer/commit/b359c4b8a50079737d5bea8226a73a2d6d0682df I managed to load the meshes without using Qt on Linux. I will try to handle also the Windows case.

[traversaro]

Great! Once we have the right code, the places to update are:

• https://github.com/robotology/idyntree/blob/17d5a7db7f7190944aa6d13f96daaf1d4615946c/src/visualization/src/IrrlichtUtils.h#L214
• https://github.com/robotology/idyntree/blob/17d5a7db7f7190944aa6d13f96daaf1d4615946c/src/solid-shapes/src/InertialParametersSolidShapesHelpers.cpp#L188

[traversaro]

Great! Once we have the right code, the places to update are:

* https://github.com/robotology/idyntree/blob/17d5a7db7f7190944aa6d13f96daaf1d4615946c/src/visualization/src/IrrlichtUtils.h#L214

* https://github.com/robotology/idyntree/blob/17d5a7db7f7190944aa6d13f96daaf1d4615946c/src/solid-shapes/src/InertialParametersSolidShapesHelpers.cpp#L188

Probably we could have a method directly in ExternalMesh to obtain the absolute filename.

[S-Dafarra]

I would like to keep separated the input read from the URDF from its interpretation. Nonetheless, a method in ExternalMesh might be the best place where to put such code :thinking: In any case, I would keep the output of getFileName as it is. A static method might be reasonable, but at that point the API would be a little weird :thinking:

[S-Dafarra]

I have tried to consider also the Windows case in https://github.com/S-Dafarra/dyn-visualizer/commit/30c39bc151a569be8894b78827d9dadf57d6a1c1

What about adding a class in ModelIO? Something like ExternalMeshLocator?

[traversaro]

In any case, I would keep the output of getFileName as it is. A static method might be reasonable, but at that point the API would be a little weird 🤔

I think it is ok, getFileName will return the filename specified via an URI (i.e. a string that starts with file://, package://, but in the future could be also http:// or similar), while getFileLocationOnLocalFileSystem (please select any name) will return the location of the file on the local disk.

What about adding a class in ModelIO? Something like ExternalMeshLocator?

The existing code does not seem to have state, perhaps a function may be also be ok?

[S-Dafarra]

The existing code does not seem to have state, perhaps a function may be also be ok?

I was thinking to allow the possibility to specify also the prefix, and/or to add some search paths, and/or to specify the environmental variables to check. That's why I thought about a class

Edit: Thinking about it, indeed, also a function is doable

[traversaro]

I was thinking to allow the possibility to specify also the prefix, and/or to add some search paths, and/or to specify the environmental variables to check. That's why I thought about a class

That make sense, the tricky part is understand how to make that composable w.r.t. to the use of those functionality in deeply nested code, see https://github.com/robotology/idyntree/issues/291#issuecomment-756837515 , however this can be done in a second step if the default behaviour is ok to get the visualizer to work with iCub models.

[S-Dafarra]

I was thinking to allow the possibility to specify also the prefix, and/or to add some search paths, and/or to specify the environmental variables to check. That's why I thought about a class

That make sense, the tricky part is understand how to make that composable w.r.t. to the use of those functionality in deeply nested code, see #291 (comment) , however this can be done in a second step if the default behaviour is ok to get the visualizer to work with iCub models.

Good point! Ok, let's keep it simple and stupid :grin:

[S-Dafarra]

Relative PR https://github.com/robotology/idyntree/pull/798

[traversaro]

Related bullet issue: https://github.com/bulletphysics/bullet3/issues/3224 .

[traversaro]

Fixed by https://github.com/robotology/idyntree/pull/798 .