Using Euler angles and how to get the Adyn, Bdyn, etc.

[hedaniel7]

Hi

I think it should be possible to swap the Rodriguez-parameter representation in your code with Euler-angles as TinyMPC generally works with and accelerates MPC problems , correct?
If yes, how does one get the information/ guess-estimate the Adyn_data, Bdyn_data, Pinf_data, Kinf_data, etc. for example for the Crazyflie 2.1?

Thank you for your time and effort :)

[xkhainguyen]

Hi there,

Our quadrotor examples use a 12-dim state model, hence, Euler angles are completely compatible.

The quickest and easiest way is reuse our existing pipeline. You only need to convert Rodriguez and Euler angle representations at the input (feedback state) and output (trajectory) of the MPC problem. You can find it online here

Otherwise, you need different A and B matrices, and then other precomputed variables. You may want to start from nonlinear dynamics with Euler angles first. For a better understanding of the entire workflow (even with the real hardware), we recommend you check this example. This should provide you with a sense of modeling as well as MPC implementation. You can see our actual Crazyflie model parameters (we guess-estimated them though), but the remaining MPC part is new and has not been tested on hardware.

[hedaniel7]

Thank you for the quick answer :)

I have seen your linked example already and I have tried to do it the quickest and easiest way in my TinyMPC ROS2 node and convert the incoming Rotation information from the Webots simulation (in quaternion form) to Rodriguez to reuse your existing pipeline (if I did it correctly). As far as I understand the solver still does work with Rodriguez parameters in the end. This resulted for me in a spinning bug reported here

I think that using Euler angles would at least give me a better understanding when I try to debug the reason for this bug and visualize for myself what happens and it would simplify the problem for me overall (although at the cost of the singularities of the Euler angles, which I am willing to take on).

In that sense I really need to have different A and B matrices, etc. defined here in my [project]
I lack the needed full theoretical understanding here so please excuse the following dumb questions.

1) A and B matrices

(https://github.com/AlboAlby00/CrazyflieControllers/blob/TinyMPC/crazyflie_controllers/include/crazyflie_controllers/TinyMPC_controller.h).
As for the A and B matrices, you said that

You may want to start from nonlinear dynamics with Euler angles first.

Like in your paper stated for TinyMPC, are the matrices A and B the system matrices of the linear system. Are these therefore the following matrices for Euler angles (taken from Francesco Sabatino's Master thesis, p.20)?
if we assume a differently ordered state (Sabatino, p.15)

I would have just to slightly reorder the matrices A and B?

2) Kinf_data and Pinf_data

In your paper, suppose that we have now A and B and chose Q and R as our weighting matices, you stated

where k runs till the horizon N. Does that mean I get Pinf and Kinf by solving the discrete Riccati equations backwards until Pinf and Kinf converge, sort of like a Banach fixed point iteration?

3) Quu_inv_data , AmBKt_data and coeff_d2p_data

From your paper

Is the following valid?

Quu_inv_data = C1
AmBKt_data = C2
coeff_d2p_data = C3

If it is not true how would I for example obtain Quu_inv_data or coeff_d2p_data with the information I would have now (Kinf, Pinf, Q, R, A and B)?

Could you recommend some literature or method/code in this context, also?

Thank you