Defining a cost function with some terms not including model states ans inputs

[aabsz]

Hi,

I'm currently working on a problem where my cost function has some terms that aren't directly tied to the model inputs and states. Unfortunately, I can't define these terms using the standard state-space equation format (x_dot = Ax + Bu) to include them in the model.

Specifically, I'm tackling the issue of collision avoidance for a robot. My cost function consists of two components. The first component is related to achieving a desired goal, which can be defined using the model states. However, the second component is all about avoiding collisions with obstacles. To accomplish this, I need to maximize the distance between the robot and the obstacle, adding a term proportional to (1/dist) to the cost function. The challenge here is that this distance isn't defined as a model variable with a dynamical equation, causing the cost function to ignore it.

I'm open to any suggestions you might have on how to effectively incorporate these non-standard terms into my cost function and make them influence the optimization process. Your insights would be greatly appreciated.

[brunomorampc]

Hi,

I guess you know the position of the obstacle. I this case, you can add a distance function directly into the objective function, for example sqrt(x^2 + x_obs^2) where x-obs is the obstacle coordinate vector. Note that you need to use the generic objective function and you can't use the quadratic one.

If you want, you can define the obstacle position as a model parameter ( even though it does not enter the dynamic model) such that you can change its value online.
I hope this is clear, if not let me know and I'll provide a more detailed example.

[aabsz]

Hi again Bruno,

I've managed to resolve the previous issue. Now, I have another question:

Is it possible to define a cost function conditionally? For example, if the value of a parameter is less than a certain threshold, it would consider cost function A; otherwise, it would use cost function B.

[brunomorampc]

Hi @aabsz,

never tried it, but I think it should be possible by using "if_else" see : https://github.com/casadi/casadi/wiki/L_113.

I can imagine something like that:

mpc.stage_cost.cost = casadi.if_else(p>threshold, A, B)

let me know if it works (or if not :) )

[aabsz]

Hi Bruno,

Thank you for the advice.
Using the casadi.if_else() tool worked. I can define the conditional cost function now.

However, I have faced a new issue now. Due to a sudden change in the NMPC cost function, the controller stops working or it works very slowly and I get this error at each step.

Ups... NMPC had some problems. The ipopt error message is: error_in_step_computation. Please refer to ipopt documentation. For more info pass ipopt.print_level:5 to the solver_options in the NMPC.setup()

Is there any way to change the max iteration number or the type of the solver or other relevant changes to solve this problem? for example when defining the nmpc.setup()?

(Update: I was able to resolve the issue with the switching cost function by implementing an exponential function for transitioning between cost functions. However, if you have any recommendations for addressing the problem related to the maximum iteration count, I would greatly appreciate your insights.)