|
1 | | -# Mellinger Controller |
| 1 | +# Mellinger |
2 | 2 |
|
3 | | -The Mellinger controller is a geometric tracking controller originally developed for aggressive quadrotor maneuvers [[1]](#references). This implementation is based on the Crazyflie firmware version. |
| 3 | +::: drone_controllers.mellinger.control |
4 | 4 |
|
5 | | -## Controller Functions |
6 | | - |
7 | | -### state2attitude |
8 | | - |
9 | | -::: drone_controllers.mellinger.control.state2attitude |
10 | | - |
11 | | -The position control component of the Mellinger controller. Converts desired position, velocity, and acceleration into attitude commands. |
12 | | - |
13 | | -**Example:** |
14 | | -```python |
15 | | -from drone_controllers import parametrize |
16 | | -from drone_controllers.mellinger import state2attitude |
17 | | - |
18 | | -controller = parametrize(state2attitude, "cf2x_L250") |
19 | | - |
20 | | -rpyt, pos_err_i = controller(pos, quat, vel, cmd) |
21 | | -``` |
22 | | - |
23 | | -### attitude2force_torque |
24 | | - |
25 | | -::: drone_controllers.mellinger.control.attitude2force_torque |
26 | | - |
27 | | -The attitude control component that converts attitude commands into desired forces and torques. |
28 | | - |
29 | | -**Example:** |
30 | | -```python |
31 | | -from drone_controllers import parametrize |
32 | | -from drone_controllers.mellinger import attitude2force_torque |
33 | | - |
34 | | -controller = parametrize(attitude2force_torque, "cf2x_L250") |
35 | | - |
36 | | -force, torque, att_err_i = controller(quat, ang_vel, rpyt_cmd) |
37 | | -``` |
38 | | - |
39 | | -### force_torque2rotor_vel |
40 | | - |
41 | | -::: drone_controllers.mellinger.control.force_torque2rotor_vel |
42 | | - |
43 | | -Converts desired forces and torques into individual rotor velocities using the quadrotor mixing matrix. |
44 | | - |
45 | | -**Example:** |
46 | | -```python |
47 | | -from drone_controllers import parametrize |
48 | | -from drone_controllers.mellinger import force_torque2rotor_vel |
49 | | - |
50 | | -controller = parametrize(force_torque2rotor_vel, "cf2x_L250") |
51 | | - |
52 | | -rotor_speeds = controller(force, torque) |
53 | | -``` |
54 | | - |
55 | | -## Complete Controller Pipeline |
56 | | - |
57 | | -Here's how to use all three components together: |
58 | | - |
59 | | -```python |
60 | | -import numpy as np |
61 | | -from drone_controllers import parametrize |
62 | | -from drone_controllers.mellinger import ( |
63 | | - state2attitude, |
64 | | - attitude2force_torque, |
65 | | - force_torque2rotor_vel |
66 | | -) |
67 | | - |
68 | | -# Parametrize all three controller components |
69 | | -state_ctrl = parametrize(state2attitude, "cf2x_L250") |
70 | | -attitude_ctrl = parametrize(attitude2force_torque, "cf2x_L250") |
71 | | -rotor_ctrl = parametrize(force_torque2rotor_vel, "cf2x_L250") |
72 | | - |
73 | | -# Define state |
74 | | -pos = np.array([0.0, 0.0, 1.0]) |
75 | | -quat = np.array([0.0, 0.0, 0.0, 1.0]) # [x, y, z, w] |
76 | | -vel = np.array([0.0, 0.0, 0.0]) |
77 | | -ang_vel = np.array([0.0, 0.0, 0.0]) |
78 | | - |
79 | | -# Define command: [x, y, z, vx, vy, vz, ax, ay, az, yaw, r_rate, p_rate, y_rate] |
80 | | -cmd = np.array([1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) |
81 | | - |
82 | | -# Run the complete pipeline |
83 | | -rpyt, pos_err_i = state_ctrl(pos, quat, vel, cmd) |
84 | | -force, torque, att_err_i = attitude_ctrl(quat, ang_vel, rpyt) |
85 | | -rotor_speeds = rotor_ctrl(force, torque) |
86 | | - |
87 | | -print(f"Final rotor speeds: {rotor_speeds} rad/s") |
88 | | -``` |
89 | | - |
90 | | -## Integral Error Handling |
91 | | - |
92 | | -The Mellinger controller uses integral terms for robustness. You must pass integral errors between calls: |
93 | | - |
94 | | -```python |
95 | | -# Initialize |
96 | | -pos_err_i = None |
97 | | -att_err_i = None |
98 | | - |
99 | | -for step in range(100): |
100 | | - # Update state and command... |
101 | | - |
102 | | - # Pass previous integral errors |
103 | | - ctrl_errors = (pos_err_i,) if pos_err_i is not None else None |
104 | | - rpyt, pos_err_i = state_ctrl(pos, quat, vel, cmd, ctrl_errors=ctrl_errors) |
105 | | - |
106 | | - ctrl_errors = (att_err_i,) if att_err_i is not None else None |
107 | | - force, torque, att_err_i = attitude_ctrl(quat, ang_vel, rpyt, ctrl_errors=ctrl_errors) |
108 | | - |
109 | | - rotor_speeds = rotor_ctrl(force, torque) |
110 | | -``` |
111 | | - |
112 | | -## Array API Compatibility |
113 | | - |
114 | | -All Mellinger functions support the Python Array API and can be used with JAX, PyTorch, etc.: |
115 | | - |
116 | | -```python |
117 | | -import jax.numpy as jnp |
118 | | -from jax import jit |
119 | | - |
120 | | -# Convert to JAX arrays |
121 | | -pos_jax = jnp.array([0.0, 0.0, 1.0]) |
122 | | -quat_jax = jnp.array([0.0, 0.0, 0.0, 1.0]) |
123 | | -# ... other arrays |
124 | | - |
125 | | -# JIT compile the controller |
126 | | -jit_controller = jit(parametrize(state2attitude, "cf2x_L250")) |
127 | | - |
128 | | -rpyt, pos_err_i = jit_controller(pos_jax, quat_jax, vel_jax, cmd_jax) |
129 | | -``` |
130 | | - |
131 | | -# References |
132 | | - |
133 | | -[1] D. Mellinger and V. Kumar, "Minimum snap trajectory generation and control for quadrotors," 2011 IEEE International Conference on Robotics and Automation, Shanghai, China, 2011, pp. 2520-2525, doi: 10.1109/ICRA.2011.5980409. |
| 5 | +See the [Mellinger user guide](../user-guide/mellinger.md) for input/output tables, worked examples, and guidance on chaining all three stages. |
0 commit comments