Symmetry in iCubGenova04

georgwi commented 7 years ago

Hi @fiorisi and @gabrielenava!

When running simulations I noticed that the robot was not moving symmetrically. After looking into why this was the case I found that the model is not mirrored left to right. I am not sure if this was done on purpose to better reflect the real hardware and if this affects other iCub models also.

More specifically, I found that the same bodies for left and right arms and legs have different masses and different joint offsets. Here are two examples from the urdf:

<joint name="l_knee" type="revolute">
<origin xyz="-0.0135855 -0.145625 0" rpy="0 0 0"/>

<joint name="r_knee" type="revolute">
<origin xyz="-0.0096675 -0.145625 0" rpy="0 0 0"/>

<link name="l_upper_leg">
<mass value="1.8084"/>

<link name="r_upper_leg">
<mass value="1.8035"/>

My question is: is this expected or wanted? If so, would it be much work to provide a symmetric model for simulations? If not, do you plan of fixing this? In that case I can give you a full list of inconsistencies that I found.

Thanks for the feedback!

DanielePucci commented 7 years ago

CC @traversaro

fiorisi commented 7 years ago

Hello @georgwi, In general, the models are not symmetric because we adopt a semi-automatic procedure to extract the mass parameters of each link from the iCub CAD models. Sometimes the CAD model can have small variations (usually few grams like for the upper_leg) in the mass between the left and right links. This difference can be due to inaccuracies in the density of some components or can be due to some approximations during the generation of the equivalent link geometries (named "shrinkwrap" in the previous document).

Similarly, the joint axis and link origins are computed automatically, and depending on the axis that has been used to define the joint in the Mechanism model, we can have small differences between the left side and the right side. In this case anyway, we usually check that the origin of the joint is "shifted" along the joint axis and that the mass parameters of the child link (like CoM position) are "shifted" accordingly.

Since you say that there are some issues with the walking simulations we will check the model.

Generating a symmetric model is possible. One possibility is to create a symmetric Mechanism model and then run the generation script, but this might take some time. Maybe @traversaro can suggest other solutions.

If you already have the list it would be nice if you can send it to us, so we can check the inconsistencies. Otherwise, we will proceed anyway with the check.
Furthermore, if it possible, probably would be useful to have a video of the simulation where the robot is not moving symmetrically.

Thanks!

fiorisi commented 7 years ago

A quick check of the sdf. Seems that the origin shift is along the y axis:

<link name='l_lower_leg'>
      <pose frame=''>0.007309 -0.056515 -0.354438 1.5708 0 -1.5708</pose>

<link name='r_lower_leg'>
      <pose frame=''>0.007309 0.079768 -0.354438 1.5708 0 -1.5708</pose>

georgwi commented 7 years ago

Hey Luca!

Here is an example of the non-symmetric walk:

It is not very obvious but the robot stretches the left leg a little more during the gait. I have not confirmed that this is indeed caused by the model. But that is my best guess since there is nothing else that I think of that was non-symmetrical.

Excluding mass differences of less then 10g and joint translation differences of less then 1mm the list of discrepancies are:

l_ankle_2 and r_ankle_2 - joint offset l_ankle_1 and r_ankle_1 - joint offset l_lower_leg and r_lower_leg - joint offset l_forearm and r_forearm - joint offset l_elbow_1 and r_elbow_1 - joint offset

It seems like there are no mass differences that exceed 10g.

We can ignore this issue for now since you said this is expected on your side and I do not think it will influence out simulation results much.

fiorisi commented 7 years ago

Thanks @georgwi for the check on the masses and joints origins! We are currently checking the legs. We started from the CAD Mechanism model by testing the transform from the ROOT frame to the L/R_SOLE frames for 3 different configurations of the leg joints. As shown in the following subsections we changed the angular position of the joints and measured the transform. The vector q defines the joint angles (from hip to ankle).
The CAD Mechanism model seems to be accurate.

Concerning the URDF, @traversaro has already implemented a test to check the transform in the "zero configuration". Probably we can also add the tests with different joint angles.

q(0,0,0,0,20,20)

icublegstr

Transform from ROOT to R_SOLE

-0.939693   0.116978    -0.321394   27.9747
0.0000000000    -0.939693   -0.342020   92.0919
-0.342020   -0.321394   0.883022    -611.717

