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