Update Denavit-Hartenberg parameters of left and right iCubArm v2.x

[mfussi66]

Copying it from #758 since I accidentally closed that one.

---

The aim of this PR is to align the Denavit-Hartenberg parameters of left and right iCubArm v2.x with the values found in the CAD models.

Motivation

While comparing the iKin forward kinematics with the iDynTree one extracted from the .urdf model, a significant misalignment was found in the left and right arm's end effector's position.

The comparison was made with a task-specific application I developed, which instantiates an arm with iKin and iDynTree (which loads the .urdf file), and given a specific joints configuration, it prints out the forward kinematics transformation matrix. See the example below for the left arm.

iKin
-1.0000	-0.0087	0	-0.2297
0	0	-1	-0.08414
0.0087	-1.0000	0	0.009799
0	0	0	1

iDynTree
-1.0000	-0.0087	0	-0.2273
0	0	-1	-0.08434
0.0087	-1.0000	0	0.001105
0	0	0	1

The difference of the translational component is:

e_x = 0.00245053871179957
e_y = -0.000205222132508828
e_z = -0.00869452121972735

The misalignment was validated in MATLAB with Peter Corke's Robot toolbox

and with other joints configurations:

cfg	joint 1	joint 2	joint 3	joint4	joint 5	joint 6	joint 7	joint 8	joint 9	joint 10
qdes5	25	0	-9.6	9.45	160.80	79.56	19.005	0	0	0.6
qdes6	25	10	-9.6	9.45	160.80	79.56	19.005	0	0	0.6
qdes7	25	-15	-9.6	9.45	160.80	79.56	19.005	0	0	0.6
qdes8	-25	30	-9.6	9.45	160.80	79.56	19.005	0	0	0.6

Equivalent results were found for the right arm v2.x.

Contribution

The PR's contribution changes the kinematic parameters of iCubArm v2.x left and right, specifically:

• base matrix H0
• Denavit-Hartenberg parameters
• end effector matrix Hn

If-else conditions are added to keep the parameters of v1.x and v3.0 unaltered.

Result

The contribution was validated with the red ball test:

demoredball_crop.mp4