[cob_twist_controller] GPM and SelfMotions when close to singular configurations

[fmessmer]

Moving this issue from #53 into a separate issue

Discussion was stared in this comment.
So far, the possibility to use both damped inverse and undamped inverse during the GPM projection has been implemented (see https://github.com/ipa320/cob_control/issues/53#issuecomment-142896187). As first mathematical verifications (i.e. doing Forward kinematics) proved to be SelfMotions, indeed.

However, while discussing it with @ipa-fxm-mb as well as due to results from the octave scripts the argument war raised that the verification might only be correct for cases wher damping is low...i.e. far away from singular configuration.

Thus, this phenomenon needs to be investigated further in situations where GPM is used close to singular configurations.

• re-verify that self-motions are self-motions in case damping is significant - use constant damping (See discussion with @ipa-fxm-mb and respective octave scripts
• apply fixes to all solvers (i.e. GPM, StackOfTasks, TaskPriority)

[fmessmer]

Testing this with the following setup:
roslaunch cob_twist_controller example.launch
Using GPM-solver, CA_OFF, JLA_OFF and faking q_dot_0 to be Eigen::MatrixXd::Ones(projector.cols(), 1) (in order to not introduce any relation to one of the constraints - as nullspace projection is supposed to work for any q_dot_0).
For testing, I modified the GPM solve() function to compute both variants, i.e. homogeneous_solution using the inverse of both the damped and the pure kinematic Jacobian.
To trigger a Zero-Twist (for the particular solution!), I use

rostopic pub /arm/twist_controller/command_twisgeometry_msgs/Twist "linear:
  x: 0.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.0" -r50

Once running, I modify the damping from NO_DAMPING to CONSTANT damping with values 0.01, 0.1 and 0.5.
In the NO_DAMPING case, both variants produce the same homogeneous_solution, i.e. an actual nullspace motion. However, when using CONSTANT 0.5, a significant difference between resultingCartVelocities and resultingCartVelocities_undamped can be seen:

[ WARN] [1452942971.204256475]: Faking q_dot_0
[ INFO] [1452942971.204454133]: particular_solution: 0 , 0 , 0 , 0 , 0 , 0 , 0 , 
[ INFO] [1452942971.204536751]: qdot0: 1 , 1 , 1 , 1 , 1 , 1 , 1 , 
[ INFO] [1452942971.204621680]: homogeneous_solution: 0.0588235 , 0.0155903 , 0.0588235 , 0.0804587 , 0.0588235 , 0.123161 , 0.0588235 , 
[ INFO] [1452942971.204981179]: resultingCartVelocities: -0.035752 , 1.30398e-16 , 1.11331e-16 , 4.90835e-16 , -0.21921 , 0.235294 , 
[ERROR] [1452942971.205062423]: ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
[ INFO] [1452942971.205206116]: homogeneous_solution_undamped: -2.77556e-17 , 2.58386e-28 , 5.55112e-17 , -2.22045e-16 , 0 , 5.55112e-17 , 0 , 
[ INFO] [1452942971.205313412]: resultingCartVelocities_undamped: 6.63025e-17 , 6.43435e-32 , -2.57374e-31 , -8.08064e-32 , 1.66533e-16 , 2.77556e-17 , 
[ INFO] [1452942971.205384234]: ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
[ INFO] [1452942971.205445979]: ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

Also, when using the nullspace projection calculated with the damped Jacobian, the robot starts to drift away...so in cases with high damping, nullspace motions were no actual nullspace motion when using the damped jacobian during the computation of the homogeneous solution.

Thus, I am going to modify the respective solvers to use the undamped, pure kinematic Jacobian within the nullspace projection!

[fmessmer]

should be solved in #78, but needs more testing on hardware and with constraints