[cob_twist_controller] Enhance extension_ratio and add "Upright" bias

[fmessmer]

Can we add some kind of an "upright"-bias that tries to keep components "upright"?
This would be really helpful for e.g. lookat, as sometimes "the torso is bent just to look at a goal that is just in front of cob", like so:

@fmessmer FYI

[fmessmer]

Related to this is the case when using BaseActive or Cob4Torso extension together with one of the arms:
In case the arm is almost stretched out (=close to singular configuration?), it is not possible to "push" the arm back into a more bended configuration (=higher manipulability). In such case the whole motion is executed by the base only (BaseActive) and the torso starts to move really wild (Cob4Torso)...

Can this be solved by some kind of bias or "dynamic extension_ratio" (in comparison to using a constant value for it)? E.g. calculate the extension_ratio as a function of some metric...

Maybe we also need to investigate again the Damping in case kinematic extensions are enabled! Don't know whether the damping is applied for the "extended kinematic" (arm + base/torso) or only "main component" (arm)