I am getting started in the world of mpc and optimal control. I have used acados with hpipm recently for drone mpc with decent success and I want to use the framework in another closed loop control problem. My low-level understanding of the solver is still limited so I apologize in advance.
I would like to know if the following mpc problem is too much of a stretch for the target formulation of acados, or otherwise how you would implement the solver.
Let us assume we have a vehicle we want to drive to a certain position p_f with a certain non-zero terminal velocity v_f, at exactly time t_f. However, currently, my reference y_ref only contains the instantaneous values for p_f, v_f, t_f without any actual reference sequence. Assuming p(t) and v(t) are observable, is it possible to formulate such a problem in acados using linear least squares cost?
As my first intuition, I thought about using a terminal-cost only cost function. Note t_f remains fixed as the system evolves with time, so effectively the time horizon of the problem would shrink with each iteration.
After some thinking and trying to compile such a solver, I thought of other possible options:
(Not preferred) Implementing a reference generator and use a normal fixed time-horizon formulation
Setting tf as a problem parameter or a problem variable with equality constraints, such that I can choose the time horizon on-line without needing to re-compile the solver (does not seem possible with acados)
Using an extenal cost function defined in casadi syntax with parameters (is there any example for defining such a cost?), such that I can define a cost function unique for every shooting node
Doing a state augmentation of the sort p'(i) = d(i) * (p(i) - p_f) (and similar for v), where d(i) would be a dirac-style function for each node i, such that the cost is only defined at the shooting node closest to my time target. Since my state is 6 dimensional, probably this is not a very nice idea either.
As stated above, due to my problem circumstances I would prefer not to have an intermediate planning algorithm. Is any of the above options feasible with acados?
As my first intuition, I thought about using a terminal-cost only cost function. Note t_f remains fixed as the system evolves with time, so effectively the time horizon of the problem would shrink with each iteration.
I think this makes sense.
The number of shooting nodes is fixed in acados, but you can just change the weights of the cost term at each stage.
Using first zeros for the weights everywhere and your some weights at the terminal shooting node.
In a closed loop setting, you could then just set the terminal weights to zero and set the same weights at the shooting node N-1.
Does this make sense?
Thanks for the reply! Actually we started with the state agumentation idea and got a working solver, but we will definitely try your suggested approach - I didn’t know you could change the weights on-line and never tried it for some reason. It should be a much cleaner solution.
Regarding the paper - please go ahead. I am the first author (Guillem Torrente).
Thanks a lot for your response and your support of the Acados community.
Maybe some more context is needed to clarify our (@recksahr) problem setup. We are currently generating solvers by defining the optimal control problem in Python. This C code is subsequently used in a C++ code base. We would not be able to compile a new C object every time the cost is updated. From my current understanding of Acados, what you suggest doing seems to require recompiling the code (please correct me if I’m wrong).
Using parameters, we can directly modify some characteristics of the C object. By using ‘set’, it is possible to change problem variables such as [‘p’, ‘S_adj’, ‘T’, ‘x’, ‘u’, ‘xdot’, ‘z’]. Unfortunately, this is not the case for W or V. I haven’t found an equivalent setter that directly acts on the C object for the cost or selection matrices.
This made me think that using a parameter and augmenting the state with copies of the states we want to penalize would be the next best approach. In the dynamics function, one copies the states of interest and multiplies it with a parameter that can be used to set this state value to the desired value, as to effectively put the cost function to zero.
I do feel that this is a bit of a hack and that there should be a better way to tackle the problem. However, it is not clear to me how to achieve this without having to recompile the C object online.
Thanks a lot for your response. I implemented the suggested approach, and put both methods side by side. I came to the following observation. The computation time for the cold started problem with the “hacked” augmented state (let’s call this (1)) is ~70ms. The varying cost matrix problem (2) has >600ms computation time for the cold started solver. Warm started, both approaches are around 40ms. Let’s not go in much detail about the absolute numbers, but instead focus on the relative difference between the two methods.
This observation feels counterintuitive since the augmented approach (1) increases the size of the problem, and therefore I would also expect it to increase the solver time. The only thing I could come up with, is that the varying cost matrix formulation (2) does not exploit sparsity properties of the cost matrix, since in a general case, one can set all elements (aside from the ones on the main diagonal) to nonzero values. (1) does exploit this since it knows the elements of W will not be changed on the go. I do not have enough insight in the low-level implementation to confirm or deny this hypothesis; could you shed some light on the situation?
(2) does not exploit sparsity properties of the cost matrix, since in a general case, one can set all elements (aside from the ones on the main diagonal) to nonzero values.
This matrix is indeed Cholesky factorized without exploiting sparsity. But that does not explain the huge difference in timings you are reporting.
Did you measure those timings from C by getting them from the acados OCP solver?
What do you mean by warm starting?
The reported timings are measured in Python. Do you think the differences would be smaller in C++ when using the generated C code? (I can test this as well)
The warm started case is the case where we call the solver a second time on a very similar problem as the first run.
