Include foot positioning into optimization problem to give the solver more control authority

[LouKordos]

Allow the solver to optimize the desired foot position as well, instead of using a predefined formula. This might be especially helpful when reacting to large, sudden disturbances, as the precise location of force exertion can be altered by the MPC.

This would require discretization in the NLP itself (at runtime), but @WPBack found a way of simplifying the discretization into a single multiplication, allowing everything but the discretized B matrix to be calculated outside the solver. Other integration methods might also be of interest.

There are still a bunch of issues that would come up, but this might be the most important hurdle in terms of solver time.
Another option would also be running a separate optimization problem for the foot positioning to parallelize the computation.

[LouKordos]

It would also be necessary to limit the desired foot position based on the leg configuration, maybe by sweeping all possible configurations and generating a reachability meshgrid.