Updated kinematic and mass model for Centauro

[alaurenzi]

I have integrated the new data for Centauro's lower body from @Lorentz (branch name updated_mass). This contains:

• different frame definitions for most of the leg links, so as to improve the uniformity w.r.t. the other centauro-like robots
• slightly more precise mass values for all leg links
• substantial mass increase for the pelvis and torso_yaw links

Note that the upper body is already shared with Pholus and Nexus and, therefore, is correct according to its creator @Lorentz.

This model will become the default after a period of testing!

The new model is tested to be practically identical to the old one in terms of kinematics by an included script which checks that our "control frames", i.e. wheel_X and contact_X behave exactly as before w.r.t. the base_link frame. The output confirms that errors are in the range of 1-2 mm (could probably be zeroed by slightly modifying some of the frame definitions, but for now I keep the errors).

Old mass = 95.02240268 kg, new mass = 112.51670648 kg

Old COM = [ 1.07737153e-01  9.07127697e-05 -2.12252207e-02] m, new COM = [ 0.09225258  0.00097502 -0.02227371] m

Checking joint names... 

Checking joint limits... 
Joint limits max error is 0

Checking links... 

Checking poses... 
\>\>\> Pose mismatch in link "wheel_1": error 0.00127549199408 m, 180.0 deg
Old: 
translation: [-0.2296,  0.4535,  0.1473]
rotation   : [ 0.8356, 0.01218, -0.5229, -0.1681]
New: 
translation: [-0.2284,  0.4541,  0.1477]
rotation   : [ 0.1681,  0.5229, 0.01218,  0.8356]
\>\>\> Twist mismatch in link "wheel_1": error is 0.0023669615616
Old: 
[ 0.24693362  0.69794366  0.3112792   1.66951913 -1.39402297 -1.35250213]
New: 
[ 0.24761848  0.70009657  0.31057317  1.66951913 -1.39402297 -1.35250213]
\>\>\> Pose mismatch in link "wheel_2": error 0.000145601370077 m, 179.999999146 deg
Old: 
translation: [-0.07366,  -0.1156,  -0.2556]
rotation   : [ 0.7455, -0.1025, -0.1989,  0.6278]
New: 
translation: [-0.07358,  -0.1156,  -0.2555]
rotation   : [ 0.1025,  0.7455, -0.6278, -0.1989]
\>\>\> Twist mismatch in link "wheel_2": error is 0.000857478463709
Old: 
[1.30253657 0.77610333 0.87646559 0.48620453 1.65305786 0.49752442]
New: 
[1.30168516 0.77610397 0.87636378 0.48620453 1.65305786 0.49752442]
\>\>\> Pose mismatch in link "wheel_3": error 0.000461621052344 m, 180.0 deg
Old: 
translation: [-0.4539, -0.4286, -0.3229]
rotation   : [ 0.1539, -0.4735, -0.2905,  0.8172]
New: 
translation: [ -0.454, -0.4285, -0.3233]
rotation   : [ 0.8172, -0.2905,  0.4735, -0.1539]
\>\>\> Twist mismatch in link "wheel_3": error is 0.000887070391555
Old: 
[ 0.7406322   0.05411322  1.66245409  1.32143126 -0.78636601  0.96555194]
New: 
[ 0.73986956  0.05381336  1.66279372  1.32143126 -0.78636601  0.96555194]
\>\>\> Pose mismatch in link "wheel_4": error 0.00147590709483 m, 179.999999146 deg
Old: 
translation: [0.1937,  0.181,   0.13]
rotation   : [  0.9483, -0.05941, -0.09304,  -0.2976]
New: 
translation: [0.1944, 0.1799, 0.1307]
rotation   : [ 0.05941,   0.9483,   0.2976, -0.09304]
\>\>\> Twist mismatch in link "wheel_4": error is 0.00248978198223
Old: 
[ 0.33216102  1.1998551   1.03250893  1.0704519   0.77022792 -2.22315579]
New: 
[ 0.3326195   1.20202097  1.03364813  1.0704519   0.77022792 -2.22315579]
\>\>\> Pose mismatch in link "contact_1": error 0.000849999998148 m, 179.999999146 deg
Old: 
translation: [-0.257, 0.4663, 0.2188]
rotation   : [ 0.7901,  0.5779, 0.09887,  0.1787]
New: 
translation: [-0.2566,  0.4671,  0.2188]
rotation   : [-0.5779,  0.7901, -0.1787, 0.09887]
\>\>\> Twist mismatch in link "contact_1": error is 0.000646273062275
Old: 
[ 0.11258006  0.7540715   0.21583005  0.85654766 -1.05590563 -1.82658446]
New: 
[ 0.11231446  0.75461703  0.21560751  0.85654766 -1.05590563 -1.82658446]
\>\>\> Pose mismatch in link "contact_2": error 0.00084999999898 m, 1.7075472925e-06 deg
Old: 
translation: [-0.01636,  -0.1346,  -0.3044]
rotation   : [-0.03504,  -0.4285,  -0.2261,   0.8741]
New: 
translation: [-0.01664,  -0.1354,  -0.3044]
rotation   : [-0.03504,  -0.4285,  -0.2261,   0.8741]
\>\>\> Twist mismatch in link "contact_2": error is 0.00062890571561
Old: 
[ 1.2997714   0.79282142  0.9237349   0.66716712  1.33363335 -0.43264789]
New: 
[ 1.30009189  0.79236111  0.92401937  0.66716712  1.33363335 -0.43264789]
\>\>\> Pose mismatch in link "contact_3": error 0.00084999999937 m, 180.0 deg
Old: 
translation: [-0.4671, -0.5022, -0.3438]
rotation   : [ -0.555, -0.2457, -0.4514,  0.6541]
New: 
translation: [-0.4679, -0.5021, -0.3438]
rotation   : [-0.2457,   0.555,  0.6541,  0.4514]
\>\>\> Twist mismatch in link "contact_3": error is 0.00119852801415
Old: 
[ 0.72515624 -0.0323389   1.72112153  1.30517027  0.02631228  0.38306625]
New: 
[ 0.72467124 -0.03327967  1.72055922  1.30517027  0.02631228  0.38306625]
\>\>\> Pose mismatch in link "contact_4": error 0.000849999994953 m, 0.0 deg
Old: 
translation: [0.2453, 0.2317, 0.1583]
rotation   : [ 0.8164, -0.1049, -0.3482,  0.4486]
New: 
translation: [0.2459, 0.2311, 0.1583]
rotation   : [ 0.8164, -0.1049, -0.3482,  0.4486]
\>\>\> Twist mismatch in link "contact_4": error is 0.00109253810891
Old: 
[ 0.25381502  1.24208232  1.04059492  0.11769627  0.59638085 -1.97408976]
New: 
[ 0.25443896  1.24267441  1.04126854  0.11769627  0.59638085 -1.97408976]

