Create trunkAndLeftArmKinematics.ini and trunkAndRightArmKinematics.ini

[rsantos88]

ToDo: create trunkAndLeftArmKinematics.ini and trunkAndRightArmKinematics.ini to use with KDL

[rsantos88]

I'll assign this issue to you, @jgvictores (if you are agree) because I think it's very useful and interesting to have this files working as soon as possible and you should know how to do that

[jgvictores]

Sounds good. I've opened related #13

[jgvictores]

WIP on issue-13-trunk branch, currently 357e637.

Done pretty much by head using these as reference (pointing to commit ATOW):

• https://github.com/roboticslab-uc3m/teo-developer-manual/blob/5d9dc0ce1c074834ac8390b787c7132ee5f3a512/appendix/a-teo-diagrams.md#dh-parameter-table
• https://github.com/roboticslab-uc3m/teo-developer-manual/blob/5d9dc0ce1c074834ac8390b787c7132ee5f3a512/appendix/a-teo-diagrams.md#dh-coordinate-systems-drawing
• https://github.com/roboticslab-uc3m/teo-developer-manual/blob/5d9dc0ce1c074834ac8390b787c7132ee5f3a512/appendix/a-teo-diagrams.md#link-lengths

Adding testing required label. : )

[rsantos88]

Ok, I've been doing some tests:
Now, we run the BasicCartesianControl with this parameters:

yarpdev --device BasicCartesianControl --name /teoSim/trunkAndRightArm/CartesianControl --from /usr/local/share/teo-configuration-files/contexts/kinematics/trunkAndRightArmKinematics.ini --angleRepr axisAngle --robot remote_controlboard --local /BasicCartesianControl/teoSim/trunkAndRightArm --remote /teoSim/trunkAndRightArm

It works ok, no errors.

teo@teo-oliver:~$ yarp rpc /teoSim/trunkAndRightArm/CartesianControl/rpc:s
>>stat
Response: [ccnc] 7.19504487435053e-18 -0.34692 -0.220306 1.92367069372179e-16 1.5707963267949 9.61835346860895e-17
>>inv 7.19504487435053e-18 -0.34692 -0.220306 1.92367069372179e-16 1.5707963267949 9.61835346860895e-17
Response: 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0

With the right elbow rotated 90 degrees, this is the result:

>>stat
Response: [ccnc] 0.386992263498608 -0.34692 0.125097801050256 2.44666614971685e-16 0.113446401379631 6.94658861621753e-18
>>inv  0.386992263498608 -0.34692 0.125097801050256 2.44666614971685e-16 0.113446401379631 6.94658861621753e-18
Response: 0.0 0.0 0.0 0.0 0.0 83.5 0.0 1.41562597847745e-13

If I try to use movj with this values, this is the final pose of the robot:

And the inverse kinematic is:

>>inv  0.386992263498608 -0.34692 0.125097801050256 2.44666614971685e-16 0.113446401379631 6.94658861621753e-18
Response: -18.4847513039382 -17.4710327794713 25.8363553378364 4.48498792884712 -37.501977875285 19.494286664461 25.3048920057904 22.8876940117882

[jgvictores]

FK of trunkAndRightArmKinematics.ini returns the same results of rightArmKinematics.ini, so taking as valid. IK goes out of joint limits, will open new issue.

Will continue with trunkAndLeftArmKinematics.ini.

[jgvictores]

WIP on issue-13-trunk branch, currently 3f09520 with trunkAndLeftArmKinematics.ini.

[jgvictores]

Turns out trunkAndRightArmKinematics.ini was merged at cf9bb81 as part of #13 duplicate I created.

• Closing the duplicate
• Also removing the testing required as this has been tested (well, we do know it is dangerous and not thread-safe)

[jgvictores]

Branch with trunkAndLeftArmKinematics.ini rebased and merged at 89d6e27.

[jgvictores]

PS: Obviously will be reviewed at roboticslab-uc3m/teo-main#38