Add support for using icub-models out of the box with PyBullet

[anqingd]

To use iCubGenova04 urdf model in pybullet simulator, I deleted all the lines containing the keyword Gazebo in the urdf file and relocated the meshes folder directory. I'm not sure if the model is still reliable but at least by doing so I can load the model. In view of the simulation, there are some problems:

• iCub hands have collision with the body.
• I received many warnings and most of them are complaining about no inertial data and skin such as

b3Printf: No inertial data for link, using mass=1, localinertiadiagonal = 1,1,1, identity local inertial frame
b3Printf: b3Warning[examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp,115]:

b3Printf: r_lower_leg_skin_10
b3Printf: b3Warning[examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp,115]:

• there are in total 138 joints(is this the correct number?)

[traversaro]

iCub hands have collision with the body.

This is the problem of how joints zero position are defined for the iCub: if all the joints are set to zero, the arms are colliding for the body. This is typically not a problem because the initial position of the arm joints in simulations is the one defined in the initialConfiguration tag, see https://github.com/robotology-playground/icub-models/blob/master/iCub/robots/iCubGenova04/model.urdf#L1866 .
There is any support in PyBullet for loading the model in a specified joint configuration?

I received many warnings and most of them are complaining about no inertial data and skin such as

Unfortunately this seems to be related of the impossibilities of expressing multiple frames for a single link in URDF. See https://discourse.ros.org/t/urdf-ng-link-and-frame-concepts/56 . In a nutshell, we represent "additional frames" as zero-mass links attached to their parent with a fixed joint. This has been working well because by default in Gazebo links attached to fixed joints are automatically lumped to their parent and the fixed joint removed, while in iDynTree this kind of frame is automatically recognized by our URDF parser: https://github.com/robotology/idyntree/blob/master/src/model_io/urdf/src/URDFModelImport.cpp#L751 . However, the handling of this links seems to be different in PyBullet. As a solution, have you considered loading directly the SDF of the robot using the loadSDF method? See bulletphysics/bullet3#1251 (comment) for details on how to use the loadSDF method.

there are in total 138 joints(is this the correct number?)

For the reason outlined before, all the "additional frames" are modeled as links with zero attached to the actual link with a fixed joint. The most interesting quantity for control I think are the "internal non-fixed joints", i.e. the joints on which you can exert a torque through a motor. How many there are of them?

I deleted all the lines containing the keyword Gazebo in the urdf file and relocated the meshes folder directory. I'm not sure if the model is still reliable but at least by doing so I can load the model.

For the meshes, the problem is that meshes URI in URDF are properly defined only when used together with the ROS packages, see robotology/icub-models-generator#28 . However, I hope that this problem is solved if you use loadSDF.

[anqingd]

After trying sdf (with yarp plugin contained and excluded) , the simulation cannot even render. I received warnings as follows

ven = Intel Open Source Technology Center
Workaround for some crash in the Intel OpenGL driver on Linux/Ubuntu
b3Printf: b3Warning[examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp,115]:

b3Printf: expected a world element, continuing without it.
Warning: b3Error[examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp,110]:

Warning: joint dynamics element specified with no damping and no friction
Warning: b3Error[examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp,110]:

Warning: Could not parse dynamics element for joint:
Warning: b3Error[examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp,110]:

Warning: l_arm_ft_sensor
Warning: b3Error[examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp,110]:

Warning: joint xml is not initialized correctly
Traceback (most recent call last):
  File "icubsdf.py", line 12, in <module>
    p.loadSDF("icub_with_hands02.sdf") 
pybullet.error: Cannot load SDF file.

I also tried loading leg and hand sdf files separately and hand can render but not leg.

[diegoferigo]

Can you try loading the sdf model without hands? @anqingd

[traversaro]

If you are using the sdf models in icub-gazebo, I suggest you to try instead the SDF files contained in icub-models .

[anqingd]

If you are using the sdf models in icub-gazebo, I suggest you to try instead the SDF files contained in icub-models

@traversaro is this icub-models the icub-models you were referring to? It seems urdf is the only model type... Or should I turn to somewhere else for sdf?

[traversaro]

Sorry, I thought we were committing directly the SDF files in that repo.
You can convert those urdf to sdf using the command gz sdf -p model.urdf > model.sdf (this is exactly what Gazebo does internally when loading a URDF file).

[anqingd]

Thanks @traversaro
Now the SDF model is loadable without fake links complaining.

[traversaro]

Issue in bullet related to loading SDF files converted from URDF files: bulletphysics/bullet3#1707 .
Issue in sdformat related to produce SDF models without <use_parent_model_frame>1</use_parent_model_frame> :
https://bitbucket.org/osrf/sdformat/issues/189/convert-urdf-files-to-sdf-with .

[traversaro]

Relatively to the problem on how to have kinematic frames in URDF/SDF models, see also RobotLocomotion/drake#9350 .

[fiorisi]

@anqingd can we close this issue? In case, feel free to close.

[erwincoumans]

