New pseudo-inverse IK solver with faster convergence, better numerical behaviour

[wxmerkt]

This PR replaces the linear-system-based IK with a weighted and regularised pseudo-inverse IK with adaptive regularisation and backtracking line-search. It further includes more stopping criteria. These changes result in much better convergence and numerical behaviour/conditioning. The implementation is performance-oriented and uses Cholesky decomposition for the matrix decomposition/inverse.

The solution the new solver returns are also better (read: closer to the original state, within joint limits).

Changes:

• It now implements a regularised and weighted Jacobian pseudo-inverse instead of solving a linear system.
• The support for NominalState regularisation by appending a nominal state to the error vector was removed.
• Adaptive regularisation and backtracking line-search result in better convergence and numerical behaviour.
• Use of pre-allocation and Cholesky decomposition makes the solver fast :-).
• MaxStep for interactive IK is still supported, however, only when MaxIterations == 1.
• example_ik now converges in 6 iterations, previously 43.
• Turns out the old implementation had a bug when Regularisation was on (C>0.0) - it overwrote the final value of the task error with an erroneously oversized regularisation matrix - this is now also resolved as the old code has been replaced.

Here's a plot of the behaviour on the LWR example (example_ik) for the first target - before and after:

And corresponding convergence logs:

Before

iter     cost          stop         grad          reg    step
   0  3.22242e+00  2.90930e-01  7.82000e+00  0.00000e+00  1.0000
   1  1.83584e+00  7.36646e-01  1.04546e+01  0.00000e+00  1.0000
   2  2.00144e+00  8.53389e-01  1.11840e+01  0.00000e+00  1.0000
   3  1.60733e+00  3.97460e-01  1.35734e+01  0.00000e+00  1.0000
   4  1.00728e+00  5.74187e-01  1.54258e+01  0.00000e+00  1.0000
   5  6.81838e-01  6.21559e-01  1.82345e+01  0.00000e+00  1.0000
   6  6.56756e-01  6.22272e-01  2.07452e+01  0.00000e+00  1.0000
   7  7.08747e-01  6.26042e-01  2.29382e+01  0.00000e+00  1.0000
   8  7.94282e-01  6.37213e-01  2.47424e+01  0.00000e+00  1.0000
   9  8.70059e-01  6.25281e-01  2.60146e+01  0.00000e+00  1.0000
iter     cost          stop         grad          reg    step
  10  8.00613e-01  1.08680e-01  2.64694e+01  0.00000e+00  1.0000
  11  5.56834e-01  1.28919e-01  2.65169e+01  0.00000e+00  1.0000
  12  3.64317e-01  1.30440e-01  2.66567e+01  0.00000e+00  1.0000
  13  2.35836e-01  1.16878e-01  2.68654e+01  0.00000e+00  1.0000
  14  1.53387e-01  1.02169e-01  2.70985e+01  0.00000e+00  1.0000
  15  1.00689e-01  8.80847e-02  2.73241e+01  0.00000e+00  1.0000
  16  6.68940e-02  7.48868e-02  2.75245e+01  0.00000e+00  1.0000
  17  4.50687e-02  6.28797e-02  2.76925e+01  0.00000e+00  1.0000
  18  3.08148e-02  5.22850e-02  2.78275e+01  0.00000e+00  1.0000
  19  2.13667e-02  4.31689e-02  2.79326e+01  0.00000e+00  1.0000
iter     cost          stop         grad          reg    step
  20  1.49974e-02  3.54708e-02  2.80121e+01  0.00000e+00  1.0000
  21  1.06302e-02  2.90559e-02  2.80706e+01  0.00000e+00  1.0000
  22  7.58925e-03  2.37589e-02  2.81125e+01  0.00000e+00  1.0000
  23  5.44455e-03  1.94113e-02  2.81416e+01  0.00000e+00  1.0000
  24  3.91706e-03  1.58565e-02  2.81610e+01  0.00000e+00  1.0000
  25  2.82167e-03  1.29565e-02  2.81731e+01  0.00000e+00  1.0000
  26  2.03275e-03  1.05932e-02  2.81800e+01  0.00000e+00  1.0000
  27  1.46328e-03  8.66793e-03  2.81833e+01  0.00000e+00  1.0000
  28  1.05193e-03  7.09906e-03  2.81839e+01  0.00000e+00  1.0000
  29  7.54924e-04  5.81987e-03  2.81828e+01  0.00000e+00  1.0000
iter     cost          stop         grad          reg    step
  30  5.40738e-04  4.77599e-03  2.81807e+01  0.00000e+00  1.0000
  31  3.86543e-04  3.92328e-03  2.81780e+01  0.00000e+00  1.0000
  32  2.75757e-04  3.22599e-03  2.81749e+01  0.00000e+00  1.0000
  33  1.96331e-04  2.65515e-03  2.81718e+01  0.00000e+00  1.0000
  34  1.39514e-04  2.18731e-03  2.81687e+01  0.00000e+00  1.0000
  35  9.89587e-05  1.80346e-03  2.81658e+01  0.00000e+00  1.0000
  36  7.00715e-05  1.48818e-03  2.81631e+01  0.00000e+00  1.0000
  37  4.95371e-05  1.22896e-03  2.81606e+01  0.00000e+00  1.0000
  38  3.49679e-05  1.01561e-03  2.81584e+01  0.00000e+00  1.0000
  39  2.46494e-05  8.39858e-04  2.81564e+01  0.00000e+00  1.0000
iter     cost          stop         grad          reg    step
  40  1.73536e-05  6.94949e-04  2.81547e+01  0.00000e+00  1.0000
  41  1.22029e-05  5.75372e-04  2.81532e+01  0.00000e+00  1.0000
[EXOTica]: [IKSolver] Reached tolerance (0.0000 < 0.0000)
Solution found in 0.0003771s [ 2.73987663  0.69071349 -2.75179532  1.28968653 -0.26322913  0.36207069   
0.        ] in 43 iterations

After

iter     cost          stop         grad          reg    step
   0  3.22242e+00  8.23631e-01  2.60311e+00  1.00000e-09  1.0000
   1  1.63402e-01  1.51001e+00  2.65507e+00  1.00000e-09  0.4000
   2  1.14527e-01  4.73361e-01  2.62015e+00  1.00000e-09  1.0000
   3  1.11041e-02  1.49634e-01  2.62473e+00  1.00000e-09  1.0000
   4  5.05987e-04  5.23253e-03  2.65237e+00  1.00000e-09  1.0000
   5  1.25895e-06  5.23253e-03  2.65237e+00  1.00000e-09  1.0000
[EXOTica]: [IKSolver] Reached absolute function tolerance (0.0000 < 0.0000)
Solution found in 0.000144965s [-0.40312432 -0.69274757  0.39142517  1.28995832 -0.26320049  0.36257304   
0.        ] in 6 iterations

[wxmerkt]

Here is an overview of what got changed by this pull request:

Clones removed
==============
+ exotations/solvers/exotica_ik_solver/src/ik_solver.cpp  -2

See the complete overview on Codacy

[VladimirIvan]

Could you add this as a new solver? The linear system solver may still be good for teaching purposes.

[wxmerkt]

Sure, I will need to fix the memory overwriting bugs in the old linear system solver though. Do we want adaptive regularisation and line-search in the old solver? It did make some convergence increase (~10 iterations faster).

I initially had a new version of the linear system solver before changing to pseudo-inverse... but didn't commit it as this one performed a lot better. So will have to recreate.

[wxmerkt]

Actually, for teaching purposes the LevenbergMarquardtSolver solves a linear system. This solver is now aligned with the RSS teaching slides by using the pseudo-inverse.