[cob_twist_controller] CollisionAvoidance constraint does not work with KinematicExtensions

[fmessmer]

The current implementation cannot be used when the kinematic chain is extended by selecting available KinematicExtensions (e.g. BaseActive, COB_TORSO or LOOKAT).

This is due to the fact that the constraint (at it's initialization) receives a KDL::Chain that only includes the (non-extended) main chain (e.g arm_left). This chain is used to initialize FKSolvers as well as JntToJacSolvers (for prediction) within CA.
However, this leads to dimension mismatches as soon as e.g. adjustedJacobian or adjustedJointStates are passed in due to active KinematicExtensions.

For now, whenever a KinematicExtension is activated, CA constraint is disabled!!!

In order to solve this a significant refactoring of the CA implementaiton is required

@fmessmer FYI

[fmessmer]

Also investigate - might be related:

• CA in Stack-of-Task-Solver vs. CA in GPM-Solver
• Phenomenon from TwistControl-Video-Shoot ("Move-TCP-into-Obstacle"), TCP cannot be moved away from obstacle anymore..."gets stuck at obstacle" (?)

[fmessmer]

idea would be to "shrink"/"cut" the full Jacobian to only contain the DoFs of the "main chain", i.e. the first dof columns....this should work as extensions are always appended at the end (?)

[fmessmer]

Remove collision_check_link registration from dynamic_reconfigure callback - it always comes back "already registered" as it is not dynamic reconfigurable, thus it's unnecessary service calls

[ INFO] [1455987130.629360324, 25.749000000]: Parameters seem to be ok.
[ INFO] [1455987134.038055303, 26.282000000]: Trying to register for arm_left_3_link
[ INFO] [1455987134.312656164, 26.323000000]: DistanceManager::Registration: Element arm_left_3_link already registered.
[ INFO] [1455987134.330067638, 26.325000000]: Called registration service with success: 1. Got message: Element arm_left_3_link already registered.

[fmessmer]

#89 fixes

• the segfault (from wrong dimensions due to missing initialization)
• the superfluous service calls

As to the "Getting-Stuck"-Phenomenon, few more comments:

• The "Getting-Stuck"-Phenomenon might be related to the usage of prediction. In this branch prediction has been commented in order to test whether this improves anything. To be tested on HW.
• If prediction proves to be the reason - and cannot be fixed - quite some more can be cleaned up within the code...

However, the actual issue - i.e. use CollisionAvoidance together with KinematicExtensions - still exists! Few comments:

• Whether CA works together with KE seems not to be related to the solver being used (GPM, SoT and T2P). It's a problem within CA alone!
• The neck-breaker are the JntToJac-Solver and the ChainFkSolverVel passed from the InverseDifferentialKinematicsSolver in its constructor via the ConstraintSolverFactory. Those are configured/initalized with the chain for the main component only - thus additional DoFs from the KinematicExtension are not considered!
• Problem is also that e.g. for the Torso, the additional DoFs are __pre__pendend to the front while for e.g. BaseActive, the additional DoFs are _a_pended to the back.

[fmessmer]

For documentation:
#104 included an WIP-approach that intended to fix CA with BaseActiveExtension (untested!), #104 will be further investigated in #121