 Hello,

I am trying to implement a MPC controller on a quadrotor using acados. I have 5 inputs:
1 - The total thrust of the drone: T
2 - The quatrenion, which is made out of 4 elements to output a certain orientation for the drone: qw, qx, qy, qz.

I am using the python interface. So, the inputs are defined as follows:

``````T  = MX.sym("T")
qw = MX.sym("qw")
qx = MX.sym("qx")
qy = MX.sym("qy")
qz = MX.sym("qz")
u = vertcat(T, qw, qx, qy, qz)
``````

However, I have a problem. I need the quaternion [qw, qx, qy, qz] to be of unit norm:

`sqrt(qw**2 + qx**2 + qy**2 + qz**2) = 1`

Is there a way to inform the solver of this constraint?
I know that I can add lower and upper bounds on each element. And, I did that for the thrust input T.
However, I don’t know how to make the quaternion of unit norm within the solver.
It would be of great if someone can help.

Thank you

Hi,

I haven’t personally used the Python interface, but acados has multiple forms of constraint available. The ones you’re using are the simple state and input bounds, but you could also use the `h(x,u)` (nonlinear inequality constraint). Note that, as I understand it, these are mutually exclusive, so if you do use `h(x,u)`, you need to collate all your simple bounds in there as well, writing `h` as one large vector-valued function.

(Side note: the problem formulation page has more details on this (types of constraints allowed, types of cost functions, etc.) if you’re interested.)

For this problem, you’ll want to set `h(x,u)` as the CasADi expression, `sqrt(qw**2 + qx**2 + qy**2 + qz**2)`, and then set the expression’s upper and lower bounds to 1.

You can also check out our recent discussion here about the practical difficulties of working with quats and the norm constraint: Implementing cost functions which are nonlinear in y_ref? - #7 by jcwj

1 Like

Thanks for your reply! I will try to implement it and will let you know what happens.

Hi @jcwj