Avoid to use modified models to deal with numerical instabilities in Gazebo and related tools

[traversaro]

Back in the autumn of 2013, I was preparing the first ever model of iCub for Gazebo (the one that is currently stored in https://github.com/robotology/icub-gazebo .

Unfortunately, the model with correct inertia was not being correctly simulated, mainly due to the default timestep used by Gazebo/ODE (1 ms), non-error-controlled integrator used by Gazebo/ODE and the friction parameters I was using at the time. The problem could have been solved by decreasing the timestep of Gazebo. Instead, I decided to increase manually the inertia of some links of the iCub model to reduce the stiffness of the dynamical system. That decision created the duality between the iCub models that we use for estimation and control on the real robot (typically named iCubCityNameXX) and the models used in Gazebo simulation, i.e. icubGazeboSim and later iCubGazeboV2_5 . The strategy was eventually propagated to this repo as well, see robotology/icub-models-generator#52 (comment) and robotology/icub-models-generator#71 . I think that the design decision of having different models was wrong, and this is getting more clear every day.

In particular, I think that having different models is problematic for the following reasons:

• It is not clear to users, that prefer to have one single source of true for their software architectures
• If you are able to simulate the model with stiff inertias (for example by reducing the timestep in Classic Gazebo with ODE, or using a more advanced physics engine with Ignition Gazebo) you can't easily, you need instead to completely change model

An alternative solution that I think it would make sense to use instead of duplicated models, would be a custom Gazebo Classic plugin that, during loading of the model in Gazebo Classic, would increase the inertia and the damping of the links and joints specified in the configuration. In this way, if a user wants to customize the simulation to disable the increased inertia, for example because he decrease the timestamp, it can without problems. Note that such a solution could create an inconsistency as controllers would use the model with non-increased inertia. However, I do not think this is a big problem as if the users want accurate simulations, they should know how to appropriately reduce the Gazebo timestep to obtain accurate simulation without the increased inertia workaround.

[diegoferigo]

While having a one-size-fits-all model would be a great achievement, from our experience we realized that is hardly possible.

We could maybe remove the duality between iCubGazeboV2_5 and iCubCityNameXXX with the complexity of a new Gazebo Classic plugin. However, the usage of the official iCub models in other simulators in most cases require other modifications. See the following examples for the first vendored models that come in my mind:

• diegoferigo/icub-models-pybullet
• robotology-playground/pybullet-robot-envs
• dic-iit/gym-ignition-models

Disclaimer: I'm the maintainer of two of them 😅.

I would love to see a solution that uniforms our models and enables us the usage of a single one. Though, I don't see it feasible due to subtle differences of different simulators. While adding a Gazebo Classic plugin could permit performing these changes on the fly for that simulator, it could not be as straightforward for others.

I'm not coming here with a solution. What you propose is a step forward to improve the overall situation and I support it fully. My main point is that the situation is even more fragmented than what you described.

[traversaro]

While having a one-size-fits-all model would be a great achievement, from our experience we realized that is hardly possible.

I realized that probably the title of the issue is quite misleading, sorry. The point of the issue is to make sure that any modification for use with Gazebo or any simulator that we "officially" support in the future should be done during the startup of the simulation, rather then done at the C++/CMake configuration time, or even worse (as in the case of this repo) in a CI process of a repo that users do not even download (https://github.com/robotology/icub-model-generator).

So the idea is not to have "one-size-fits-all" model, but rather make sure that any custom modification can be done automatically or semi-automatically. With semi-automatically, I mean using an automatic process, but with some additional inputs that may be prepared by hand, think for example the process of adding motor control plugins: you need to specify once which joints compose a parts and their gains, but this is not something that will change if the input model changes, for example if the iCub ankle is redesigned to have more range of motion. This is what we already achieve with https://github.com/robotology/icub-model-generator, but for some aspects I think we should move this automatic procedure closer to the user.

Note that the focus on "automatic procedure" is not just to simplify maintenance of the model, but rather to enable "closing the loop" at the mechanical design level. While IIT is one of the few (research) places that collect under the same room people that design robots and people that do research on high level algorithms to use (i.e. control, plan, learn) with these robot, the mechanical design process is still done largely in open loop: if we want to design a new leg for the iCub for improved walking, a mechanical engineer will go to the team working on walking, collect some high level feedback about the existing problems, and try to find a solution in a form of a proposed mechanical modification. Ideally, we would like for the mechanical engineer to directly test his improvement with our existing "walking simulators", but this is not possible if going from a model in CAD to a model that can be used in a high level simulator requires manual modifications every time. Note that most impressive results in robotics in the last decade (for example in Boston Dynamics) has been indeed reached via closing the loop at the mechanical level (see ‘Build It, Break It, Fix it.), but in their case they actual built a physical prototype for an idea, try it out, and see out the problems. Clearly doing with real prototypes is really expensive and not feasible for us, so we need to be smart, and be able to build that (or at least approximate that) via simulation tools and automatic model processing pipelines. Note that this goal is one of the reasons we try to make sure that all our software run on Windows: all CAD programs are mainly Windows-based, so to integrate our software with them (for example to wrap or controllers in an FMU and load it in the CAD-integrated simulators) we need not to accumulate technical debt that would prevent to use our software on Windows.

Though, I don't see it feasible due to subtle differences of different simulators.

Can you make an example of modification that can't be implemented with some automatic processing, for example by processing the DOM at xml level? I totally understand if it is more convenient to do some modifications manually during research to fast iterate, but I don't see anything "not feasible" or "hardly possible", but probably I am missing some tricky technical details.

[diegoferigo]

The point of the issue is to make sure that any modification for use with Gazebo or any simulator that we "officially" support in the future should be done during the startup of the simulation

So the idea is not to have "one-size-fits-all" model, but rather make sure that any custom modification can be done automatically or semi-automatically.

This is more clear, thanks! In this moment we officially support only Gazebo Classic, but things could change in the near future. Particularly, considering the tools and needs of researchers in robot learning (which use extensively simulations) we should start thinking of extending the support also to other solutions. In fact, Gazebo Classic is neither the best choice nor the most used simulator by the community of this domain. I'm not saying that we should aim of providing key-in-hands solutions (model, PID gains, etc), but at least a model that could be imported without much hassle would be nice.

As I wrote above, I support your initiative. Though, what I wanted to say is that if we replace the model used for simulations with solutions that edit the model before loading it, we (or someone else) has to develop the same for the simulator they use. For Gazebo Classic it could be a plugin, for Ignition Gazebo it could be either a plugin or a bindings call that modify the ECM between model loading and next physics step, maybe an XML parser for PyBullet. Considering the needs of most of the researchers, I fear that in the meanwhile these solutions for the common simulators will be developed and distributed, more and more parties will decide to just vendor a model that just works (not that now the situation is that much different...). And it could increase even more fragmentation.

but I don't see anything "not feasible" or "hardly possible", but probably I am missing some tricky technical details

That sentence was poorly written, sorry, the last part of it is more realistic: it could not be as straightforward for others.

As examples, here few issues / PRs I'm aware of that discuss using these models in simulators we do not currently officially support:

• Add support for using icub-models out of the box with PyBullet #12 (note that in this case our urdf is wrongly parsed due to the fake links to add extra frames)
• Add iCubGazeboV2_5 model gym-ignition-models#1

To conclude, you're opening an open door when you propose to automatize even further our model generation pipeline. We just have to try smoothing as much as possible the transition.

[traversaro]

Considering the needs of most of the researchers, I fear that in the meanwhile these solutions for the common simulators will be developed and distributed, more and more parties will decide to just vendor a model that just works (not that now the situation is that much different...). And it could increase even more fragmentation.

Ah, I think I finally got what you mean. Let me try to rephrase: with iCubGazeboXX models, users of simulators different from Gazebo, let's say Morse that suffer from the same problems of small inertia can just take the iCubGazeboXX model, and start to use it directly without modifications/vendoring. If we retire the iCubGazeboXX models, then they will need custom modifications, and they will then do those manually and upgrading to a new model will be more complex. That is completely true, and to be honest I personally think it is acceptable and an improvement on the current situation. In particular, we will not create false expectations regarding our models, and the users of the models realize directly an existing limitation of their simulators in simulation real mechanical system, instead of blindly assuming that everything is working thanks to a workaround none of them will be fully aware of.

[traversaro]

(Previous comment has been edited and is now complete).

[traversaro]

Good news, the iCub 3 model added in robotology/icub-models-generator#145 does not require any workarounds (for now) to run in Gazebo, so we have a proper iCubV3 model instead of a iCubGazeboV3 . Let's try to keep it that way, and keep workarounds downstream.

[Nicogene]

Good news, the iCub 3 model added in robotology/icub-models-generator#145 does not require any workarounds (for now) to run in Gazebo, so we have a proper iCubV3 model instead of a iCubGazeboV3. Let's try to keep it that way, and keep workarounds downstream.

Unfortunately, also iCub 3 model needs the inertia workaround, the small inertias of the arms' links make explode the model.
In this sense, iCubV3 became iCubGazeboV3 and iCubGenova09 keeps the real inertias.

(see robotology/icub-models-generator#147)

[diegoferigo]

Maybe Proposal for parameter passing can be a powerful solution. It seems based on SDFormat 1.7, therefore also Gazebo Classic 11 (#77) should be supported.

Edit: maybe not, at the time of writing Gazebo Classic 11 uses sdformat9 and the proposal targets sdformat10. It's not remote the possibility that the supported sdformat version by Gazebo will be increased in the future.

[traversaro]

Edit: maybe not, at the time of writing Gazebo Classic 11 uses sdformat9 and the proposal targets sdformat10. It's not remote the possibility that the supported sdformat version by Gazebo will be increased in the future.

Unfortuantly I don't think this will ever happen. Bumping sdformat major version used effectively breaks the ABI of Gazebo 11, essentially meaning releasing Gazebo 12 that has been explicitly excluded by OR.

[diegoferigo]

Edit: maybe not, at the time of writing Gazebo Classic 11 uses sdformat9 and the proposal targets sdformat10. It's not remote the possibility that the supported sdformat version by Gazebo will be increased in the future.

Unfortuantly I don't think this will ever happen. Bumping sdformat major version used effectively breaks the ABI of Gazebo 11, essentially meaning releasing Gazebo 12 that has been explicitly excluded by OR.

😞 I view of Ignition Gazebo, that definitely supports all the new sdformat standards, maybe the parameter passing feature could be used in such a way:

1. New file with <sdf version="1.7"> only for Ignition Gazebo
2. Include the Gazebo Classic model
3. Use the parameter passing feature to remove plugins and adapt the autogenerated model

[gabrielenava]

Unfortunately, also iCub 3 model needs the inertia workaround, the small inertias of the arms' links make explode the model.
In this sense, iCubV3 became iCubGazeboV3 and iCubGenova09 keeps the real inertias.

hello @Nicogene @traversaro

on iRonCub3 we also needed the inertia workaround for Gazebo simulations. We basically copied the following modification in our yaml file: https://github.com/robotology/icub-models-generator/blob/master/simmechanics/data/icub3/ICUB_3_all_options_gazebo.yaml.in#L481

I was wondering what is the meaning of:

mirroredInertia:
- mirroredLink: l_upper_arm
  originalLink: r_upper_arm
  simmetryReferenceLink: root_link
  symmetryPlane: xz

because, comparing a model.urdf with/without this modification I see that not only the inertia, but also the mass, position, orientation of the link are modified. The orientation is actually heavily modified:

without mirroredInertia

 <link name="l_upper_arm">
    <inertial>
      <origin xyz="0.0010548304233867212 0.0024663703554936023 -0.050596915652018154" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
      <mass value="1.6203"/>
      <inertia ixx="0.002863606589878253" ixy="6.96396353067558e-05" ixz="-0.0004505518970518024" iyy="0.002841423681069468" iyz="0.0002762768153497255" izz="0.00142003494270709"/>
    </inertial>
    <visual>

with mirroredInertia

<link name="l_upper_arm">
    <inertial>
      <origin xyz="0.0010476375729993916 0.002559622582259849 -0.05065314985074498" rpy="-0.062128608300921935 0.07142689081245782 0.27099931149673573"/>
      <mass value="1.67729"/>
      <inertia ixx="0.002960829234557446" ixy="-1.0441222117313817e-05" ixz="0.0002229885938828402" iyy="0.0030756376591531418" iyz="0.00020156448677983343" izz="0.001327263106289413"/>
    </inertial>
    <visual>

is this behavior expected?

cc @vpunithreddy

[traversaro]

is this behavior expected?

This issue is probably not directly related to your problem, but anyhow at a first glance the behaviour is expected: the orientation that is being changed is the orientation of the "inertial" frame (note that is not an "inertial" frame in Newtonian sense https://en.wikipedia.org/wiki/Inertial_frame_of_reference, but in the URDF sense: http://wiki.ros.org/urdf/XML/link), i.e. the frame that has the origin of the center of mass and in which it is expressed the 3D inertia matrix. So it is expected that if you mirror the inertia information (mass, center of mass, 3D inertia matrix), also the inertial frame (in URDF sensor) changes. See https://github.com/robotology/simmechanics-to-urdf/blob/30ceec80f8c37fb9638f6117a04e64267c0d9a08/simmechanics_to_urdf/firstgen.py#L1252 for the related code, is relatively documented.

[gabrielenava]

thank you @traversaro!

we have two different urdf, one is used in Gazebo and the other with the real robot. The gazebo one has augmented inertias, for the rest they are the same.

do you think that this mirroredInertia modification should be only implemented in the Gazebo model, or instead should we propagate it to all models? Because in the link you sent in your comment is written:

# if link is mirroredInertia, we should not trust the inertial infromation
# provided by SimMechanics XML because it could be buggy. We will mirror
# the inertia information of another link instead

so at this point, it makes sense to me to propagate the mirrored inertia to all models