Implementing a new solver

Implementing a new solver

So far, you can only use qld or LSSOL with Tasks. However, if you want to implement a new solver, and if it manages to perform better than LSSOL it would be great!

Now, how to do this? You need to add two files, MyQPSolver.{cpp,h} in which you define a new class:

• MyQPSolver should derive from GenQPSolver
• The constructor, to hold all the necessary data
• updateSize() will be called every time the size of the problem changes. This only happens when the number of constraints changes. It happens for example when changing contacts, and when triggering the joint limits activation. You thus need to resize accordingly your inner data structures.
• updateMatrix() will give you the list of current tasks (objectives) and constraints.
• solve() should only solve the problem.
• result() should return the vector of variables.

Note that there will often be a lot of updateMatrix and solve() calls in between updateSize() calls, thus you should do all your allocation and expensive transformations at that time.

Then, to actually use your solver:

• You should edit the solver map in src/GenQPSolver.cpp (line 55), something like {"MySolver", allocateQP<MyQPSolver>}
• You can then use your solver by calling solver("MySolver") on your QPSolver instance
• Alternatively, you can also change the default solver in src/GenQPSolver.cpp (l. 41) to Ceres. This will also let the unit tests run using this solver.

Note that we are still thinking about how to make Tasks more easily expansible, so if you have any remarks/suggestions, feel free to ask :) I think that you might run into trouble with least-squares solvers here, as we usually manage directly Q and c matrices instead of relying on the actual jacobians of the linear forms.

Ceres

I do not know Ceres very well, but here are my thoughts:

• In your constructor, you probably need to create a Problem. You should also set the solver options here. As we are linear, one iteration of Ceres should be enough, with the trust region size set to maximum. This corresponds to saying that the second-order approximation is valid everywhere.
• Your cost function should return something like:

double* residual = new double[totalTaskSize];
for(const auto task : tasks)
{
    for(int i = curIndex; i < task.dim(); ++i)
    {
        residual[i] = task->weight * task->eval();
     }
     curIndex += task.dim();
}

• Then, you will need to compose in a similar way the jacobians to return the jacobian of the problem, that will roughly be the statcked, weighted jacobians (you can get it from task->jac()).
• Finally, if you can, your problem should return the (constant) hessian of the problem.
• The constraints should be handled similarly, but don't forget that we have various types of constraints:
  • Equality : A x = b
  • Inequality : A x <= u
  • GenInequality i.e. l <= A x <= u

For example, when manipulating SetPointTasks you will notice that:

Q = J^T * dimW * J
C = -J^T*W*(kp*eval() - kv*speed() - normalAcc)

For ceres to work, I think you have to return:

error = kp*eval() - kv*speed() - normalAcc
jac = J