Issue while testing the model iCubGazeboV2_5 with Yoga demo
Closed this issue · 14 comments
I'm testing the model iCubGazeboV2_5 with the yoga controller.
After increasing the damping parameter from 1 to 2 for all the torque controlled joints, and after a bit of gain tuning, the robot was able to perform the yoga demo while balancing on left foot:
However, with the same tuning the robot was not able to perform the yoga demo while balancing on the right foot:
I suspect there might be an issue related with the iCubGazeboV2_5 model.
Hi @gabrielenava , people on github don't have access to your private repository.
Please copy&paste the relevant text in the issue, thanks.
Done :-)
I am not able to see the attached GIFs.
@traversaro updated. Something strange happened while cutting the second gif, I got errors every time I tried to cut it and even with different software, but the bad behavior is visible anyway.
I was having a look at this issue.
We have increased the inertia of some links in the GAZEBO model, probably even too much. In the following picture a comparison of a model with and without increase inertias.
I am starting to realize that perhaps increasing the inertia and the damping is just a wrong strategy, perhaps we should just decrease the Gazebo timestep (or change other simulation parameters) until the simulation is stable.
After increasing the damping parameter from 1 to 2 for all the torque controlled joints, and after a bit of gain tuning, the robot was able to perform the yoga demo while balancing on left foot:
Did you try also with the low inertia model shown in this comment?
As suggested by @traversaro, you may also try to decrease the timestep
I tried only the model that is linked in the first comment, i.e. https://github.com/robotology-playground/icub-models/tree/master/iCub/robots/iCubGazeboV2_5
With @gabrielenava we are going to perform the following checks:
- check the link masses and inertia in the CAD model used to generate the URDF
- check the damping parameters in the SimMechanics to URDF parameter files
- try to run again the simulation to see if this behavior is persistent
-
try to run again this simulation reducing thetimestep, as suggested by @traversaro -
try to run again this simulation using the model without increased inertia reducing thetimestep, as suggested by @traversaro
try to run again this simulation reducing the timestep, as suggested by @traversaro
try to run again this simulation using the model without increased inertia reducing the timestep, as suggested by @traversaro
Useful Gazebo tutorials (one of it not merged unfortunately):
check the link masses and inertia in the CAD model used to generate the URDF
| PART (URDF name) | MASS [Kg] | PRINCIPAL MOMENTS OF INERTIA [Kg mm^2] |
|---|---|---|
| r_sole | 4.7299444e-01 | 3.4657330e+02 1.6062101e+03 1.8418494e+03 |
| l_sole | 4.7548248e-01 | 3.4729760e+02 1.6183731e+03 1.8539088e+03 |
| r_ankle_2 | 3.4756778e-01 | 2.1453064e+02 3.9913000e+02 5.2516944e+02 |
| l_ankle_2 | 3.4757026e-01 | 2.1459399e+02 3.9932633e+02 5.2542722e+02 |
| r_ankle_1 | 1.1114447e+00 | 5.7160146e+02 7.5668717e+02 8.2602702e+02 |
| l_ankle_1 | 1.1114106e+00 | 5.7145617e+02 7.5699060e+02 8.2625387e+02 |
| r_lower_leg | 3.9015594e+00 | 5.7325218e+03 1.2822647e+04 1.3690962e+04 |
| l_lower_leg | 3.9227599e+00 | 5.7934437e+03 1.2864165e+04 1.3732730e+04 |
| r_upper_leg | 4.6628968e+00 | 7.0184081e+03 1.5130033e+04 1.5253161e+04 |
| l_upper_leg | 4.6716307e+00 | 7.0268646e+03 1.5154276e+04 1.5297280e+04 |
| r_hip_3 | 1.4080568e-01 | 3.4714747e+01 3.4736729e+01 6.1841208e+01 |
| l_hip_3 | 1.4080568e-01 | 3.4714747e+01 3.4736729e+01 6.1841208e+01 |
| r_hip_2 | 5.1017350e-01 | 3.8081167e+02 4.6768209e+02 7.2176516e+02 |
| l_hip_2 | 5.1201071e-01 | 3.8194965e+02 4.6917524e+02 7.2404969e+02 |
| r_hip_1 | 1.1267950e+00 | 5.7083941e+02 8.3780309e+02 8.5354902e+02 |
| l_hip_1 | 1.1243041e+00 | 5.6970686e+02 8.3428708e+02 8.5073955e+02 |
check the damping parameters in the SimMechanics to URDF parameter files
Currently the damping is set through the CMake variable @LOWERBODY_JOINT_DAMPING@ which is the same for all the lower body joints, and as a consequence damping is symmetric for the left and right legs.
It seems that the issue was due to the Simulink control gains. They were not symmetric for the yoga on the left leg and on the right leg:
% % TORSO %% LEFT ARM %% RIGHT ARM %% LEFT LEG %% RIGHT LEG %%
Gain.impedances = [10 30 20, 10 10 10 8, 10 10 10 8, 30 30 20 20 100 100, 30 50 30 60 100 100 % state == 1 TWO FEET BALANCING
10 30 20, 10 10 10 8, 10 10 10 8, 30 30 20 20 100 100, 30 50 30 60 100 100 % state == 2 COM TRANSITION TO LEFT
10 30 20, 10 10 10 8, 10 10 10 8, 30 50 30 60 100 100, 30 30 20 20 100 100 % state == 3 LEFT FOOT BALANCING
30 30 30, 10 10 10 10, 10 10 10 10,100 200 100 400 100 100,100 50 30 100 100 100 % state == 4 YOGA LEFT FOOT
30 30 30, 5 5 10 10, 10 10 20 10,200 250 20 20 10 50,220 350 120 200 65 100 % state == 5 PREPARING FOR SWITCHING
30 30 30, 10 10 20 10, 10 10 20 10,100 350 20 200 10 100,220 350 120 200 65 100 % state == 6 LOOKING FOR CONTACT
10 30 20, 10 10 10 8, 10 10 10 8, 30 50 60 30 5 5, 30 30 30 20 5 5 % state == 7 TRANSITION TO INITIAL POSITION
10 30 20, 10 10 10 8, 10 10 10 8, 30 50 60 30 100 100, 30 30 30 20 100 100 % state == 8 COM TRANSITION TO RIGHT FOOT
10 30 20, 10 10 10 8, 10 10 10 8, 30 30 20 20 100 100, 30 50 30 60 100 100 % state == 9 RIGHT FOOT BALANCING
30 30 30, 10 10 10 10, 10 10 10 10,100 200 100 400 100 100,100 50 30 100 100 100 % state == 10 YOGA RIGHT FOOT
30 30 30, 5 5 10 10, 10 10 20 10,200 250 20 20 10 50,220 350 120 200 65 100 % state == 11 PREPARING FOR SWITCHING
30 30 30, 10 10 20 10, 10 10 20 10,100 350 20 200 10 100,220 350 120 200 65 100 % state == 12 LOOKING FOR CONTACT
10 30 20, 10 10 10 8, 10 10 10 8, 30 30 20 20 100 100, 30 50 30 60 100 100];% state == 13 TRANSITION TO INITIAL POSITION
I made the gains symmetric and then the Yoga was qualitative very similar on both feet. I close this issue then.
cc @ahmadgazar