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.