I am using acados in python to create a collision avoidance controller. I have found a weird result:
The value of the nonlinear lsq cost which is dependent on some parameters is not equal to the cost formula evaluated with the parameters.
I have a closed loop simulation where I solve my ocp, I have added the following prints:
print("defiition of the cost is ", ocp.model.cost_y_expr)
print("params definition is ", ocp.model.p)
print(" value of params = ", params)
print(f"cost function value = {ocp_solver.get_cost()}")
I have tried with two different cost function:
norm(u_ref)
1/(norm(u_ref)
When I look at what is being printed during the simulation for the first example, there is a weight which somewhat seems to have been added, as the norm should be 1 and yet the cost function value is 5.
I was wondering if there was a way of checking what the value of the parameters in the solver are, I am currently using the following command to set it:
for i in range(N):
ocp_solver.set(i, "p", params)
I was wondering if you might have ideas to understand better what was happening, as I am a bit lost on how to proceed.
the cost function is evaluated at the current iterate.
If the solver has previously exited with an error (as in your second screenshot).
It can of course be that evaluating the cost at the current iterate gives NaN or Inf.
Also, your cost function definition is not clear to me.
Defining the cost as norm(u_ref) would make me think that u_ref are parameters that you provide (reference values). Thus the cost would be constant and not dependent on the state and control trajectory within the solver.
Maybe you mean norm(u - u_ref) ?
The way you set the parameters seems correct though.
Note that the cost value is scaled with the time steps, i.e. c = t_0 c_0(x_0, u_0) + ... + t_{N-1} c_{N-1}(x_{N-1}, u_{N-1}) + c_{N}(x_{N})
where c denotes the total cost, c_i the cost term at shooting node with index i.
Regarding the first point, I had assumed that the fact that the cost is inf would raise an error status 3 and as the value of the cost is inf from the first iteration which triggers such error, I am trying to understand how come the value of the cost can be inf when to me it shouldn’t be.
Sorry the notation I have chosen are misleading, u_ref is the reference input but as I want to be able to customise the function and have multiple different ones, I have tried to do something similar to this:
Just to summarize:
In your setup, you get a QP solver error and an invalid SQP iterate.
The cost is then evaluated at this iterate.
If you want to report that the cost is not evaluated correctly, you have to take the iterate from the solver, and evaluate the cost at that iterate yourself.
This should match the cost value that acados reports.
From what I read here, this is the expected behavior.
