Regularization of the Hessian matrix

[aescande]

Once the quadratic matrices of all tasks have been aggregated, there is a piece of code attempting to make the resulting matrix positive definite.

I have several remarks on this:

• because the test is if(std::abs(Q(i, i)) < DIAG_CONSTANT), the code does not perform as documented, i.e. if Q(i, i) < -DIAG_CONSTANT, Q(i,i) is not made positive. This case should not arise unless someone directly provide a tasks::qp::Task with non positive Q. In my opinion, the code should be
  if(Q(i, i) < DIAG_CONSTANT) { Q(i, i) = DIAG_CONSTANT; }
• DIAG_CONSTANT should not be absolute, but scaled depending the magnitude of the matrix. Otherwise, if a user chose tasks with very small weights, they will be very disturbed by the regularization. From discussions I had today, some user would naturally chose small gains to express priority between tasks. Since this regularization is deep in the code (and I think not documented), this is a potential source of error/bad control.
• if one assume that all "tasks" are of the form ||A_i x + b_i||^2_{W_i} or ||x - xref||^2_{W_i}, then having Q positive definite is only a matter of having a non-zero diagonal element for unused DoFs. This has to be done only once after adding or removing a task.
• if one want to be more general and ensure that the matrix is really positive, then one could perform a rank-revealing L D L^T and render D positive. This will have barely no overhead if the solver is made to accept the factorized version of the Hessian, which both LSSOL and QLD accepts.
• of course, given that Tasks is quite low-level, one could also consider that it is the responsibility of the user to specify a well-posed problem. In particular, if the tasks are not using all DoFs, the user should add a damping term. In this case the regularization should be simply removed.

Any opinion on this? I can provide a patch easily for the 2 first points.