Nonlinear constraint not satisfied along trajectory
The nonlinear constraint cos(maxAttitudeAngle) < x(9)^2 - x(6)^2 - x(7)^2 + x(8)^2 is not satisfied for some collocation points of the solution.
mpc_test.zip (note: scaling is deactivated in the attached files (drone_control_ocp.hpp lines 103-106, line 119))