I am currently implementing a mpc for my robot manipulator. Essentially i am tracking a reference frame (position and orientation).
I now added merit backtracking as globalization method since it seems to boost my time until convergence (e.g. specific thresholds are met) by a fair bit, especially when using a low number of solver iterations. However it seems like the control values that the solver produces oscillate when the endeffector frame comes close to the reference (see screenshot), like the solution becomes unstable when residuals become small. I do added penalization terms on the joint velocity, whichs effects can be seen in a first and fast reduction of the error and then converging slowly towards the target. However after a slowdown-period, the solver seems to escalate.
I did already encounter those problems before using merit backtracking and calmed them down by playing with lm-factor, nlp_step_size and nlp_solver_iter. But I cannot wrap my head around why this is happening? Could you possibly point me towards some explaination? I do understand that those are fairly little information and the possible reasons can be… quite diverse. But maybe you encountered this behaviour before and there is a quick fix!
It is hard to tell in general.
Is this a simulation with model mismatch?
I don’t see the u_ref in the first plot.
Did you check the residuals in the latter part of this simulation.
Is there some error in the solver, or is this actually a locally optimal solution to your OCP?
If you have a strong penalty on the joint velocity, this behavior surely doesnt seem optimal
first of all: thank you so much for your support and effort you put into this framework!
Yes, I indeed use a second order delay (PT2) plant simulation outside of acados to simulate my torque controlled robot behaviour on joint-level. However, even if I just use a (fairly low) first order delays (PT1), the same oscillation occure.
you cannot actually see u_ref since the simulated, delayed u_actual follows u_ref really fast. You can only see the mismatch when zoomed in.
I actually seem to have solved this problem by adapting my cost function for the position tracking.
I changed it from 1/2 * sqrt(p * p_ref) to 1/2 * (p*p_ref) which results ofc in a quadratic functional instead of a linear one. This seems to relax the solver and prevent oscillations.
