Implementing cost functions which are nonlinear in y_ref?

Hi, I’m trying to implement a controller that uses quaternions to represent orientation/attitude. As I understand it, the external cost module allows us to implement costs that are non-linear in x, u, slack variables z. But to compute the quaternion attitude error, I need to apply non-linear operations on y_ref as well (quat-multiply x_quat with y_ref_quat). Is it possible to do this in acados?


the cost function you are describing doesn’t fit into the nonlinear least squares form.
I suggest using the external cost module and implement what you denote as y_ref_quat as a parameter.


1 Like

I see, I guessed there might be a dedicated handle for the y_ref symbol (like there is with x/u/z) but this method makes sense too. Will give it a shot!

@jcwj I recommend using external costs and then creating a CasADi function for your attitude error. I’ve used that and it works fine.

Indeed, I did it like you suggested and after some work, the solver now compiles without crashing. However, it seems to be having trouble converging to a solution, perhaps because of the unit norm constraint… Does your implementation use quaternions as well?

I also have that issue and I’m not sure what is the best way to fix it. In my case, I normalize it outside (with a small performance penalty thereof). I thought about trying different integrators, but didn’t get to it…

1 Like

I just tried the normalization method and it works surprisingly well, considering that the solver doesn’t know about the norm constraint… I guess the dynamics constraint is helping a lot with that, and, like you said, better integrators might improve it even further.

Many thanks for sharing your solution! I’m glad my code works properly now at least.

1 Like