Transform from ROOT to L_SOLE

-0.939693   -0.116978   -0.321394   27.9747
0.0000000000    -0.939693   0.342020    -92.0919
-0.342020   0.321394    0.883022    -611.717

q(0,0,-40,-35,20,20)

icublegstr2

Transform from ROOT to R_SOLE

-0.439385   0.818643    -0.369817   119.185
-0.368688   -0.539759   -0.756789   192.683
-0.819152   -0.196175   0.538986    -553.335

Transform from ROOT to L_SOLE

-0.439385   -0.818643   -0.369817   119.185
0.368688    -0.539759   0.756789    -192.683
-0.819152   0.196175    0.538986    -553.335

q(45,30,-40,-35,20,20)

icublegstr3

Transform from ROOT to R_SOLE

-0.942669   0.267902    -0.199006   -135.910
0.0902830   -0.369357   -0.924891   392.971
-0.321285   -0.889833   0.323995    -421.704

Transform from ROOT to L_SOLE

-0.942669   -0.267902   -0.199006   -135.910
-0.0902830  -0.369357   0.924891    -392.971
-0.321285   0.889833    0.323995    -421.704

traversaro commented 7 years ago

The test mentioned by @fiorisi are the one in https://github.com/robotology-playground/icub-model-generator/blob/master/tests/icub-model-test.cpp#L321 .

georgwi commented 7 years ago

I have loaded the sdf in simulation and tried recreating your test. The transformations from the sole to the pelvis indeed match pretty well for left and right. If I check for some other part of the leg (e.g. the knee joint position) they do not match though. Here is an image that shows that the knee joint positions are not at the same position in the leg (the red arrows pointing out of the shin are the x-axes of the knee joint frames). The same goes for the ankles where the left ankle frame seems to be shifted forward in the foot.

Here is an idea that could explain why we have trouble and you do not: if you offset a joint along its own axis and then "undo" that offset in the transform to the next joint your end effector position does not change. However the joint position might move. E.g. if you have a joint at the origin rotating around the x axis and an end effector attached with an offset of [0.0, 0.0, 1.0] this would be equivalent of moving the joint along the x axis to [1.0, 0.0, 0.0] and changing the offset to the end effector to [-1.0, 0.0, 1.0]. If you do not use the location of your joint you will not see a difference.

Is this thought experiment correct @traversaro @fiorisi? We do use joint frames (esp. the ankle frame) for some things throughout the controller so that might be the issue. If this is indeed the case we have two options:

We stop relying on joint frames and only use end effector frames in the controller code (not sure how big of a refactor that would be). I will talk to a few people here and determine if this is something we could possibly fix on our side.
We change the model to be symmetrical at joint position level. We can fix the urdf/sdf manually and then decide if you could provide a new model. This might be a lot of work on your side and I am not sure if it would benefit other people using the model. If not, it might not be worth it.

fiorisi commented 7 years ago

Yes @georgwi you are correct, we checked only the end-effector, but if you use also the "intermediate" frames, then the "shift" of the joints origins along the joint axes is a problem. We can try to modify our source model such that the URDF has the same origins for the left and right leg joints. Not sure that it will work though. It should require one (or two days) to perform the changes in the CAD model and regenerate the URDF. We will do some test and let you know if it is working as soon as we have some results.

fiorisi commented 7 years ago

@georgwi the new iCubGenova04 model is ready. We did some preliminary tests to check the correctness of the model and seems to be accurate. Nevertheless, since we had to completely redesign the lower-body, we might need a preliminary debug phase. Let us know if you experience any issue.

georgwi commented 7 years ago

Hey @fiorisi Thank you very much for this fix! I updated our simulation model and checked it. The lower body is symmetrical and the simulations that I had trouble with before are now a lot better also.

I found during the symmetry check that for the *_wrist_prosup and the *_elbow joints the transformation to the the parent joint is still different for the left and right side. After reading your comment I am guessing that you did not update the upper body? That is fine with us I just wanted to confirm that you didn't miss those joints by accident.

fiorisi commented 7 years ago

Hello @georgwi, we are happy that the new model has solved the simulation issues.
For the moment we focused only on the lower body, but we will open an issue to work also on the upper body.
If it is fine for everyone we can close the issue.

robotology / icub-models