NOTE frames orientations are very different, but this should only matter in rare cases (i.e. where explicit references for the wheel full pose are hardcoded)

Tests

• [x] pose consistency check
• [x] poses v2
• [x] rviz markers (manipulation stack)
• [x] rviz markers (car frame stack)
• [ ] other?

Notes

The repo now takes care to generate the urdf/srdf for the "car frame" model as well. The repository centauro_cartesio must therefore be switched to the updated_centauro_model branch in order to work well with the new model.

@IoannisDadiotis @liesrock @DavideAntonucci @aled96 @torydebra @FrancescoRuscelli @lucarossini-iit

[lucarossini-iit]

Thank you very much Arturo! As soon as I will need to work again on Centauro I will test the new models but I can also anticipate these with some old codes of mine, if required.

[IoannisDadiotis]

Great news! Let's see what discrepancies will come out the following period.

@alaurenzi Are you going to make a new branch in the centauro_cartesio repo ?

[IoannisDadiotis]

Force Estimation at the feet EE expressed in the contact_* frames:

header: 
  seq: 33903
  stamp: 
    secs: 55
    nsecs: 553250000
  frame_id: "contact_1"
wrench: 
  force: 
    x: 9.71675950667
    y: -63.594618696
    z: 336.129544256
  torque: 
    x: 0.0
    y: 0.0
    z: 0.0

header: 
  seq: 34002
  stamp: 
    secs: 55
    nsecs: 677000000
  frame_id: "contact_2"
wrench: 
  force: 
    x: 9.622013685
    y: 63.7280763059
    z: 334.968351127
  torque: 
    x: 0.0
    y: 0.0
    z: 0.0

header: 
  seq: 33824
  stamp: 
    secs: 55
    nsecs: 454500000
  frame_id: "contact_3"
wrench: 
  force: 
    x: -0.414737415376
    y: -12.2772626401
    z: 198.559287592
  torque: 
    x: 0.0
    y: 0.0
    z: 0.0

header: 
  seq: 33603
  stamp: 
    secs: 55
    nsecs: 178250000
  frame_id: "contact_4"
wrench: 
  force: 
    x: -0.308877272759
    y: 12.4370218997
    z: 196.947961854
  torque: 
    x: 0.0
    y: 0.0
    z: 0.0

Based on the above: Sum of vertical forces = 1066.54 N corresponding mass = 108.7 kg

There is a deviation of 4 kg wrt to the new mass reported by Arturo.

Every foot in air exhibits zero estimated force, as expected.

[alaurenzi]

Maybe slight rotations of contact frames w.r.t. world frame can cause this

[IoannisDadiotis]

Real robot measured today was 111.5 kg (includes cameras, lidar)