drone-models 0.1.0__tar.gz
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- drone_models-0.1.0/PKG-INFO +145 -0
- drone_models-0.1.0/README.md +124 -0
- drone_models-0.1.0/docs/gen_ref_pages.py +103 -0
- drone_models-0.1.0/drone_models/__init__.py +73 -0
- drone_models-0.1.0/drone_models/_typing.py +8 -0
- drone_models-0.1.0/drone_models/core.py +163 -0
- drone_models-0.1.0/drone_models/data/assets/cf21B/cf21B_PropL.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf21B/cf21B_PropR.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf21B/cf21B_battery-holder.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf21B/cf21B_battery.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf21B/cf21B_connector-pins.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf21B/cf21B_connectors.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf21B/cf21B_full.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf21B/cf21B_header.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf21B/cf21B_motors.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf21B/cf21B_no-prop.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf21B/cf21B_pcb.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf21B/cf21B_prop-guards.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf21B/cf_led-diffusor.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf2x/cf2xL_PropL.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf2x/cf2xL_PropR.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf2x/cf2xL_motors.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf2x/cf2xP_PropL.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf2x/cf2xP_PropR.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf2x/cf2xP_motors.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf2x/cf2xT_motors.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf2x/cf2x_battery-holder.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf2x/cf2x_battery.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf2x/cf2x_connector-pins.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf2x/cf2x_connectors.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf2x/cf2x_motor-holder.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf2x/cf2x_pcb.stl +0 -0
- drone_models-0.1.0/drone_models/data/assets/cf2x/cf_led-diffusor.stl +0 -0
- drone_models-0.1.0/drone_models/data/cf21B_500.xml +101 -0
- drone_models-0.1.0/drone_models/data/cf2x_L250.xml +101 -0
- drone_models-0.1.0/drone_models/data/cf2x_P250.xml +101 -0
- drone_models-0.1.0/drone_models/data/cf2x_T350.xml +102 -0
- drone_models-0.1.0/drone_models/data/params.toml +152 -0
- drone_models-0.1.0/drone_models/drones.py +16 -0
- drone_models-0.1.0/drone_models/first_principles/__init__.py +88 -0
- drone_models-0.1.0/drone_models/first_principles/model.py +287 -0
- drone_models-0.1.0/drone_models/first_principles/params.toml +13 -0
- drone_models-0.1.0/drone_models/so_rpy/__init__.py +33 -0
- drone_models-0.1.0/drone_models/so_rpy/model.py +294 -0
- drone_models-0.1.0/drone_models/so_rpy/params.toml +30 -0
- drone_models-0.1.0/drone_models/so_rpy_rotor/__init__.py +32 -0
- drone_models-0.1.0/drone_models/so_rpy_rotor/model.py +331 -0
- drone_models-0.1.0/drone_models/so_rpy_rotor/params.toml +34 -0
- drone_models-0.1.0/drone_models/so_rpy_rotor_drag/__init__.py +38 -0
- drone_models-0.1.0/drone_models/so_rpy_rotor_drag/model.py +357 -0
- drone_models-0.1.0/drone_models/so_rpy_rotor_drag/params.toml +54 -0
- drone_models-0.1.0/drone_models/symbols.py +49 -0
- drone_models-0.1.0/drone_models/transform.py +132 -0
- drone_models-0.1.0/drone_models/utils/__init__.py +18 -0
- drone_models-0.1.0/drone_models/utils/data_utils.py +227 -0
- drone_models-0.1.0/drone_models/utils/identification.py +502 -0
- drone_models-0.1.0/drone_models/utils/rotation.py +562 -0
- drone_models-0.1.0/drone_models.egg-info/PKG-INFO +145 -0
- drone_models-0.1.0/drone_models.egg-info/SOURCES.txt +68 -0
- drone_models-0.1.0/drone_models.egg-info/dependency_links.txt +1 -0
- drone_models-0.1.0/drone_models.egg-info/requires.txt +9 -0
- drone_models-0.1.0/drone_models.egg-info/top_level.txt +4 -0
- drone_models-0.1.0/pyproject.toml +165 -0
- drone_models-0.1.0/setup.cfg +4 -0
- drone_models-0.1.0/tests/conftest.py +10 -0
- drone_models-0.1.0/tests/integration/test_identification_pipeline.py +57 -0
- drone_models-0.1.0/tests/unit/test_identification.py +60 -0
- drone_models-0.1.0/tests/unit/test_models.py +442 -0
- drone_models-0.1.0/tests/unit/test_parametrization.py +19 -0
- drone_models-0.1.0/tests/unit/test_rot.py +223 -0
|
@@ -0,0 +1,145 @@
|
|
|
1
|
+
Metadata-Version: 2.4
|
|
2
|
+
Name: drone_models
|
|
3
|
+
Version: 0.1.0
|
|
4
|
+
Summary: Models of quadrotor drones for estimation and control tasks.
|
|
5
|
+
Author: Marcel Rath, Martin Schuck
|
|
6
|
+
Classifier: License :: OSI Approved :: MIT License
|
|
7
|
+
Classifier: Programming Language :: Python
|
|
8
|
+
Classifier: Programming Language :: Python :: 3
|
|
9
|
+
Classifier: Intended Audience :: Developers
|
|
10
|
+
Classifier: Intended Audience :: Education
|
|
11
|
+
Classifier: Intended Audience :: Science/Research
|
|
12
|
+
Description-Content-Type: text/markdown
|
|
13
|
+
Requires-Dist: numpy>=2.0.0
|
|
14
|
+
Requires-Dist: scipy>=1.17.0
|
|
15
|
+
Requires-Dist: casadi>=3.7.0
|
|
16
|
+
Requires-Dist: array-api-compat
|
|
17
|
+
Requires-Dist: array-api-extra
|
|
18
|
+
Provides-Extra: sysid
|
|
19
|
+
Requires-Dist: matplotlib; extra == "sysid"
|
|
20
|
+
Requires-Dist: jax>=0.7; extra == "sysid"
|
|
21
|
+
|
|
22
|
+
$$
|
|
23
|
+
\huge \displaystyle \dot{x} = f(x,u)
|
|
24
|
+
$$
|
|
25
|
+
|
|
26
|
+
---
|
|
27
|
+
|
|
28
|
+
Physics-based and data-driven quadrotor dynamics models for estimation, control, and simulation.
|
|
29
|
+
|
|
30
|
+
[![Python Version]][Python Version URL] [![Ruff Check]][Ruff Check URL] [![Tests]][Tests URL] [![Docs]][Docs URL]
|
|
31
|
+
|
|
32
|
+
[Python Version]: https://img.shields.io/badge/python-3.10+-blue.svg
|
|
33
|
+
[Python Version URL]: https://www.python.org
|
|
34
|
+
|
|
35
|
+
[Ruff Check]: https://github.com/utiasDSL/drone-models/actions/workflows/ruff.yml/badge.svg?style=flat-square
|
|
36
|
+
[Ruff Check URL]: https://github.com/utiasDSL/drone-models/actions/workflows/ruff.yml
|
|
37
|
+
|
|
38
|
+
[Tests]: https://github.com/utiasDSL/drone-models/actions/workflows/testing.yml/badge.svg
|
|
39
|
+
[Tests URL]: https://github.com/utiasDSL/drone-models/actions/workflows/testing.yml
|
|
40
|
+
|
|
41
|
+
[Docs]: https://github.com/utiasDSL/drone-models/actions/workflows/docs.yml/badge.svg
|
|
42
|
+
[Docs URL]: https://utiasdsl.github.io/drone-models/
|
|
43
|
+
|
|
44
|
+
## Overview
|
|
45
|
+
|
|
46
|
+
`drone-models` provides quadrotor dynamics as pure Python functions, from full physics-based implementations to lightweight data-driven approximations. All models support NumPy, JAX, and any [Array API](https://data-apis.org/array-api/latest/) backend, plus [CasADi](https://web.casaid.org/) symbolic variants for optimization-based control. Pre-fitted parameters are included for several Crazyflie platforms.
|
|
47
|
+
|
|
48
|
+
**Available models** — ranging from high-fidelity to lightweight:
|
|
49
|
+
|
|
50
|
+
| Model | Description |
|
|
51
|
+
|---|---|
|
|
52
|
+
| `first_principles` | Full rigid-body physics with optional rotor dynamics and aerodynamic drag |
|
|
53
|
+
| `so_rpy_rotor_drag` | Data-driven, attitude as roll/pitch/yaw, with rotor dynamics and drag |
|
|
54
|
+
| `so_rpy_rotor` | Data-driven, attitude as roll/pitch/yaw, with rotor dynamics |
|
|
55
|
+
| `so_rpy` | Lightest data-driven model, attitude as roll/pitch/yaw, no rotor dynamics |
|
|
56
|
+
|
|
57
|
+
**Pre-fitted configurations** for Crazyflie 2.x (brushed) and Crazyflie 2.1 Brushless:
|
|
58
|
+
`cf2x_L250`, `cf2x_P250`, `cf2x_T350`, `cf21B_500`
|
|
59
|
+
|
|
60
|
+
## Installation
|
|
61
|
+
|
|
62
|
+
```bash
|
|
63
|
+
pip install drone-models
|
|
64
|
+
```
|
|
65
|
+
|
|
66
|
+
For system identification (fitting parameters from your own flight data):
|
|
67
|
+
|
|
68
|
+
```bash
|
|
69
|
+
pip install "drone-models[sysid]"
|
|
70
|
+
```
|
|
71
|
+
|
|
72
|
+
> **Note:** `drone_models` must be imported before SciPy to enable Array API support. If you encounter a `RuntimeError`, either import `drone_models` first or set `export SCIPY_ARRAY_API=1` in your shell.
|
|
73
|
+
|
|
74
|
+
## Usage
|
|
75
|
+
|
|
76
|
+
### Basic
|
|
77
|
+
|
|
78
|
+
Bind parameters to a drone configuration with `parametrize`, then call the model with state and command arrays:
|
|
79
|
+
|
|
80
|
+
```python
|
|
81
|
+
import numpy as np
|
|
82
|
+
from drone_models import parametrize
|
|
83
|
+
from drone_models.first_principles import dynamics
|
|
84
|
+
|
|
85
|
+
model = parametrize(dynamics, drone_model="cf2x_L250")
|
|
86
|
+
|
|
87
|
+
pos = np.zeros(3)
|
|
88
|
+
quat = np.array([0., 0., 0., 1.]) # xyzw, identity
|
|
89
|
+
vel = np.zeros(3)
|
|
90
|
+
ang_vel = np.zeros(3)
|
|
91
|
+
rotor_vel = np.ones(4) * 12_000. # current motor RPMs
|
|
92
|
+
cmd = np.full(4, 15_000.) # commanded motor RPMs
|
|
93
|
+
|
|
94
|
+
pos_dot, quat_dot, vel_dot, ang_vel_dot, rotor_vel_dot = model(
|
|
95
|
+
pos, quat, vel, ang_vel, cmd, rotor_vel
|
|
96
|
+
)
|
|
97
|
+
```
|
|
98
|
+
|
|
99
|
+
The model returns continuous-time state derivatives $\dot{x} = f(x, u)$. Integrate with any ODE solver (e.g. `scipy.integrate.solve_ivp`) to simulate forward in time.
|
|
100
|
+
|
|
101
|
+
### Switching backends
|
|
102
|
+
|
|
103
|
+
Pass any Array API-compatible array and the output type follows automatically — no code changes needed:
|
|
104
|
+
|
|
105
|
+
```python
|
|
106
|
+
import jax.numpy as jnp
|
|
107
|
+
from drone_models import parametrize
|
|
108
|
+
from drone_models.first_principles import dynamics
|
|
109
|
+
|
|
110
|
+
model = parametrize(dynamics, drone_model="cf2x_L250", xp=jnp)
|
|
111
|
+
# Pass jax arrays — get jax arrays back
|
|
112
|
+
```
|
|
113
|
+
|
|
114
|
+
Arbitrary leading batch dimensions work out of the box: stack states for a thousand drones and evaluate them in one call.
|
|
115
|
+
|
|
116
|
+
### Symbolic models (CasADi)
|
|
117
|
+
|
|
118
|
+
Every model exposes a `symbolic_dynamics` function returning CasADi `MX` expressions, for use in MPC, trajectory optimization, or estimation:
|
|
119
|
+
|
|
120
|
+
```python
|
|
121
|
+
from drone_models import parametrize
|
|
122
|
+
from drone_models.first_principles import symbolic_dynamics
|
|
123
|
+
|
|
124
|
+
sym_model = parametrize(symbolic_dynamics, drone_model="cf2x_L250")
|
|
125
|
+
```
|
|
126
|
+
|
|
127
|
+
## Development
|
|
128
|
+
|
|
129
|
+
Clone and install with [pixi](https://pixi.sh):
|
|
130
|
+
|
|
131
|
+
```bash
|
|
132
|
+
git clone https://github.com/utiasDSL/drone-models.git
|
|
133
|
+
cd drone-models
|
|
134
|
+
pixi install
|
|
135
|
+
```
|
|
136
|
+
|
|
137
|
+
Run tests:
|
|
138
|
+
|
|
139
|
+
```bash
|
|
140
|
+
pixi run -e tests tests
|
|
141
|
+
```
|
|
142
|
+
|
|
143
|
+
## Citation
|
|
144
|
+
|
|
145
|
+
Citation information coming soon. See the [docs](https://utiasdsl.github.io/drone-models/) for updates.
|
|
@@ -0,0 +1,124 @@
|
|
|
1
|
+
$$
|
|
2
|
+
\huge \displaystyle \dot{x} = f(x,u)
|
|
3
|
+
$$
|
|
4
|
+
|
|
5
|
+
---
|
|
6
|
+
|
|
7
|
+
Physics-based and data-driven quadrotor dynamics models for estimation, control, and simulation.
|
|
8
|
+
|
|
9
|
+
[![Python Version]][Python Version URL] [![Ruff Check]][Ruff Check URL] [![Tests]][Tests URL] [![Docs]][Docs URL]
|
|
10
|
+
|
|
11
|
+
[Python Version]: https://img.shields.io/badge/python-3.10+-blue.svg
|
|
12
|
+
[Python Version URL]: https://www.python.org
|
|
13
|
+
|
|
14
|
+
[Ruff Check]: https://github.com/utiasDSL/drone-models/actions/workflows/ruff.yml/badge.svg?style=flat-square
|
|
15
|
+
[Ruff Check URL]: https://github.com/utiasDSL/drone-models/actions/workflows/ruff.yml
|
|
16
|
+
|
|
17
|
+
[Tests]: https://github.com/utiasDSL/drone-models/actions/workflows/testing.yml/badge.svg
|
|
18
|
+
[Tests URL]: https://github.com/utiasDSL/drone-models/actions/workflows/testing.yml
|
|
19
|
+
|
|
20
|
+
[Docs]: https://github.com/utiasDSL/drone-models/actions/workflows/docs.yml/badge.svg
|
|
21
|
+
[Docs URL]: https://utiasdsl.github.io/drone-models/
|
|
22
|
+
|
|
23
|
+
## Overview
|
|
24
|
+
|
|
25
|
+
`drone-models` provides quadrotor dynamics as pure Python functions, from full physics-based implementations to lightweight data-driven approximations. All models support NumPy, JAX, and any [Array API](https://data-apis.org/array-api/latest/) backend, plus [CasADi](https://web.casaid.org/) symbolic variants for optimization-based control. Pre-fitted parameters are included for several Crazyflie platforms.
|
|
26
|
+
|
|
27
|
+
**Available models** — ranging from high-fidelity to lightweight:
|
|
28
|
+
|
|
29
|
+
| Model | Description |
|
|
30
|
+
|---|---|
|
|
31
|
+
| `first_principles` | Full rigid-body physics with optional rotor dynamics and aerodynamic drag |
|
|
32
|
+
| `so_rpy_rotor_drag` | Data-driven, attitude as roll/pitch/yaw, with rotor dynamics and drag |
|
|
33
|
+
| `so_rpy_rotor` | Data-driven, attitude as roll/pitch/yaw, with rotor dynamics |
|
|
34
|
+
| `so_rpy` | Lightest data-driven model, attitude as roll/pitch/yaw, no rotor dynamics |
|
|
35
|
+
|
|
36
|
+
**Pre-fitted configurations** for Crazyflie 2.x (brushed) and Crazyflie 2.1 Brushless:
|
|
37
|
+
`cf2x_L250`, `cf2x_P250`, `cf2x_T350`, `cf21B_500`
|
|
38
|
+
|
|
39
|
+
## Installation
|
|
40
|
+
|
|
41
|
+
```bash
|
|
42
|
+
pip install drone-models
|
|
43
|
+
```
|
|
44
|
+
|
|
45
|
+
For system identification (fitting parameters from your own flight data):
|
|
46
|
+
|
|
47
|
+
```bash
|
|
48
|
+
pip install "drone-models[sysid]"
|
|
49
|
+
```
|
|
50
|
+
|
|
51
|
+
> **Note:** `drone_models` must be imported before SciPy to enable Array API support. If you encounter a `RuntimeError`, either import `drone_models` first or set `export SCIPY_ARRAY_API=1` in your shell.
|
|
52
|
+
|
|
53
|
+
## Usage
|
|
54
|
+
|
|
55
|
+
### Basic
|
|
56
|
+
|
|
57
|
+
Bind parameters to a drone configuration with `parametrize`, then call the model with state and command arrays:
|
|
58
|
+
|
|
59
|
+
```python
|
|
60
|
+
import numpy as np
|
|
61
|
+
from drone_models import parametrize
|
|
62
|
+
from drone_models.first_principles import dynamics
|
|
63
|
+
|
|
64
|
+
model = parametrize(dynamics, drone_model="cf2x_L250")
|
|
65
|
+
|
|
66
|
+
pos = np.zeros(3)
|
|
67
|
+
quat = np.array([0., 0., 0., 1.]) # xyzw, identity
|
|
68
|
+
vel = np.zeros(3)
|
|
69
|
+
ang_vel = np.zeros(3)
|
|
70
|
+
rotor_vel = np.ones(4) * 12_000. # current motor RPMs
|
|
71
|
+
cmd = np.full(4, 15_000.) # commanded motor RPMs
|
|
72
|
+
|
|
73
|
+
pos_dot, quat_dot, vel_dot, ang_vel_dot, rotor_vel_dot = model(
|
|
74
|
+
pos, quat, vel, ang_vel, cmd, rotor_vel
|
|
75
|
+
)
|
|
76
|
+
```
|
|
77
|
+
|
|
78
|
+
The model returns continuous-time state derivatives $\dot{x} = f(x, u)$. Integrate with any ODE solver (e.g. `scipy.integrate.solve_ivp`) to simulate forward in time.
|
|
79
|
+
|
|
80
|
+
### Switching backends
|
|
81
|
+
|
|
82
|
+
Pass any Array API-compatible array and the output type follows automatically — no code changes needed:
|
|
83
|
+
|
|
84
|
+
```python
|
|
85
|
+
import jax.numpy as jnp
|
|
86
|
+
from drone_models import parametrize
|
|
87
|
+
from drone_models.first_principles import dynamics
|
|
88
|
+
|
|
89
|
+
model = parametrize(dynamics, drone_model="cf2x_L250", xp=jnp)
|
|
90
|
+
# Pass jax arrays — get jax arrays back
|
|
91
|
+
```
|
|
92
|
+
|
|
93
|
+
Arbitrary leading batch dimensions work out of the box: stack states for a thousand drones and evaluate them in one call.
|
|
94
|
+
|
|
95
|
+
### Symbolic models (CasADi)
|
|
96
|
+
|
|
97
|
+
Every model exposes a `symbolic_dynamics` function returning CasADi `MX` expressions, for use in MPC, trajectory optimization, or estimation:
|
|
98
|
+
|
|
99
|
+
```python
|
|
100
|
+
from drone_models import parametrize
|
|
101
|
+
from drone_models.first_principles import symbolic_dynamics
|
|
102
|
+
|
|
103
|
+
sym_model = parametrize(symbolic_dynamics, drone_model="cf2x_L250")
|
|
104
|
+
```
|
|
105
|
+
|
|
106
|
+
## Development
|
|
107
|
+
|
|
108
|
+
Clone and install with [pixi](https://pixi.sh):
|
|
109
|
+
|
|
110
|
+
```bash
|
|
111
|
+
git clone https://github.com/utiasDSL/drone-models.git
|
|
112
|
+
cd drone-models
|
|
113
|
+
pixi install
|
|
114
|
+
```
|
|
115
|
+
|
|
116
|
+
Run tests:
|
|
117
|
+
|
|
118
|
+
```bash
|
|
119
|
+
pixi run -e tests tests
|
|
120
|
+
```
|
|
121
|
+
|
|
122
|
+
## Citation
|
|
123
|
+
|
|
124
|
+
Citation information coming soon. See the [docs](https://utiasdsl.github.io/drone-models/) for updates.
|
|
@@ -0,0 +1,103 @@
|
|
|
1
|
+
"""Generate the code reference pages and navigation."""
|
|
2
|
+
|
|
3
|
+
from pathlib import Path
|
|
4
|
+
|
|
5
|
+
import mkdocs_gen_files
|
|
6
|
+
|
|
7
|
+
SKIP_PARTS = {"_typing", "__main__"}
|
|
8
|
+
|
|
9
|
+
# model.py in these packages is re-exported via __init__, so document only at index level
|
|
10
|
+
SKIP_MODULE_PAGES = {
|
|
11
|
+
("drone_models", "first_principles", "model"),
|
|
12
|
+
("drone_models", "so_rpy", "model"),
|
|
13
|
+
("drone_models", "so_rpy_rotor", "model"),
|
|
14
|
+
("drone_models", "so_rpy_rotor_drag", "model"),
|
|
15
|
+
}
|
|
16
|
+
|
|
17
|
+
TOP_LEVEL_PAGE = """\
|
|
18
|
+
::: drone_models
|
|
19
|
+
options:
|
|
20
|
+
members:
|
|
21
|
+
- parametrize
|
|
22
|
+
- available_models
|
|
23
|
+
- model_features
|
|
24
|
+
show_submodules: false
|
|
25
|
+
|
|
26
|
+
## Submodules
|
|
27
|
+
|
|
28
|
+
### Models
|
|
29
|
+
|
|
30
|
+
| Module | Description |
|
|
31
|
+
|--------|-------------|
|
|
32
|
+
| [first_principles](first_principles/index.md) | Full physics model |
|
|
33
|
+
| [so_rpy_rotor_drag](so_rpy_rotor_drag/index.md) | Fitted model with rotor dynamics and drag |
|
|
34
|
+
| [so_rpy_rotor](so_rpy_rotor/index.md) | Fitted model with rotor dynamics |
|
|
35
|
+
| [so_rpy](so_rpy/index.md) | Simplest fitted model |
|
|
36
|
+
|
|
37
|
+
### Core
|
|
38
|
+
|
|
39
|
+
| Module | Description |
|
|
40
|
+
|--------|-------------|
|
|
41
|
+
| [core](../core.md) | `load_params`, `supports` decorator, internal `parametrize` |
|
|
42
|
+
| [drones](../drones.md) | `available_drones` tuple |
|
|
43
|
+
| [symbols](../symbols.md) | CasADi symbolic utilities |
|
|
44
|
+
| [transform](../transform.md) | Motor/PWM/rotor conversion utilities |
|
|
45
|
+
|
|
46
|
+
### Utilities
|
|
47
|
+
|
|
48
|
+
| Module | Description |
|
|
49
|
+
|--------|-------------|
|
|
50
|
+
| [utils.rotation](../utils/rotation.md) | Quaternion/Euler/angular velocity conversions |
|
|
51
|
+
| [utils.data_utils](../utils/data_utils.md) | `preprocessing`, `derivatives_svf` |
|
|
52
|
+
| [utils.identification](../utils/identification.md) | `sys_id_translation`, `sys_id_rotation` |
|
|
53
|
+
"""
|
|
54
|
+
|
|
55
|
+
for path in sorted(Path("drone_models").rglob("*.py")):
|
|
56
|
+
module_path = path.relative_to(".").with_suffix("")
|
|
57
|
+
doc_path = path.relative_to(".").with_suffix(".md")
|
|
58
|
+
full_doc_path = Path("reference", doc_path)
|
|
59
|
+
|
|
60
|
+
parts = tuple(module_path.parts)
|
|
61
|
+
|
|
62
|
+
if any(part in SKIP_PARTS for part in parts):
|
|
63
|
+
continue
|
|
64
|
+
|
|
65
|
+
if parts in SKIP_MODULE_PAGES:
|
|
66
|
+
continue
|
|
67
|
+
|
|
68
|
+
if parts[-1] == "__init__":
|
|
69
|
+
parts = parts[:-1]
|
|
70
|
+
doc_path = doc_path.with_name("index.md")
|
|
71
|
+
full_doc_path = full_doc_path.with_name("index.md")
|
|
72
|
+
elif parts[-1] == "__main__":
|
|
73
|
+
continue
|
|
74
|
+
|
|
75
|
+
with mkdocs_gen_files.open(full_doc_path, "w") as fd:
|
|
76
|
+
if parts == ("drone_models",):
|
|
77
|
+
fd.write(TOP_LEVEL_PAGE)
|
|
78
|
+
else:
|
|
79
|
+
ident = ".".join(parts)
|
|
80
|
+
fd.write(f"::: {ident}\n")
|
|
81
|
+
|
|
82
|
+
mkdocs_gen_files.set_edit_path(full_doc_path, path)
|
|
83
|
+
|
|
84
|
+
summary = """\
|
|
85
|
+
* [drone_models](drone_models/index.md)
|
|
86
|
+
* [first_principles](drone_models/first_principles/index.md)
|
|
87
|
+
* [so_rpy_rotor_drag](drone_models/so_rpy_rotor_drag/index.md)
|
|
88
|
+
* [so_rpy_rotor](drone_models/so_rpy_rotor/index.md)
|
|
89
|
+
* [so_rpy](drone_models/so_rpy/index.md)
|
|
90
|
+
* Core
|
|
91
|
+
* [core](drone_models/core.md)
|
|
92
|
+
* [drones](drone_models/drones.md)
|
|
93
|
+
* [symbols](drone_models/symbols.md)
|
|
94
|
+
* [transform](drone_models/transform.md)
|
|
95
|
+
* Utilities
|
|
96
|
+
* [utils](drone_models/utils/index.md)
|
|
97
|
+
* [utils.rotation](drone_models/utils/rotation.md)
|
|
98
|
+
* [utils.data_utils](drone_models/utils/data_utils.md)
|
|
99
|
+
* [utils.identification](drone_models/utils/identification.md)
|
|
100
|
+
"""
|
|
101
|
+
|
|
102
|
+
with mkdocs_gen_files.open("reference/SUMMARY.md", "w") as nav_file:
|
|
103
|
+
nav_file.write(summary)
|
|
@@ -0,0 +1,73 @@
|
|
|
1
|
+
"""drone-models: quadrotor dynamics models for estimation, control, and simulation.
|
|
2
|
+
|
|
3
|
+
This package provides numeric and symbolic quadrotor dynamics models at multiple
|
|
4
|
+
fidelity levels. Models are pure functions compatible with any Array API backend
|
|
5
|
+
(NumPy, JAX, PyTorch, etc.) and with CasADi for symbolic computation.
|
|
6
|
+
|
|
7
|
+
Use [parametrize][drone_models.parametrize] to bind a dynamics function to a named drone configuration,
|
|
8
|
+
and [available_models][drone_models.available_models] to enumerate all registered models.
|
|
9
|
+
"""
|
|
10
|
+
|
|
11
|
+
import os
|
|
12
|
+
import sys
|
|
13
|
+
from typing import Callable
|
|
14
|
+
|
|
15
|
+
# SciPy array API check. We use the most recent array API features, which require the
|
|
16
|
+
# SCIPY_ARRAY_API environment variable to be set to "1". This flag MUST be set before importing
|
|
17
|
+
# scipy, because scipy's C extensions cannot be unloaded once they have been imported. Therefore, we
|
|
18
|
+
# have to error out if the flag is not set. Otherwise, we immediately import scipy to ensure that no
|
|
19
|
+
# other package sets the flag to a different value before importing scipy.
|
|
20
|
+
|
|
21
|
+
if "scipy" in sys.modules and os.environ.get("SCIPY_ARRAY_API") != "1":
|
|
22
|
+
msg = """scipy has already been imported and the 'SCIPY_ARRAY_API' environment variable has not
|
|
23
|
+
been set. Please restart your Python session and set SCIPY_ARRAY_API="1" before importing any
|
|
24
|
+
packages that depend on scipy, or import this package first to automatically set the flag."""
|
|
25
|
+
raise RuntimeError(msg)
|
|
26
|
+
|
|
27
|
+
os.environ["SCIPY_ARRAY_API"] = "1"
|
|
28
|
+
import scipy # noqa: F401, ensure scipy uses array API features
|
|
29
|
+
|
|
30
|
+
from drone_models.core import parametrize
|
|
31
|
+
from drone_models.first_principles import dynamics as _first_principles_dynamics
|
|
32
|
+
from drone_models.so_rpy import dynamics as _so_rpy_dynamics
|
|
33
|
+
from drone_models.so_rpy_rotor import dynamics as _so_rpy_rotor_dynamics
|
|
34
|
+
from drone_models.so_rpy_rotor_drag import dynamics as _so_rpy_rotor_drag_dynamics
|
|
35
|
+
|
|
36
|
+
__all__ = ["parametrize", "available_models", "model_features"]
|
|
37
|
+
|
|
38
|
+
|
|
39
|
+
available_models: dict[str, Callable] = {
|
|
40
|
+
"first_principles": _first_principles_dynamics,
|
|
41
|
+
"so_rpy": _so_rpy_dynamics,
|
|
42
|
+
"so_rpy_rotor": _so_rpy_rotor_dynamics,
|
|
43
|
+
"so_rpy_rotor_drag": _so_rpy_rotor_drag_dynamics,
|
|
44
|
+
}
|
|
45
|
+
|
|
46
|
+
|
|
47
|
+
def model_features(model: Callable) -> dict[str, bool]:
|
|
48
|
+
"""Return the feature flags declared by a dynamics function.
|
|
49
|
+
|
|
50
|
+
Feature flags are set by the [supports][drone_models.core.supports] decorator on each
|
|
51
|
+
dynamics function and describe which optional inputs the model accepts.
|
|
52
|
+
|
|
53
|
+
Args:
|
|
54
|
+
model: A dynamics function, or a ``functools.partial`` wrapping one (as
|
|
55
|
+
returned by [parametrize][drone_models.parametrize]).
|
|
56
|
+
|
|
57
|
+
Returns:
|
|
58
|
+
A dict of feature names to booleans. Currently contains:
|
|
59
|
+
- ``"rotor_dynamics"``: ``True`` if the model accepts and integrates
|
|
60
|
+
``rotor_vel``, ``False`` if passing ``rotor_vel`` raises a
|
|
61
|
+
``ValueError``.
|
|
62
|
+
|
|
63
|
+
Example:
|
|
64
|
+
```python
|
|
65
|
+
from drone_models import model_features
|
|
66
|
+
from drone_models.first_principles import dynamics
|
|
67
|
+
|
|
68
|
+
model_features(dynamics) # {'rotor_dynamics': True}
|
|
69
|
+
```
|
|
70
|
+
"""
|
|
71
|
+
if hasattr(model, "func"): # Is a partial function
|
|
72
|
+
return model_features(model.func)
|
|
73
|
+
return getattr(model, "__drone_model_features__")
|
|
@@ -0,0 +1,8 @@
|
|
|
1
|
+
"""This file is to be remove later as soon as a proper typing is available by the official array-api."""
|
|
2
|
+
|
|
3
|
+
from typing import Any, TypeAlias
|
|
4
|
+
|
|
5
|
+
import numpy.typing as npt
|
|
6
|
+
|
|
7
|
+
Array: TypeAlias = Any # To be changed to array_api_typing later
|
|
8
|
+
ArrayLike: TypeAlias = Array | npt.ArrayLike
|
|
@@ -0,0 +1,163 @@
|
|
|
1
|
+
"""Core tools for registering and capability checking for the drone models."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
import inspect
|
|
6
|
+
import tomllib
|
|
7
|
+
import warnings
|
|
8
|
+
from functools import partial, wraps
|
|
9
|
+
from pathlib import Path
|
|
10
|
+
from typing import TYPE_CHECKING, Any, Callable, ParamSpec, TypeVar
|
|
11
|
+
|
|
12
|
+
import numpy as np
|
|
13
|
+
|
|
14
|
+
if TYPE_CHECKING:
|
|
15
|
+
from types import ModuleType
|
|
16
|
+
|
|
17
|
+
from drone_models._typing import Array # To be changed to array_api_typing later
|
|
18
|
+
|
|
19
|
+
|
|
20
|
+
F = TypeVar("F", bound=Callable[..., Any])
|
|
21
|
+
P = ParamSpec("P")
|
|
22
|
+
R = TypeVar("R")
|
|
23
|
+
|
|
24
|
+
|
|
25
|
+
def supports(rotor_dynamics: bool = True) -> Callable[[F], F]:
|
|
26
|
+
"""Decorator that declares which optional inputs a dynamics function supports.
|
|
27
|
+
|
|
28
|
+
Wraps the decorated function so that:
|
|
29
|
+
|
|
30
|
+
* If ``rotor_dynamics=False`` and the caller passes ``rotor_vel``, a
|
|
31
|
+
``ValueError`` is raised immediately.
|
|
32
|
+
* If ``rotor_dynamics=True`` and the caller omits ``rotor_vel``, a
|
|
33
|
+
``UserWarning`` is issued and the commanded value is used directly.
|
|
34
|
+
|
|
35
|
+
The decorator also attaches a ``__drone_model_features__`` attribute to the
|
|
36
|
+
wrapper, which [model_features][drone_models.model_features] reads.
|
|
37
|
+
|
|
38
|
+
Args:
|
|
39
|
+
rotor_dynamics: Whether the decorated function models rotor velocity
|
|
40
|
+
dynamics. Set to ``False`` for models that do not accept or integrate
|
|
41
|
+
``rotor_vel`` (e.g. ``so_rpy``). Defaults to ``True``.
|
|
42
|
+
|
|
43
|
+
Returns:
|
|
44
|
+
A decorator that wraps the dynamics function with the capability checks
|
|
45
|
+
described above.
|
|
46
|
+
"""
|
|
47
|
+
|
|
48
|
+
def decorator(fn: F) -> F:
|
|
49
|
+
@wraps(fn)
|
|
50
|
+
def wrapper(
|
|
51
|
+
pos: Array,
|
|
52
|
+
quat: Array,
|
|
53
|
+
vel: Array,
|
|
54
|
+
ang_vel: Array,
|
|
55
|
+
cmd: Array,
|
|
56
|
+
rotor_vel: Array | None = None,
|
|
57
|
+
*args: Any,
|
|
58
|
+
**kwargs: Any,
|
|
59
|
+
) -> tuple[Array, Array, Array, Array, Array | None]:
|
|
60
|
+
if not rotor_dynamics and rotor_vel is not None:
|
|
61
|
+
raise ValueError("Rotor dynamics not supported, but rotor_vel is provided.")
|
|
62
|
+
if rotor_dynamics and rotor_vel is None:
|
|
63
|
+
warnings.warn("Rotor velocity not provided, using commanded rotor velocity.")
|
|
64
|
+
return fn(pos, quat, vel, ang_vel, cmd, rotor_vel, *args, **kwargs)
|
|
65
|
+
|
|
66
|
+
wrapper.__drone_model_features__ = {"rotor_dynamics": rotor_dynamics}
|
|
67
|
+
|
|
68
|
+
return wrapper # type: ignore
|
|
69
|
+
|
|
70
|
+
return decorator
|
|
71
|
+
|
|
72
|
+
|
|
73
|
+
def parametrize(
|
|
74
|
+
fn: Callable[P, R], drone_model: str, xp: ModuleType | None = None, device: str | None = None
|
|
75
|
+
) -> Callable[P, R]:
|
|
76
|
+
"""Parametrize a dynamics function with the default dynamics parameters for a drone model.
|
|
77
|
+
|
|
78
|
+
Args:
|
|
79
|
+
fn: The dynamics function to parametrize.
|
|
80
|
+
drone_model: The drone model to use.
|
|
81
|
+
xp: The array API module to use. If not provided, numpy is used.
|
|
82
|
+
device: The device to use. If none, the device is inferred from the xp module.
|
|
83
|
+
|
|
84
|
+
Example:
|
|
85
|
+
```{ .python notest }
|
|
86
|
+
from drone_models.core import parametrize
|
|
87
|
+
from drone_models.first_principles import dynamics
|
|
88
|
+
|
|
89
|
+
dynamics_fn = parametrize(dynamics, drone_model="cf2x_L250")
|
|
90
|
+
pos_dot, quat_dot, vel_dot, ang_vel_dot, rotor_vel_dot = dynamics_fn(
|
|
91
|
+
pos=pos, quat=quat, vel=vel, ang_vel=ang_vel, cmd=cmd, rotor_vel=rotor_vel
|
|
92
|
+
)
|
|
93
|
+
```
|
|
94
|
+
|
|
95
|
+
Returns:
|
|
96
|
+
The parametrized dynamics function with all keyword argument only parameters filled in.
|
|
97
|
+
"""
|
|
98
|
+
try:
|
|
99
|
+
xp = np if xp is None else xp
|
|
100
|
+
physics = fn.__module__.split(".")[-2]
|
|
101
|
+
sig = inspect.signature(fn)
|
|
102
|
+
kwonly_params = [
|
|
103
|
+
name
|
|
104
|
+
for name, param in sig.parameters.items()
|
|
105
|
+
if param.kind == inspect.Parameter.KEYWORD_ONLY
|
|
106
|
+
]
|
|
107
|
+
params = load_params(physics, drone_model, xp=xp)
|
|
108
|
+
params = {k: xp.asarray(v, device=device) for k, v in params.items() if k in kwonly_params}
|
|
109
|
+
except KeyError as e:
|
|
110
|
+
raise KeyError(
|
|
111
|
+
f"Model `{physics}` does not exist in the parameter registry for drone `{drone_model}`"
|
|
112
|
+
) from e
|
|
113
|
+
except ValueError as e:
|
|
114
|
+
raise ValueError(f"Drone model `{drone_model}` not supported for `{physics}`") from e
|
|
115
|
+
return partial(fn, **params)
|
|
116
|
+
|
|
117
|
+
|
|
118
|
+
def load_params(physics: str, drone_model: str, xp: ModuleType | None = None) -> dict:
|
|
119
|
+
"""Load and merge physical and model-specific parameters for a drone configuration.
|
|
120
|
+
|
|
121
|
+
Reads parameters from two TOML files:
|
|
122
|
+
|
|
123
|
+
* ``drone_models/data/params.toml`` — physical parameters shared across all
|
|
124
|
+
models (mass, inertia, thrust curves, …).
|
|
125
|
+
* ``drone_models/<physics>/params.toml`` — model-specific coefficients
|
|
126
|
+
(e.g. fitted RPY coefficients for ``so_rpy``).
|
|
127
|
+
|
|
128
|
+
The two dicts are merged (model-specific values take precedence), and
|
|
129
|
+
``J_inv`` is computed from ``J`` and added to the result.
|
|
130
|
+
|
|
131
|
+
Args:
|
|
132
|
+
physics: Name of the model sub-package, e.g. ``"first_principles"``,
|
|
133
|
+
``"so_rpy"``, ``"so_rpy_rotor"``, or ``"so_rpy_rotor_drag"``.
|
|
134
|
+
drone_model: Name of the drone configuration, e.g. ``"cf2x_L250"``.
|
|
135
|
+
Must exist as a section in both TOML files.
|
|
136
|
+
xp: Array API module used to convert parameter values. If ``None``,
|
|
137
|
+
NumPy is used.
|
|
138
|
+
|
|
139
|
+
Returns:
|
|
140
|
+
A flat dict mapping parameter names to arrays (or scalars) in the
|
|
141
|
+
requested array namespace. Always contains at least ``mass``, ``J``,
|
|
142
|
+
``J_inv``, ``gravity_vec``, and the model-specific coefficients for
|
|
143
|
+
``physics``.
|
|
144
|
+
|
|
145
|
+
Raises:
|
|
146
|
+
KeyError: If ``drone_model`` is not found in either TOML file, or if
|
|
147
|
+
``physics`` does not correspond to a known sub-package.
|
|
148
|
+
"""
|
|
149
|
+
xp = np if xp is None else xp
|
|
150
|
+
with open(Path(__file__).parent / "data/params.toml", "rb") as f:
|
|
151
|
+
physical_params = tomllib.load(f)
|
|
152
|
+
if drone_model not in physical_params:
|
|
153
|
+
raise KeyError(f"Drone model `{drone_model}` not found in data/params.toml")
|
|
154
|
+
with open(Path(__file__).parent / f"{physics}/params.toml", "rb") as f:
|
|
155
|
+
model_params = tomllib.load(f)
|
|
156
|
+
if drone_model not in model_params:
|
|
157
|
+
raise KeyError(f"Drone model `{drone_model}` not found in model params.toml")
|
|
158
|
+
params = physical_params[drone_model] | model_params[drone_model]
|
|
159
|
+
# Make sure J_inv does not have a dtype fixed before conversion to xp arrays to avoid fixing it
|
|
160
|
+
# to np.float64 when other frameworks might prefer a different dtype.
|
|
161
|
+
params["J_inv"] = np.linalg.inv(params["J"]).tolist()
|
|
162
|
+
params = {k: xp.asarray(v) for k, v in params.items()} # if k in fields
|
|
163
|
+
return params
|
|
Binary file
|
|
Binary file
|
|
Binary file
|
|
Binary file
|
|
Binary file
|
|
Binary file
|
|
Binary file
|
|
Binary file
|
|
Binary file
|
|
Binary file
|
|
Binary file
|
|
Binary file
|
|
Binary file
|
|
Binary file
|
|
Binary file
|
|
Binary file
|
|
Binary file
|
|
Binary file
|
|
Binary file
|
|
Binary file
|
|
Binary file
|
|
Binary file
|