[model-generation] Numerical instability due to low articulated body inertia in the iCub shoulder

[traversaro]

The mechanical department of iCub Facility recently extracted a URDF/SDF of the iCub v2.5 (with backpack and battery) from the CAD model of the robot, using the process described in https://www.icub.org/wiki/Creo_Mechanism_to_URDF .

That model was committed in robotology-legacy/icub-gazebo-legacy@0e631bd .
Tests performed with Gazebo 6 (i.e. ODE Physics Engine with Semi Explicit Euler integration with a fixed timestsamp of 1ms) and with a fixed base robot show that when the *_shoulder_pitch and *_shoulder_yaw are close to be coincident (for example in the -90 90 0 configuration of the shoulder) then the simulation explodes.

The reason why is apparently that given the relatively small dimensions of the _shoulder_1 and _shoulder_2 links the associated apparent inertias seen by the *_shoulder_pitch is low (~ 10^-4 Kg m^2 ) and that, coupled with a stiff position control by the low level PID, results in a system that is numerically difficult to integrate.
Consistently with this theory, reducing the fixed timestamp to 0.1 ms solves the problem on ODE.

Anyway it is desirable to be able to provide a model that works flawless on the stock version of Gazebo. A possible way is to increase artificially the inertia matrices of _shoulder_1 and _shoulder_2 given that most (all?) users will not concerned by such a negligible mismatch between reality and simulation (other differences, such as the fact that the transmission chain is not modeled, are orders of magnitude more relevant) .

Theory note: multiplying an inertia matrix at the com by K without changing neither the mass nor the COM position is equivalent to stretch the mass distribution of the rigid body around the COM by a factor of sqrt(k).

With a quick check it seems that a multiplication of the inertia matrices of _shoulder_1 and _shoulder_2 by 10 (i.e. a stretch of ~ 3.1623 ) is solving the issue.

[DanielePucci]

Can you be a little clearer on the theory note?

[traversaro]

In the 2D case : http://mathb.in/45652 . The 3D case is similar .

[DanielePucci]

Got it. I agree on the 3D case.

[traversaro]

I think this can be also fixed by just enabling the implicit damping in Gazebo, see robotology/icub-models-generator#24 . This needs to be investigated.

[nunoguedelha]

@traversaro , regarding the theory note, I noticed some typos. Could you check this review:
http://mathb.in/147496

Regarding the implicit damping, I've just tried it on the iCubGenova04 model generated from CAD and it didn't work. I still had to edit the inertial matrices. I've pushed the updated model to my fork of icub-models.

[traversaro]

: ( .
For which links did you had to increase the inertia?

[traversaro]

I think we need to try to fix this sooner or later. By the way, I never noticed at the time I increased the iCub inertias (around October/November 2013 to get Gazebo stability that a similar modification was performed also in the Atlas model for the VRC a few months before:

• https://bitbucket.org/osrf/drcsim/pull-requests/157

I found this link in this paper, that is interesting and related to this issues:

• https://www.osrfoundation.org/wordpress2/wp-content/uploads/2015/04/simpar2014.pdf
  Another interesting read is:
• http://robotics.cs.tamu.edu/dshell/papers/simpar2010ode.pdf

Another interesting related pull request in drcsim are:

• https://bitbucket.org/osrf/drcsim/pull-requests/532/updated-inertia-from-issue-487/diff
• https://bitbucket.org/osrf/drcsim/pull-requests/191/fixes-to-atlas-limits-and-inertias/diff
• https://bitbucket.org/osrf/drcsim/issues/487/mass-properties-for-the-atlas-unplugged-v4
• https://bitbucket.org/osrf/drcsim/pull-requests/490/use-dynamic-moi-rescaling/diff

In particular, there are a few interesting options related to Gazebo-specific modifications that are worth checking out. The only problem is those options seems to be global to all the ode simulation, and this would mean that a iCub model spawned in a default Gazebo run would be unstable, that is clearly undesirable.

[nunoguedelha]

Replying to https://github.com/robotology/icub-gazebo/issues/22#issuecomment-311278364 by @traversaro :

: ( .
For which links did you had to increase the inertia?

The modified links are l_ankle_1 l_hip_2 r_ankle_1 r_hip_2 :

   <link name="r_hip_2">
       <mass value="0.4194"/>
-      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
+      <inertia ixx="0.00048916" ixy="-2.70159e-07" ixz="1.85233e-07" iyy="0.00028697" iyz="1.28773e-05" izz="0.000315599"/>

   <link name="r_ankle_1">
       <mass value="0.6803"/>
-      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
+      <inertia ixx="0.000405201087095" ixy="7.4060158032e-07" ixz="-3.03314661581e-05" iyy="0.000449108136626" iyz="2.88975725979e-06" izz="0.000310063350592"/>

   <link name="l_hip_2">
       <mass value="0.421161"/>
-      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
+      <inertia ixx="0.000494475" ixy="1.54783e-07" ixz="-1.86267e-07" iyy="0.000288821" iyz="1.53903e-05" izz="0.000319114"/>

   <link name="l_ankle_1">
       <mass value="0.6798"/>
-      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
+      <inertia ixx="0.000405061299357" ixy="-1.12515439246e-06" ixz="3.06763770037e-05" iyy="0.00044834556253" iyz="2.88992928055e-06" izz="0.000309415719323"/>

I didn't apply your "stretch" rule for now, just the quick solution setting the diagonal matrix inertia you've used on the cub-gazebo models.

[traversaro]

Great, the number of links affected is limited!
I am glad to see that the shoulder links (the one for which I opened this issue: https://github.com/robotology/icub-gazebo/issues/22#issue-114402827) do not need any inertia modifications.

[traversaro]

Moving the issue to icub-model-generator, as it seems to me that is more a model-generation related issue.

[traversaro]

Moving the issue to icub-model-generator, as it seems to me that is more a model-generation related issue.

Hi Traversaro from 2017, you never this this apparently. Better to move it!