Sole mismatch in iCubHeidelberg01 model

[traversaro]

[traversaro]

I compared the location of the F/T sensors in iCubHeidelberg01 model against the iCub2.5 model (link frame can change among different simmechanics model, but F/T location should be fixed):

traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubGenova02/model.urdf --frame-pose-reference-frame root_link --frame-pose l_leg_ft_sensor
root_link_H_l_leg_ft_sensor is 
-1 -9.79318e-12 -4.89659e-12 0.007309 
-9.79318e-12 1 4.89659e-12 -0.0701 
4.89659e-12 4.89659e-12 -1 -0.194213 
0 0 0 1 

traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubGenova02/model.urdf --frame-pose-reference-frame root_link --frame-pose r_leg_ft_sensor
root_link_H_r_leg_ft_sensor is 
-1 -9.79318e-12 -4.89659e-12 0.007309 
-9.79318e-12 1 4.89659e-12 0.0701 
4.89659e-12 4.89659e-12 -1 -0.194213 
0 0 0 1 

traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubGenova02/model.urdf --frame-pose-reference-frame root_link --frame-pose l_foot_ft_sensor
root_link_H_l_foot_ft_sensor is 
-1 -9.79318e-12 -9.5907e-23 0.010809 
-9.79318e-12 1 9.79318e-12 -0.0701 
3.46189e-28 9.79318e-12 -1 -0.615738 
0 0 0 1 

traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubGenova02/model.urdf --frame-pose-reference-frame root_link --frame-pose r_foot_ft_sensor
root_link_H_r_foot_ft_sensor is 
-1 -9.79318e-12 -9.5907e-23 0.010809 
-9.79318e-12 1 9.79318e-12 0.0701 
3.46189e-28 9.79318e-12 -1 -0.615738 
0 0 0 1 

traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubHeidelberg01/model.urdf --frame-pose-reference-frame root_link --frame-pose l_leg_ft_sensor
root_link_H_l_leg_ft_sensor is 
-1 4.92949e-12 -2.06822e-13 0.0068098 
4.92949e-12 1 4.89648e-12 -0.0701 
2.06822e-13 4.89648e-12 -1 -0.194218 
0 0 0 1 

traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubHeidelberg01/model.urdf --frame-pose-reference-frame root_link --frame-pose r_leg_ft_sensor
root_link_H_r_leg_ft_sensor is 
-1 -9.79318e-12 -4.89659e-12 0.007309 
-9.79318e-12 1 4.89659e-12 0.0701 
4.89659e-12 4.89659e-12 -1 -0.194213 
0 0 0 1 

traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubHeidelberg01/model.urdf --frame-pose-reference-frame root_link --frame-pose l_foot_ft_sensor
root_link_H_l_foot_ft_sensor is 
-1 6.59229e-14 -4.33774e-12 0.00745298 
6.59229e-14 1 9.79296e-12 -0.0739 
4.33774e-12 9.79296e-12 -1 -0.619157 
0 0 0 1 

traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubHeidelberg01/model.urdf --frame-pose-reference-frame root_link --frame-pose r_foot_ft_sensor
root_link_H_r_foot_ft_sensor is 
-1 -4.89671e-12 -2.40786e-05 0.0108076 
-4.89671e-12 1 -2.06941e-13 0.0739 
2.40786e-05 -2.06823e-13 -1 -0.619139 
0 0 0 1 

[traversaro]

From the check, it seems that the error is in the left leg.

[traversaro]

Note that as the frame orientation of the root_link is constant across models, a quick workaround would be to copy all the left leg joint links from a model such as https://github.com/robotology-playground/icub-model-generator/blob/master/generated/robots/iCubGenova02/model.urdf and copy it in the iCubHeidelberg model .

Actually I just remember that the masses of the links of the legs of iCubHeidelberg01 model and of the iCubGenova02 models are probably different, so if you copy the legs copy both of them.

cc @hu-yue @KevinAStein

[traversaro]

Interestingly, there seems to be no simmetry problems in the coms :

traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubHeidelberg01/model.urdf --link-com-frame root_link --link-com root_link
The COM of link root_link with respect to frame root_link is  x 0.0251137 y 0.000104218 z -0.0437451 m . 
traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubHeidelberg01/model.urdf --link-com-frame root_link --link-com root_link
The COM of link root_link with respect to frame root_link is  x 0.0251137 y 0.000104218 z -0.0437451 m . 
traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubHeidelberg01/model.urdf --link-com-frame root_link --link-com r_hip_1
The COM of link r_hip_1 with respect to frame root_link is  x 0.0069903 y 0.0640681 z -0.119953 m . 
traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubHeidelberg01/model.urdf --link-com-frame root_link --link-com r_hip_2
The COM of link r_hip_2 with respect to frame root_link is  x 0.0078306 y 0.0700965 z -0.164191 m . 
traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubHeidelberg01/model.urdf --link-com-frame root_link --link-com l_hip_1
The COM of link l_hip_1 with respect to frame root_link is  x 0.0069903 y -0.0640681 z -0.119953 m . 
traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubHeidelberg01/model.urdf --link-com-frame root_link --link-com r_hip_1
The COM of link r_hip_1 with respect to frame root_link is  x 0.0069903 y 0.0640681 z -0.119953 m . 
traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubHeidelberg01/model.urdf --link-com-frame root_link --link-com r_hip_2
The COM of link r_hip_2 with respect to frame root_link is  x 0.0078306 y 0.0700965 z -0.164191 m . 
traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubHeidelberg01/model.urdf --link-com-frame root_link --link-com l_hip_2
The COM of link l_hip_2 with respect to frame root_link is  x 0.0078306 y -0.0700965 z -0.164191 m . 
traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubHeidelberg01/model.urdf --link-com-frame root_link --link-com l_hip_3
The COM of link l_hip_3 with respect to frame root_link is  x 0.007432 y -0.0698177 z -0.202931 m . 
traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubHeidelberg01/model.urdf --link-com-frame root_link --link-com r_hip_3
The COM of link r_hip_3 with respect to frame root_link is  x 0.007432 y 0.0698177 z -0.202931 m . 
traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubHeidelberg01/model.urdf --link-com-frame root_link --link-com r_upper_leg
The COM of link r_upper_leg with respect to frame root_link is  x 0.0040078 y 0.0701589 z -0.291359 m . 
traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubHeidelberg01/model.urdf --link-com-frame root_link --link-com l_upper_leg
The COM of link l_upper_leg with respect to frame root_link is  x 0.0040078 y -0.0701589 z -0.291359 m . 
traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubHeidelberg01/model.urdf --link-com-frame root_link --link-com l_lower_leg
The COM of link l_lower_leg with respect to frame root_link is  x 0.00485688 y -0.0762931 z -0.441196 m . 
traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubHeidelberg01/model.urdf --link-com-frame root_link --link-com r_lower_leg
The COM of link r_lower_leg with respect to frame root_link is  x 0.0048569 y 0.0762931 z -0.441196 m . 
traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubHeidelberg01/model.urdf --link-com-frame root_link --link-com r_foot
The COM of link r_foot with respect to frame root_link is  x -0.0230752 y 0.0793048 z -0.617984 m . 
traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubHeidelberg01/model.urdf --link-com-frame root_link --link-com l_foot
The COM of link l_foot with respect to frame root_link is  x -0.0230752 y -0.0793048 z -0.617984 m . 
traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubHeidelberg01/model.urdf --link-com-frame root_link --link-com l_ankle_1
The COM of link l_ankle_1 with respect to frame root_link is  x -0.00785242 y -0.074761 z -0.558885 m . 
traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubHeidelberg01/model.urdf --link-com-frame root_link --link-com r_ankle_1
The COM of link r_ankle_1 with respect to frame root_link is  x -0.0078524 y 0.074761 z -0.558885 m . 
traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubHeidelberg01/model.urdf --link-com-frame root_link --link-com r_ankle_2
The COM of link r_ankle_2 with respect to frame root_link is  x 0.0065407 y 0.0739082 z -0.59704 m . 
traversaro@traversaro-VirtualBox:~/src/icub-model-generator/generated/robots$ idyntree-model-info -m iCubHeidelberg01/model.urdf --link-com-frame root_link --link-com l_ankle_2
The COM of link l_ankle_2 with respect to frame root_link is  x 0.00654068 y -0.0739082 z -0.59704 m . 

Perhaps the problem is only in the user added frames? The only other thing to check (i.e. that needs to be consistent across different models that described the same robot) would be the axis.

[traversaro]

Addenda to https://github.com/robotology-playground/icub-model-generator/issues/16#issuecomment-238510444 :

Actually I just remember that the masses of the links of the legs of iCubHeidelberg01 model and of the iCubGenova02 models are probably different, so if you copy the legs copy both of them.

[KevinAStein]

what do you mean with "user added frames"? which one would this be?

[hu-yue]

@traversaro And with different masses do you mean just that the distribution of the mass between the links is different or that the total is also different? If the total is different I guess a copy should not do as the actual weight of the robot is the same as the one that comes from the urdf.

[traversaro]

@KevinAStein The "user added frame" refers to frames that are explicitly added to the CAD model in Creo, see http://wiki.icub.org/wiki/Creo_Mechanism_to_URDF#Add_additional_reference_frames .

@hu-yue : yes, I remember that. When extracting the model for the iCubGenova02 however the total mass did not matched the one of the actual robot, and so after some test on the disassembled parts a new set of a masses for some links was determined, info on this can be found in https://github.com/robotology-playground/icub-model-generator/blob/master/simmechanics/data/icub2_5/ConversionProcess.xlsx and in https://github.com/robotology-playground/icub-model-generator/blob/master/simmechanics/data/icub2_5/ICUB_2-5_BB_simmechanics_options.yaml#L369 .

I can search for more information on this on some internal emails, but our tests with such a model have been gave some good results, especially with respect to gravity compensation.

We need to understand if it make sense to bring this values also to the iCubHeidelberg01 model.

[hu-yue]

@traversaro I checked the files you linked. First of all (assuming that koroibot = iCubHeidelberg), I see that the leg masses in the table are not exactly the same I have in the iCubHeidelberg URDF (small variations), the total is therefore also a little different (7.0712 in iCubHeidelberg). I also see that in the xlsx file there are small differences between left and right legs masses, which do not apply to the iCubHeidelberg model, where the masses of left and right are exactly the same.

Then from the yaml options I see that the legs of the iCubGenova02 are about 1 kg lighter per leg, which amounts to a difference of about 2 kg in the model (and therefore with the actual robot), I think the difference is a bit too big for our methods, this might cause problems/inconsistent results in the model based ocp.

Of course we have never unmounted the robot's legs to weight them, so they might as well weight as the ones of iCubGenova02 and have more weight elsewhere, however if we just replace the legs with the ones from iCubGenova02 then there would be this 2 kg mismatch between the actual robot and the model.

[traversaro]

Yes, that it something that always puzzled me. I wonder if the root_link or chest of the iCubHeidelberg01 is actually heavier that in the model, and thus this would explain the 2kg mismatch. I will try to recap something with mechanical guys tomorrow (unfortunately when all of this happened I was in the Netherlands, so I was not physically present during the weighting).

[hu-yue]

@traversaro ok thanks, I wait for your update then =)