Allow blocking joint and/or dof

[jgvictores]

Here: https://github.com/roboticslab-uc3m/teo-configuration-files/blob/be671ec5b791d9681ba096f9767fe273dbec2d27/share/kinematics/leftArm3dofKinematics.ini we can see a hack for a 3 DoF solver. iCub iKin has a way to block joints in the API. This could be interesting, also for the 8 DoF case (trunk+arm) to avoid other hacks like https://github.com/roboticslab-uc3m/teo-configuration-files/blob/be671ec5b791d9681ba096f9767fe273dbec2d27/share/kinematics/rightArmKinematics-pan45-tilt30.ini for ironing.

Context: cleaning up teo-configuration-files at roboticslab-uc3m/teo-configuration-files#7 as part of reviewing TEO kinematic model at roboticslab-uc3m/teo-main#38

[jgvictores]

PS: Yet another case was https://github.com/roboticslab-uc3m/teo-configuration-files/blob/be671ec5b791d9681ba096f9767fe273dbec2d27/share/kinematics/leftArm22Kinematics.ini, where we used a single active joint to test GCMP.

[jgvictores]

Just to sum up use cases and solutions:

1. Ironing (most important use case): For ironing, we move TEO's trunk. Therefore, H0 must be set accordingly. At best BCC could be configured to dynamically read /teo/trunk for all computations (an idea issue could be open for this). For now, simply establishing the practice to init BCC via parameters and not end up with infinite .ini files for infinite trunk configuratios would be great.
2. Robot where they decided to expose arm and hand on the same port (e.g. iCub as far as I recall): This issue is solvable simply by depending on the number of joints established in our configuration files, and not relying on data from getAxes().
3. A 3 DOF solver: In this use case, we decide to implement a solver that uses 3 DOF to reach 3D positions, and only moves 3 selected joints, not caring about orientation. I guess we can postpone this and track its development on a separate issue.

[PeterBowman]

With KDL, it is actually possible to block certain joints via weighting matrices in certain differential IK solvers (internally used by some position IK solvers, so it's involved in IK computation), namely KDL::ChainIkSolverVel_wdls (for chains) and KDL::TreeIkSolverVel_wdls (for trees). The latter has been implemented in KdlTreeSolver and successfully tested, see #190 (comment).

See also roboticslab-uc3m/teo-configuration-files#28 regarding trunk+arm cartesian control.

[PeterBowman]

KDL::ChainIkSolverVel_wdls has been implemented at 09e823a, it accepts two additional weightJS and weightTS options in the same way as explained in the tree structure scenario at #190 (comment). To enable it, use --ikVel wdls (default is --ikVel pinv). The --ik option is now an alias for --ikPos.

PS This is probably something we might want to add tutorials for at teo-developer-manual.