Change feet collision meshes to a simpler primitive shape

[nunoguedelha]

The collision meshes in the foot should be substituted with a simpler primitive shapes (box or four spheres). This would improve the simulation performance and reduce the computations for estimating the contact forces.

[fiorisi]

The iCub foot has a quite complex shape. The SDF format provides 3 possible solutions to approximate the collision geometry.

Sphere

In theory, we can have 4 or more little spheres on each corner of the foot. The major drawback, in this case, is that collisions that don't involve the spheres are not detected (i.e. the foot can intersect with a step, if the step edge is in contact only with the center of the sole).

Box

In this case, we can either draw a box smaller than the foot or a bit bigger.

Polyline

Starting from SDF version 1.5, it is possible to use also polylines. In this case we can better approximate the foot shape. If we go for this geometry, we need to update the supported geometries in the simmechanics-to-urdf tool. This requires also making sure that the library package GTS is installed with the Gazebo simulator. This is automatically done by upgrading to the latest Gazebo version 8.1.1.7 or explicitly installing the package, for instance through a package manager (apt-get install libgts-dev on linux, or brew intall gts on macos.

4 points

6 points (compatible with IHMC controller)

8 points

[nunoguedelha]

We decided to go for the use of polylines for defining the collision geometry. As a reminder: the library package GTS has to be installed. It has been checked in macos that the library can be installed with Gazebo by upgrading to latest version 8.1.1.7 through Homebrew. The simmechanics-to-urdf dependency is implemented by ...

[nunoguedelha]

Background

Collision geometry and Polyline definition in the SDF model

A link can contain multiple collision elements (refer to collision definition in this Gazebo tutorial). For instance:

<link name='l_foot'>
  ...
  <collision name='l_foot_collision'>
      ...
  </collision>
  <collision name='l_foot_collision_2'>
      ...
  </collision>
  ...
</link>

In each collision element, only the first defined geometry element will be considered, the remaining ones ignored.

Collision geometry and Polyline definition in the URDF model

As per the URDF specification here, Polyline shapes are not supported in URDF format. Multiple collision bodies are not supported in URDF either. Custom collision elements, for instance meant for collision checking by a controller, can be added wrapped in a custom tag——which will be ignored by the ROS URDF parser.

Supported URDF to SDF conversions

• Polyline shapes are ignored by the Gazebo parser if embedded in a URDF file. For this reason, the Polyline parameters must be embedded in a <gazebo> tag.
• The Gazebo parser considers only the first defined collision element and in it, only the first geometry element, ignoring all the other geometries and collision blocks. So we cannot generate multiple collision elements through the <gazebo> tag.

So, two solutions are left:

1. define an additional link ("sole") including the desired collision shape based on Polylines
2. replace, in the foot link description, the current mesh by a box, waiting for the Polylines to be properly supported by URDF and the Gazebo URDF parser.

[nunoguedelha]

Solution 1 Requirements

Desired resulting collision geometry in SDF format

• The overall desired foot collision geometry would be defined in the URDF or explicitely in a SDF block wrapped in a tag <gazebo>
• the geometry is composed by the union of:
  • the upper part of the sole mesh
  • the extruded polyline shaping the contact with the ground

We give here an example of a model composed by a standalone foot link with simplified collision geometry. The extruded polyline geometry element between the foot mesh and the ground has 4 points and an exagerated height for making it more visible.

 #### YAML config parameters The Polyline shapes are required only for iCub Gazebo models so they could be inserted through Cmake while generating the YAML config file, as for the chest assigned mass `@CHEST_ASSIGNED_MASS@` or the links assigned inertias `@GAZEBO_ASSIGNED_INERTIAS@` variables. Considering the current partial support of polylines, it would be preferable to use the polylines only on iCub Gazebo models. The input parameters to define are as follows: ``` collisionShapes: - linkName: l_foot type: polyline point: [,] point: [,] ... height: - linkName: r_foot ... ``` #### Derived requirements - Considering the constraints on defining Polylines in URDF and on the `` tag behavior, the retained solution would be to define two new links "l_sole" and "r_sole", wrapped in a `` tag, including only a `collision` and `inertial` blocks (no visual block) and rigidly fixed respectively to the left and right foot links. - the `inertial` has to be defined. Actually we cannot add a mass less link in a SDF since Gazebo doesn't handle it properly (the sole link won't move along with the foot link during the simulation, although both are dragged together while the simulation is paused). We then have to define the inertia. If both "sole" and "foot" inertial frames coincide, we can give to each link half of the initial foot inertia, and transfer a 1% mass from the foot to the sole. This way, we conserve the overall initial properties. Left sole case: ``` 0 0 0 0 0 0 0.0336668 -0.0053736 -0.001164 -1.5707963267948961 0 -1.5707963267948961 0.001 0.00121242 -3.69533e-07 5.79273e-05 0.00141712 -7.64972e-06 0.000273846 0 0 0 0 0 0 -0.05 -0.05 -0.05 0.04 0.12 0.04 0.12 -0.05 0.04 l_sole l_foot 0 0 0 0 0 0 ``` Adding the block below for the collision parameters already defined previously (max contacts, friction, bounce, ...) won't work since it is parsed before the "l_sole" link block (added to the model, so last to be parsed) ``` 0 100000 4 18000000 100 100 0.0001 1 1 0 0 0 ``` So these bounce/friction parameters have to be integrated directly in the "l/r_sole" link definition.

[fiorisi]

Thanks @nunoguedelha for the analysis. Do you think that we need the collision in the URDF? Otherwise, for the time being, I would keep it simple: use the polyline as the collision geometry of the sole for all the models (also the one that we don't use in Gazebo).

[nunoguedelha]

Solution 2 Requirements

Foot link change in the URDF

Replace the collision mesh by the box defined through the YAML config parameters.

YAML config parameters

As for the previous solution, the collision box is required only for iCub Gazebo models so the parameters could be inserted through Cmake when generating the YAML file from the YAML template. The input parameters would be generated in the YAML file as:

collisionShapes:
  - linkName: l_foot
    type: box
    xyz: [<x>,<y>,<z>]
    originXYZ: [<x>,<y>,<z>]
    originRPY: [<r>,<p>,<y>]
  - linkName: r_foot
    ...

[nunoguedelha]

Thanks @nunoguedelha for the analysis. Do you think that we need the collision in the URDF? Otherwise, for the time being, I would keep is simple: use the polyline as the collision geometry of the sole for all the models (also the one that we don't use in Gazebo).

Well, if we go for the Polyline choice (solution 1), it requires defining two new links "l_sole" and "r_sole" that we fixe to the feet. restraining it to Gazebo models through Cmake is no big deal and avoids adding complexity (additional links) to the other models. Consider that sooner or later, Polylines should be supported also in URDF, or at least by the Gazebo URDF parser (unless they don't use any layer over the urdfdom parser), and only then we can replace the mesh or add to it the polylines within the foot link definition.

The biggest simplification would be to use just a box for now (solution 1), on all models or just the Gazebo model.

[nunoguedelha]

If we go for solution 1, we can proceed as follows:

• replace the feet mesh by a box on all models
• add a second geometry in the foot collision element labeled <collision_controller_check> that would be ignored by Gazebo, and could be parsed any of our controllers if required.

[traversaro]

Consider that sooner or later, Polylines should be supported also in URDF, or at least by the Gazebo URDF parser (unless they don't use any layer over the urdfdom parser)

I am afraid Gazebo will never support Polylines in URDF files.

[nunoguedelha]

After last discussion with @fiorisi , we decided to go for solution 2, until a better support of polylines is available:

• replace the feet mesh by a box on all models The simmechanics-to-urdf parser already supports replacing meshes with simple geometric shapes, as it was tested on R1.

[fiorisi]

Possible sole approximation

iCub 2.5

iCub 2.5+ box origin

[fiorisi]

Solved with https://github.com/robotology-playground/icub-model-generator/commit/bc1ed7765d51be14a323b02cbe1936644e029d22.

[traversaro]

In the future, to do this automatically (and more importantly, systematically) it could make sense to investigate the use of approximate convex decomposition techniques such as the one contained in https://github.com/gaschler/bounding-mesh or https://github.com/kmammou/v-hacd/ .

[traversaro]

cc @aerydna @claudiofantacci @xEnVrE