You could explicitly add inertia in each link with mass zero, instead of leaving out the inertia completely for those links?

<inertial>
      <mass value="0"/>
      <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
    </inertial>

There is any support in PyBullet for loading the model in a specified joint configuration?

You can simply reset the joint state, after loading the URDF:

robot = pybullet.loadURDF("robot.urdf")
for joint in range(pybullet.getNumJoints(robot)):
    p.resetJointState(robot,joint,initialJointAngle[joint])

[traversaro]

Thanks @erwincoumans for chiming in!

You could explicitly add inertia in each link with mass zero, instead of leaving out the inertia completely for those links?

Indeed this may be the safest option, as the semantics of URDF link without inertia is not really defined in http://wiki.ros.org/urdf/XML/link , even if historically a lot of URDF produced by OSRF ( such as https://bitbucket.org/osrf/drcsim/src/194be8500fef81593f79607a21ee2badd9700a0e/atlas_description/urdf/atlas_v5.urdf?at=default&fileviewer=file-view-default#atlas_v5.urdf-788 ) suggested the use of link without inertia element to represent purely kinematic frames, that is the interpretation used in the kdl_parser and in the Gazebo URDF --> SDF converted.

I remember having a detailed comment on this in https://bitbucket.org/osrf/sdformat/issues/199/urdf-to-sdf-conversion-ignores-links , but for some reason it disappeared, probably I accidentally deleted it.

You can simply reset the joint state, after loading the URDF:

Great, thanks. cc @anqingd @diegoferigo

[traversaro]

I remember having a detailed comment on this in https://bitbucket.org/osrf/sdformat/issues/199/urdf-to-sdf-conversion-ignores-links , but for some reason it disappeared, probably I accidentally deleted it.

Apparently the comment is still available at https://bitbucket.org/osrf/sdformat/issues/199/urdf-to-sdf-conversion-ignores-links#comment-47642199 , but for some reason is not visible if I am not logged in Bitbucket. Reporting hereafter for GitHub users:

But I'd expect a URDF link without an inertia element to be converted to an SDF link without an inertia element, which is valid SDF.

The semantics of SDF <link> without an inertia element is a link with mass of 1 Kg and diagonal inertia matrix of 1 Kg m^2 .

The semantics of URDF <link> without an inertial element instead is not defined in http://wiki.ros.org/urdf/XML/link , but I would say that the major of software in ROS interpret it as a massless link, that is tipically used as a proxy to represent kinematic frames as URDF does not support any way to explicitly express frames (https://discourse.ros.org/t/urdf-ng-link-and-frame-concepts/56).

Example of where a link without inertial element is considered massless include:

• The kdl_parser , in which a link without inertia is considered to have a mass and a inertia of zero ( https://github.com/ros/kdl_parser/blob/96618e74b2d43f8b8493ad94503d8da77924846d/kdl_parser/src/kdl_parser.cpp#L135 )
• The rdbl URDF parser, in which a link without inertia is considered to have a mass and a inertia of zero ( https://bitbucket.org/rbdl/rbdl/src/0879ee8c548a07b8a9e1296bcbb5b006e1a098c7/addons/urdfreader/urdfreader.cc#lines-220 )
• Several URDF models such as the Atlas, the Fetch or the Baxter contain inertial-less <urdf> links that I think are meant to be massless. Indeed some REPs such as REP 103 or REP 105 recommend to have "fake" links in the URDF, that are tipically implemented using inertial-less links.

For these reasons I think that converting any inertial-less URDF link to an SDF inertial-less link may be quite complicated, as it will add 1 kg for each inertial-less link contained in those models. To be honest, PyBullet is instead an example of a library that converts URDF inertial-less links to links with a mass of 1 kg, and we had several problems exactly for this reason, as our robots started to weight 100 Kg more due to all the "kinematic frames" contained in the model, see #12 (comment) .

[diegoferigo]

I found this tutorial about the differences between SDF and URDF frames specifications quite interesting:

https://bitbucket.org/osrf/sdf_tutorials/src/default/pose_frame_semantics/tutorial.md

[traversaro]

Issue in bullet related to loading SDF files converted from URDF files: bulletphysics/bullet3#1707 .
Issue in sdformat related to produce SDF models without <use_parent_model_frame>1</use_parent_model_frame> :
https://bitbucket.org/osrf/sdformat/issues/189/convert-urdf-files-to-sdf-with .

This is fixed now when using sdformat >= 8.4 .

[traversaro]

I updated the issue to track the only remaining problem with PyBullet.

[erwincoumans]

We are probably not going to break the existing PyBullet API for this. We can add a flag to loadURDF to change behavior though.
You can trivially fix this by fixing the URDF file to always have a field with some meaningful mass.

Note that a single free floating link with zero mass (and no children/parent) will not simulate properly and will be regarded as static.

[traversaro]

Modified the name of the issue to clarify a bit the context. Probably as part of this we can also add a note in the README.

[traversaro]

See also https://github.com/rustlluk/pyCub .