minsnap_trajectories 0.2.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.
- minsnap_trajectories-0.2.0/.claude/settings.local.json +7 -0
- minsnap_trajectories-0.2.0/.github/workflows/ci.yml +33 -0
- minsnap_trajectories-0.2.0/.gitignore +160 -0
- minsnap_trajectories-0.2.0/.python-version +1 -0
- minsnap_trajectories-0.2.0/.vscode/settings.json +7 -0
- minsnap_trajectories-0.2.0/LICENSE +21 -0
- minsnap_trajectories-0.2.0/PKG-INFO +160 -0
- minsnap_trajectories-0.2.0/README.md +139 -0
- minsnap_trajectories-0.2.0/example/example.py +95 -0
- minsnap_trajectories-0.2.0/example/minsnap_trajectories_example.png +0 -0
- minsnap_trajectories-0.2.0/pyproject.toml +60 -0
- minsnap_trajectories-0.2.0/src/minsnap_trajectories/__init__.py +21 -0
- minsnap_trajectories-0.2.0/src/minsnap_trajectories/minimum_snap.py +661 -0
- minsnap_trajectories-0.2.0/src/minsnap_trajectories/py.typed +0 -0
- minsnap_trajectories-0.2.0/tests/data/icsl_jeon_3d.mat +0 -0
- minsnap_trajectories-0.2.0/tests/data/symao_closed_form.mat +0 -0
- minsnap_trajectories-0.2.0/tests/data/symao_simple.mat +0 -0
- minsnap_trajectories-0.2.0/tests/test_minsnap.py +310 -0
- minsnap_trajectories-0.2.0/uv.lock +718 -0
|
@@ -0,0 +1,33 @@
|
|
|
1
|
+
name: CI
|
|
2
|
+
|
|
3
|
+
on:
|
|
4
|
+
push:
|
|
5
|
+
branches: [main]
|
|
6
|
+
pull_request:
|
|
7
|
+
branches: [main]
|
|
8
|
+
|
|
9
|
+
jobs:
|
|
10
|
+
lint:
|
|
11
|
+
runs-on: ubuntu-latest
|
|
12
|
+
steps:
|
|
13
|
+
- uses: actions/checkout@v4
|
|
14
|
+
- uses: astral-sh/setup-uv@v5
|
|
15
|
+
with:
|
|
16
|
+
python-version: "3.11"
|
|
17
|
+
- run: uv tool install ruff
|
|
18
|
+
- run: ruff check .
|
|
19
|
+
- run: ruff format --check .
|
|
20
|
+
|
|
21
|
+
test:
|
|
22
|
+
runs-on: ubuntu-latest
|
|
23
|
+
strategy:
|
|
24
|
+
fail-fast: false
|
|
25
|
+
matrix:
|
|
26
|
+
python-version: ["3.11", "3.12", "3.13"]
|
|
27
|
+
steps:
|
|
28
|
+
- uses: actions/checkout@v4
|
|
29
|
+
- uses: astral-sh/setup-uv@v5
|
|
30
|
+
with:
|
|
31
|
+
python-version: ${{ matrix.python-version }}
|
|
32
|
+
- run: uv sync --dev
|
|
33
|
+
- run: uv run pytest tests/
|
|
@@ -0,0 +1,160 @@
|
|
|
1
|
+
# Byte-compiled / optimized / DLL files
|
|
2
|
+
__pycache__/
|
|
3
|
+
*.py[cod]
|
|
4
|
+
*$py.class
|
|
5
|
+
|
|
6
|
+
# C extensions
|
|
7
|
+
*.so
|
|
8
|
+
|
|
9
|
+
# Distribution / packaging
|
|
10
|
+
.Python
|
|
11
|
+
build/
|
|
12
|
+
develop-eggs/
|
|
13
|
+
dist/
|
|
14
|
+
downloads/
|
|
15
|
+
eggs/
|
|
16
|
+
.eggs/
|
|
17
|
+
lib/
|
|
18
|
+
lib64/
|
|
19
|
+
parts/
|
|
20
|
+
sdist/
|
|
21
|
+
var/
|
|
22
|
+
wheels/
|
|
23
|
+
share/python-wheels/
|
|
24
|
+
*.egg-info/
|
|
25
|
+
.installed.cfg
|
|
26
|
+
*.egg
|
|
27
|
+
MANIFEST
|
|
28
|
+
|
|
29
|
+
# PyInstaller
|
|
30
|
+
# Usually these files are written by a python script from a template
|
|
31
|
+
# before PyInstaller builds the exe, so as to inject date/other infos into it.
|
|
32
|
+
*.manifest
|
|
33
|
+
*.spec
|
|
34
|
+
|
|
35
|
+
# Installer logs
|
|
36
|
+
pip-log.txt
|
|
37
|
+
pip-delete-this-directory.txt
|
|
38
|
+
|
|
39
|
+
# Unit test / coverage reports
|
|
40
|
+
htmlcov/
|
|
41
|
+
.tox/
|
|
42
|
+
.nox/
|
|
43
|
+
.coverage
|
|
44
|
+
.coverage.*
|
|
45
|
+
.cache
|
|
46
|
+
nosetests.xml
|
|
47
|
+
coverage.xml
|
|
48
|
+
*.cover
|
|
49
|
+
*.py,cover
|
|
50
|
+
.hypothesis/
|
|
51
|
+
.pytest_cache/
|
|
52
|
+
cover/
|
|
53
|
+
|
|
54
|
+
# Translations
|
|
55
|
+
*.mo
|
|
56
|
+
*.pot
|
|
57
|
+
|
|
58
|
+
# Django stuff:
|
|
59
|
+
*.log
|
|
60
|
+
local_settings.py
|
|
61
|
+
db.sqlite3
|
|
62
|
+
db.sqlite3-journal
|
|
63
|
+
|
|
64
|
+
# Flask stuff:
|
|
65
|
+
instance/
|
|
66
|
+
.webassets-cache
|
|
67
|
+
|
|
68
|
+
# Scrapy stuff:
|
|
69
|
+
.scrapy
|
|
70
|
+
|
|
71
|
+
# Sphinx documentation
|
|
72
|
+
docs/_build/
|
|
73
|
+
|
|
74
|
+
# PyBuilder
|
|
75
|
+
.pybuilder/
|
|
76
|
+
target/
|
|
77
|
+
|
|
78
|
+
# Jupyter Notebook
|
|
79
|
+
.ipynb_checkpoints
|
|
80
|
+
|
|
81
|
+
# IPython
|
|
82
|
+
profile_default/
|
|
83
|
+
ipython_config.py
|
|
84
|
+
|
|
85
|
+
# pyenv
|
|
86
|
+
# For a library or package, you might want to ignore these files since the code is
|
|
87
|
+
# intended to run in multiple environments; otherwise, check them in:
|
|
88
|
+
# .python-version
|
|
89
|
+
|
|
90
|
+
# pipenv
|
|
91
|
+
# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
|
|
92
|
+
# However, in case of collaboration, if having platform-specific dependencies or dependencies
|
|
93
|
+
# having no cross-platform support, pipenv may install dependencies that don't work, or not
|
|
94
|
+
# install all needed dependencies.
|
|
95
|
+
#Pipfile.lock
|
|
96
|
+
|
|
97
|
+
# poetry
|
|
98
|
+
# Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control.
|
|
99
|
+
# This is especially recommended for binary packages to ensure reproducibility, and is more
|
|
100
|
+
# commonly ignored for libraries.
|
|
101
|
+
# https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control
|
|
102
|
+
#poetry.lock
|
|
103
|
+
|
|
104
|
+
# pdm
|
|
105
|
+
# Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control.
|
|
106
|
+
#pdm.lock
|
|
107
|
+
# pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it
|
|
108
|
+
# in version control.
|
|
109
|
+
# https://pdm.fming.dev/#use-with-ide
|
|
110
|
+
.pdm.toml
|
|
111
|
+
|
|
112
|
+
# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm
|
|
113
|
+
__pypackages__/
|
|
114
|
+
|
|
115
|
+
# Celery stuff
|
|
116
|
+
celerybeat-schedule
|
|
117
|
+
celerybeat.pid
|
|
118
|
+
|
|
119
|
+
# SageMath parsed files
|
|
120
|
+
*.sage.py
|
|
121
|
+
|
|
122
|
+
# Environments
|
|
123
|
+
.env
|
|
124
|
+
.venv
|
|
125
|
+
env/
|
|
126
|
+
venv/
|
|
127
|
+
ENV/
|
|
128
|
+
env.bak/
|
|
129
|
+
venv.bak/
|
|
130
|
+
|
|
131
|
+
# Spyder project settings
|
|
132
|
+
.spyderproject
|
|
133
|
+
.spyproject
|
|
134
|
+
|
|
135
|
+
# Rope project settings
|
|
136
|
+
.ropeproject
|
|
137
|
+
|
|
138
|
+
# mkdocs documentation
|
|
139
|
+
/site
|
|
140
|
+
|
|
141
|
+
# mypy
|
|
142
|
+
.mypy_cache/
|
|
143
|
+
.dmypy.json
|
|
144
|
+
dmypy.json
|
|
145
|
+
|
|
146
|
+
# Pyre type checker
|
|
147
|
+
.pyre/
|
|
148
|
+
|
|
149
|
+
# pytype static type analyzer
|
|
150
|
+
.pytype/
|
|
151
|
+
|
|
152
|
+
# Cython debug symbols
|
|
153
|
+
cython_debug/
|
|
154
|
+
|
|
155
|
+
# PyCharm
|
|
156
|
+
# JetBrains specific template is maintained in a separate JetBrains.gitignore that can
|
|
157
|
+
# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
|
|
158
|
+
# and can be added to the global gitignore or merged into this file. For a more nuclear
|
|
159
|
+
# option (not recommended) you can uncomment the following to ignore the entire idea folder.
|
|
160
|
+
#.idea/
|
|
@@ -0,0 +1 @@
|
|
|
1
|
+
3.11
|
|
@@ -0,0 +1,21 @@
|
|
|
1
|
+
MIT License
|
|
2
|
+
|
|
3
|
+
Copyright (c) 2024 hs293go
|
|
4
|
+
|
|
5
|
+
Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
6
|
+
of this software and associated documentation files (the "Software"), to deal
|
|
7
|
+
in the Software without restriction, including without limitation the rights
|
|
8
|
+
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
9
|
+
copies of the Software, and to permit persons to whom the Software is
|
|
10
|
+
furnished to do so, subject to the following conditions:
|
|
11
|
+
|
|
12
|
+
The above copyright notice and this permission notice shall be included in all
|
|
13
|
+
copies or substantial portions of the Software.
|
|
14
|
+
|
|
15
|
+
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
16
|
+
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
17
|
+
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
18
|
+
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
19
|
+
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
20
|
+
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
21
|
+
SOFTWARE.
|
|
@@ -0,0 +1,160 @@
|
|
|
1
|
+
Metadata-Version: 2.4
|
|
2
|
+
Name: minsnap_trajectories
|
|
3
|
+
Version: 0.2.0
|
|
4
|
+
Summary: Simple minimum-snap trajectory generator
|
|
5
|
+
Project-URL: Homepage, https://github.com/Hs293Go/minsnap_trajectories
|
|
6
|
+
Author-email: H S Helson Go <hei.go@mail.utoronto.ca>
|
|
7
|
+
License-Expression: MIT
|
|
8
|
+
License-File: LICENSE
|
|
9
|
+
Keywords: differential flatness,minimum snap,path planning,trajectory generation
|
|
10
|
+
Classifier: License :: OSI Approved :: MIT License
|
|
11
|
+
Classifier: Operating System :: OS Independent
|
|
12
|
+
Classifier: Programming Language :: Python :: 3
|
|
13
|
+
Classifier: Programming Language :: Python :: 3.11
|
|
14
|
+
Classifier: Programming Language :: Python :: 3.12
|
|
15
|
+
Classifier: Programming Language :: Python :: 3.13
|
|
16
|
+
Classifier: Typing :: Typed
|
|
17
|
+
Requires-Python: >=3.11
|
|
18
|
+
Requires-Dist: numpy>=2.4.4
|
|
19
|
+
Requires-Dist: scipy>=1.17.1
|
|
20
|
+
Description-Content-Type: text/markdown
|
|
21
|
+
|
|
22
|
+
# Minsnap Trajectories
|
|
23
|
+
|
|
24
|
+
Simple Python/Numpy implementation of the Minimum Snap Trajectory Generation algorithm.
|
|
25
|
+
|
|
26
|
+
This package addresses the shortage of turnkey minsnap trajectory generator implementations on PyPI. It implements two of the most time-tested algorithms in this area:
|
|
27
|
+
|
|
28
|
+
- Mellinger and Kumar's original Minimum Snap Trajectory Generator [[1]](#1)
|
|
29
|
+
|
|
30
|
+
- Roy and Bry's numerically stable, unconstrained quadratic program [[2]](#2)
|
|
31
|
+
|
|
32
|
+
and an important utility in generating quadrotor trajectories:
|
|
33
|
+
|
|
34
|
+
- [Aerodynamic-effects-aware flatness map](https://github.com/ZJU-FAST-Lab/GCOPTER/blob/main/misc/flatness.pdf) from flat outputs to quadrotor state and inputs
|
|
35
|
+
|
|
36
|
+
---
|
|
37
|
+
|
|
38
|
+
<a id="1">[1]</a>: D. Mellinger and V. Kumar. Minimum snap trajectory generation and control for quadrotors.
|
|
39
|
+
In *Proc. Int. Conf. on Robotics and Automation*, 2011
|
|
40
|
+
|
|
41
|
+
<a id="2">[2]</a>: C. Richter, A. Bry, and N. Roy. Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments.
|
|
42
|
+
In *Int. Symposium on Robotics Research*, 2013
|
|
43
|
+
|
|
44
|
+
## Get Started
|
|
45
|
+
|
|
46
|
+
### Preliminaries
|
|
47
|
+
|
|
48
|
+
Use the following convention to import this package
|
|
49
|
+
|
|
50
|
+
``` python
|
|
51
|
+
import minsnap_trajectories as ms
|
|
52
|
+
```
|
|
53
|
+
|
|
54
|
+
Derivatives on position are numbered from 0, as shown below
|
|
55
|
+
|
|
56
|
+
| Orders (# of derivatives) | 0 | 1 | 2 | 3 | 4 |
|
|
57
|
+
| ------------------------- | -------- | -------- | ------------ | ---- | ---- |
|
|
58
|
+
| Quantity | Position | Velocity | Acceleration | Jerk | Snap |
|
|
59
|
+
|
|
60
|
+
### TLDR
|
|
61
|
+
|
|
62
|
+
Four key names
|
|
63
|
+
|
|
64
|
+
- `ms.Waypoint`: Time, position, velocity, etc. waypoint. A sequence of this defines the trajectory
|
|
65
|
+
- `ms.generate_trajectory`: Generates the piecewise-polynomial trajectory
|
|
66
|
+
- `ms.compute_trajectory_derivatives`: Samples the polynomial for position/velocity/acceleration, etc.
|
|
67
|
+
- `ms.compute_quadrotor_trajectory`: Directly compute quadrotor state/inputs along the trajectory
|
|
68
|
+
|
|
69
|
+
### Full usage example
|
|
70
|
+
|
|
71
|
+

|
|
72
|
+
|
|
73
|
+
Define a sequence of position (and optionally velocity, acceleration, higher-order) references
|
|
74
|
+
|
|
75
|
+
``` python
|
|
76
|
+
refs = [
|
|
77
|
+
ms.Waypoint(
|
|
78
|
+
time=0.0,
|
|
79
|
+
position=np.array([0.0, 0.0, 10.0]),
|
|
80
|
+
),
|
|
81
|
+
ms.Waypoint( # Any higher-order derivatives
|
|
82
|
+
time=8.0,
|
|
83
|
+
position=np.array([10.0, 0.0, 10.0]),
|
|
84
|
+
velocity=np.array([0.0, 5.0, 0.0]),
|
|
85
|
+
acceleration=np.array([0.1, 0.0, 0.0]),
|
|
86
|
+
),
|
|
87
|
+
ms.Waypoint( # Potentially leave intermediate-order derivatives unspecified
|
|
88
|
+
time=16.0,
|
|
89
|
+
position=np.array([20.0, 0.0, 10.0]),
|
|
90
|
+
jerk=np.array([0.1, 0.0, 0.2]),
|
|
91
|
+
),
|
|
92
|
+
]
|
|
93
|
+
```
|
|
94
|
+
|
|
95
|
+
Generate a piecewise polynomial trajectory using Roy and Bry's closed form solution, minimizing jerk (order-3) AND snap (order-4) while constraining position, velocity, etc... up to jerk (orders 0 to 3) to be continuous
|
|
96
|
+
|
|
97
|
+
``` python
|
|
98
|
+
polys = ms.generate_trajectory(
|
|
99
|
+
refs,
|
|
100
|
+
degree=8, # Polynomial degree
|
|
101
|
+
idx_minimized_orders=(3, 4),
|
|
102
|
+
num_continuous_orders=4, # 4 orders: position, velocity, acceleration, jerk
|
|
103
|
+
algorithm="closed-form", # Or "constrained"
|
|
104
|
+
)
|
|
105
|
+
|
|
106
|
+
# Inspect the output
|
|
107
|
+
t = polys.time_reference
|
|
108
|
+
dt = polys.durations
|
|
109
|
+
cfs = polys.coefficients
|
|
110
|
+
```
|
|
111
|
+
|
|
112
|
+
Sample the polynomial trajectory to get position, velocity, acceleration (or higher-order) trajectories
|
|
113
|
+
|
|
114
|
+
``` python
|
|
115
|
+
t = np.linspace(0, 16, 100)
|
|
116
|
+
# Sample 3 derivative orders (position, velocity, acceleration) -----v
|
|
117
|
+
pva = ms.compute_trajectory_derivatives(polys, t, 3)
|
|
118
|
+
position = pva[0, ...]
|
|
119
|
+
velocity = pva[1, ...]
|
|
120
|
+
```
|
|
121
|
+
|
|
122
|
+
`num_orders` is a count, not an index: pass `4` to also include jerk, `5` for snap, etc.
|
|
123
|
+
|
|
124
|
+
Or directly generate a quadrotor UAV trajectory
|
|
125
|
+
|
|
126
|
+
``` python
|
|
127
|
+
t = np.linspace(0, 15, 100)
|
|
128
|
+
states, inputs = ms.compute_quadrotor_trajectory(
|
|
129
|
+
polys,
|
|
130
|
+
t,
|
|
131
|
+
vehicle_mass=1.0, # Quadrotor weight
|
|
132
|
+
yaw="velocity", # Align yaw angle to quadrotor velocity
|
|
133
|
+
drag_params=ms.RotorDragParameters(0.1, 0.2, 1.0),
|
|
134
|
+
)
|
|
135
|
+
```
|
|
136
|
+
|
|
137
|
+
## Limitations
|
|
138
|
+
|
|
139
|
+
Tests are not nearly enough!
|
|
140
|
+
|
|
141
|
+
Existing tests show that this piecewise polynomial planner behaves identically to two of the more *approachable* MATLAB-based trajectory generator implementations by [icsl-Jeon](https://github.com/icsl-Jeon/traj_gen-matlab) and [symao](https://github.com/symao/minimum_snap_trajectory_generation).
|
|
142
|
+
|
|
143
|
+
1. The polynomial planner is not well-guarded against the case when the polynomial planning problem is overconstrained, i.e.
|
|
144
|
+
|
|
145
|
+
- The polynomial degree is too low
|
|
146
|
+
- Too many orders of derivatives are constrained to be continuous
|
|
147
|
+
|
|
148
|
+
2. The quadrotor trajectory generator is not extensively tested (the rotor-drag effect compensation function is even more so).
|
|
149
|
+
|
|
150
|
+
Until more extensive tests are available, use the following parameters in polynomial planning (they are the defaults)
|
|
151
|
+
|
|
152
|
+
- `degree`: From 5 to 15
|
|
153
|
+
- `idx_minimized_orders`: 4 (Minimum snap)
|
|
154
|
+
- `num_continuous_orders`: 3 (Just keep position/velocity/acceleration continuous)
|
|
155
|
+
|
|
156
|
+
## Migrating to 0.2.0
|
|
157
|
+
|
|
158
|
+
- Requires Python 3.11+.
|
|
159
|
+
- `Waypoint` is now a frozen dataclass instead of a `dict` subclass. The constructor signature is unchanged, and all of `position`, `velocity`, `acceleration`, `jerk`, `snap`, and `time` are exposed as attributes (previously `jerk` and `snap` were silently dropped from the property accessors). Code that accessed waypoints by dict-key (e.g. `wp[0]`) must switch to attribute access (e.g. `wp.position`).
|
|
160
|
+
- The third positional argument to `compute_trajectory_derivatives` was renamed from `order` to `num_orders` to reflect that it counts derivative orders rather than indexing into them. Positional callers are unaffected.
|
|
@@ -0,0 +1,139 @@
|
|
|
1
|
+
# Minsnap Trajectories
|
|
2
|
+
|
|
3
|
+
Simple Python/Numpy implementation of the Minimum Snap Trajectory Generation algorithm.
|
|
4
|
+
|
|
5
|
+
This package addresses the shortage of turnkey minsnap trajectory generator implementations on PyPI. It implements two of the most time-tested algorithms in this area:
|
|
6
|
+
|
|
7
|
+
- Mellinger and Kumar's original Minimum Snap Trajectory Generator [[1]](#1)
|
|
8
|
+
|
|
9
|
+
- Roy and Bry's numerically stable, unconstrained quadratic program [[2]](#2)
|
|
10
|
+
|
|
11
|
+
and an important utility in generating quadrotor trajectories:
|
|
12
|
+
|
|
13
|
+
- [Aerodynamic-effects-aware flatness map](https://github.com/ZJU-FAST-Lab/GCOPTER/blob/main/misc/flatness.pdf) from flat outputs to quadrotor state and inputs
|
|
14
|
+
|
|
15
|
+
---
|
|
16
|
+
|
|
17
|
+
<a id="1">[1]</a>: D. Mellinger and V. Kumar. Minimum snap trajectory generation and control for quadrotors.
|
|
18
|
+
In *Proc. Int. Conf. on Robotics and Automation*, 2011
|
|
19
|
+
|
|
20
|
+
<a id="2">[2]</a>: C. Richter, A. Bry, and N. Roy. Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments.
|
|
21
|
+
In *Int. Symposium on Robotics Research*, 2013
|
|
22
|
+
|
|
23
|
+
## Get Started
|
|
24
|
+
|
|
25
|
+
### Preliminaries
|
|
26
|
+
|
|
27
|
+
Use the following convention to import this package
|
|
28
|
+
|
|
29
|
+
``` python
|
|
30
|
+
import minsnap_trajectories as ms
|
|
31
|
+
```
|
|
32
|
+
|
|
33
|
+
Derivatives on position are numbered from 0, as shown below
|
|
34
|
+
|
|
35
|
+
| Orders (# of derivatives) | 0 | 1 | 2 | 3 | 4 |
|
|
36
|
+
| ------------------------- | -------- | -------- | ------------ | ---- | ---- |
|
|
37
|
+
| Quantity | Position | Velocity | Acceleration | Jerk | Snap |
|
|
38
|
+
|
|
39
|
+
### TLDR
|
|
40
|
+
|
|
41
|
+
Four key names
|
|
42
|
+
|
|
43
|
+
- `ms.Waypoint`: Time, position, velocity, etc. waypoint. A sequence of this defines the trajectory
|
|
44
|
+
- `ms.generate_trajectory`: Generates the piecewise-polynomial trajectory
|
|
45
|
+
- `ms.compute_trajectory_derivatives`: Samples the polynomial for position/velocity/acceleration, etc.
|
|
46
|
+
- `ms.compute_quadrotor_trajectory`: Directly compute quadrotor state/inputs along the trajectory
|
|
47
|
+
|
|
48
|
+
### Full usage example
|
|
49
|
+
|
|
50
|
+

|
|
51
|
+
|
|
52
|
+
Define a sequence of position (and optionally velocity, acceleration, higher-order) references
|
|
53
|
+
|
|
54
|
+
``` python
|
|
55
|
+
refs = [
|
|
56
|
+
ms.Waypoint(
|
|
57
|
+
time=0.0,
|
|
58
|
+
position=np.array([0.0, 0.0, 10.0]),
|
|
59
|
+
),
|
|
60
|
+
ms.Waypoint( # Any higher-order derivatives
|
|
61
|
+
time=8.0,
|
|
62
|
+
position=np.array([10.0, 0.0, 10.0]),
|
|
63
|
+
velocity=np.array([0.0, 5.0, 0.0]),
|
|
64
|
+
acceleration=np.array([0.1, 0.0, 0.0]),
|
|
65
|
+
),
|
|
66
|
+
ms.Waypoint( # Potentially leave intermediate-order derivatives unspecified
|
|
67
|
+
time=16.0,
|
|
68
|
+
position=np.array([20.0, 0.0, 10.0]),
|
|
69
|
+
jerk=np.array([0.1, 0.0, 0.2]),
|
|
70
|
+
),
|
|
71
|
+
]
|
|
72
|
+
```
|
|
73
|
+
|
|
74
|
+
Generate a piecewise polynomial trajectory using Roy and Bry's closed form solution, minimizing jerk (order-3) AND snap (order-4) while constraining position, velocity, etc... up to jerk (orders 0 to 3) to be continuous
|
|
75
|
+
|
|
76
|
+
``` python
|
|
77
|
+
polys = ms.generate_trajectory(
|
|
78
|
+
refs,
|
|
79
|
+
degree=8, # Polynomial degree
|
|
80
|
+
idx_minimized_orders=(3, 4),
|
|
81
|
+
num_continuous_orders=4, # 4 orders: position, velocity, acceleration, jerk
|
|
82
|
+
algorithm="closed-form", # Or "constrained"
|
|
83
|
+
)
|
|
84
|
+
|
|
85
|
+
# Inspect the output
|
|
86
|
+
t = polys.time_reference
|
|
87
|
+
dt = polys.durations
|
|
88
|
+
cfs = polys.coefficients
|
|
89
|
+
```
|
|
90
|
+
|
|
91
|
+
Sample the polynomial trajectory to get position, velocity, acceleration (or higher-order) trajectories
|
|
92
|
+
|
|
93
|
+
``` python
|
|
94
|
+
t = np.linspace(0, 16, 100)
|
|
95
|
+
# Sample 3 derivative orders (position, velocity, acceleration) -----v
|
|
96
|
+
pva = ms.compute_trajectory_derivatives(polys, t, 3)
|
|
97
|
+
position = pva[0, ...]
|
|
98
|
+
velocity = pva[1, ...]
|
|
99
|
+
```
|
|
100
|
+
|
|
101
|
+
`num_orders` is a count, not an index: pass `4` to also include jerk, `5` for snap, etc.
|
|
102
|
+
|
|
103
|
+
Or directly generate a quadrotor UAV trajectory
|
|
104
|
+
|
|
105
|
+
``` python
|
|
106
|
+
t = np.linspace(0, 15, 100)
|
|
107
|
+
states, inputs = ms.compute_quadrotor_trajectory(
|
|
108
|
+
polys,
|
|
109
|
+
t,
|
|
110
|
+
vehicle_mass=1.0, # Quadrotor weight
|
|
111
|
+
yaw="velocity", # Align yaw angle to quadrotor velocity
|
|
112
|
+
drag_params=ms.RotorDragParameters(0.1, 0.2, 1.0),
|
|
113
|
+
)
|
|
114
|
+
```
|
|
115
|
+
|
|
116
|
+
## Limitations
|
|
117
|
+
|
|
118
|
+
Tests are not nearly enough!
|
|
119
|
+
|
|
120
|
+
Existing tests show that this piecewise polynomial planner behaves identically to two of the more *approachable* MATLAB-based trajectory generator implementations by [icsl-Jeon](https://github.com/icsl-Jeon/traj_gen-matlab) and [symao](https://github.com/symao/minimum_snap_trajectory_generation).
|
|
121
|
+
|
|
122
|
+
1. The polynomial planner is not well-guarded against the case when the polynomial planning problem is overconstrained, i.e.
|
|
123
|
+
|
|
124
|
+
- The polynomial degree is too low
|
|
125
|
+
- Too many orders of derivatives are constrained to be continuous
|
|
126
|
+
|
|
127
|
+
2. The quadrotor trajectory generator is not extensively tested (the rotor-drag effect compensation function is even more so).
|
|
128
|
+
|
|
129
|
+
Until more extensive tests are available, use the following parameters in polynomial planning (they are the defaults)
|
|
130
|
+
|
|
131
|
+
- `degree`: From 5 to 15
|
|
132
|
+
- `idx_minimized_orders`: 4 (Minimum snap)
|
|
133
|
+
- `num_continuous_orders`: 3 (Just keep position/velocity/acceleration continuous)
|
|
134
|
+
|
|
135
|
+
## Migrating to 0.2.0
|
|
136
|
+
|
|
137
|
+
- Requires Python 3.11+.
|
|
138
|
+
- `Waypoint` is now a frozen dataclass instead of a `dict` subclass. The constructor signature is unchanged, and all of `position`, `velocity`, `acceleration`, `jerk`, `snap`, and `time` are exposed as attributes (previously `jerk` and `snap` were silently dropped from the property accessors). Code that accessed waypoints by dict-key (e.g. `wp[0]`) must switch to attribute access (e.g. `wp.position`).
|
|
139
|
+
- The third positional argument to `compute_trajectory_derivatives` was renamed from `order` to `num_orders` to reflect that it counts derivative orders rather than indexing into them. Positional callers are unaffected.
|
|
@@ -0,0 +1,95 @@
|
|
|
1
|
+
"""
|
|
2
|
+
Copyright © 2024 Hs293Go
|
|
3
|
+
|
|
4
|
+
Permission is hereby granted, free of charge, to any person obtaining
|
|
5
|
+
a copy of this software and associated documentation files (the "Software"),
|
|
6
|
+
to deal in the Software without restriction, including without limitation
|
|
7
|
+
the rights to use, copy, modify, merge, publish, distribute, sublicense,
|
|
8
|
+
and/or sell copies of the Software, and to permit persons to whom the
|
|
9
|
+
Software is furnished to do so, subject to the following conditions:
|
|
10
|
+
|
|
11
|
+
The above copyright notice and this permission notice shall be included
|
|
12
|
+
in all copies or substantial portions of the Software.
|
|
13
|
+
|
|
14
|
+
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
|
|
15
|
+
EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
|
|
16
|
+
OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
|
|
17
|
+
IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
|
|
18
|
+
DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
|
|
19
|
+
TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE
|
|
20
|
+
OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
|
21
|
+
"""
|
|
22
|
+
|
|
23
|
+
import os.path
|
|
24
|
+
|
|
25
|
+
import matplotlib.pyplot as plt
|
|
26
|
+
import numpy as np
|
|
27
|
+
|
|
28
|
+
import minsnap_trajectories as ms
|
|
29
|
+
|
|
30
|
+
|
|
31
|
+
def main():
|
|
32
|
+
refs = [
|
|
33
|
+
ms.Waypoint(
|
|
34
|
+
time=0.0,
|
|
35
|
+
position=np.array([0.0, 0.0, 10.0]),
|
|
36
|
+
),
|
|
37
|
+
ms.Waypoint( # Any higher-order derivatives
|
|
38
|
+
time=8.0,
|
|
39
|
+
position=np.array([10.0, 0.0, 10.0]),
|
|
40
|
+
velocity=np.array([0.0, 5.0, 0.0]),
|
|
41
|
+
acceleration=np.array([0.1, 0.0, 0.0]),
|
|
42
|
+
),
|
|
43
|
+
ms.Waypoint( # Potentially leave intermediate-order derivatives unspecified
|
|
44
|
+
time=16.0,
|
|
45
|
+
position=np.array([20.0, 0.0, 10.0]),
|
|
46
|
+
jerk=np.array([0.1, 0.0, 0.2]),
|
|
47
|
+
),
|
|
48
|
+
]
|
|
49
|
+
|
|
50
|
+
polys = ms.generate_trajectory(
|
|
51
|
+
refs,
|
|
52
|
+
degree=8, # Polynomial degree
|
|
53
|
+
idx_minimized_orders=(3, 4), # Minimize derivatives in these orders (>= 2)
|
|
54
|
+
num_continuous_orders=4, # Constrain continuity through jerk so the waypoint
|
|
55
|
+
# jerk hint above is honored
|
|
56
|
+
algorithm="closed-form", # Or "constrained"
|
|
57
|
+
)
|
|
58
|
+
|
|
59
|
+
t = np.linspace(0, 16, 100)
|
|
60
|
+
# Sample 3 derivative orders (position, velocity, acceleration) -----v
|
|
61
|
+
pva = ms.compute_trajectory_derivatives(polys, t, 3)
|
|
62
|
+
position, *_ = pva
|
|
63
|
+
|
|
64
|
+
fig, ax = plt.subplots(subplot_kw={"projection": "3d"}, figsize=(5, 4))
|
|
65
|
+
ax.plot(position[:, 0], position[:, 1], position[:, 2], label="Position Trajectory")
|
|
66
|
+
|
|
67
|
+
position_waypoints = np.array([it.position for it in refs])
|
|
68
|
+
ax.plot(
|
|
69
|
+
position_waypoints[:, 0],
|
|
70
|
+
position_waypoints[:, 1],
|
|
71
|
+
position_waypoints[:, 2],
|
|
72
|
+
"ro",
|
|
73
|
+
label="Position Waypoints",
|
|
74
|
+
)
|
|
75
|
+
ax.quiver(
|
|
76
|
+
*refs[1].position,
|
|
77
|
+
*refs[1].velocity,
|
|
78
|
+
color="g",
|
|
79
|
+
label="Velocity specified at waypoint 1",
|
|
80
|
+
)
|
|
81
|
+
ax.set_zlim(8, 12)
|
|
82
|
+
ax.set_xlabel("X (m)")
|
|
83
|
+
ax.set_ylabel("Y (m)")
|
|
84
|
+
ax.set_zlabel("Z (m)")
|
|
85
|
+
ax.legend(loc="upper right")
|
|
86
|
+
fig.tight_layout()
|
|
87
|
+
out_dir = "example"
|
|
88
|
+
if os.path.isdir(out_dir):
|
|
89
|
+
fig.savefig(os.path.join(out_dir, "minsnap_trajectories_example.png"))
|
|
90
|
+
else:
|
|
91
|
+
plt.show()
|
|
92
|
+
|
|
93
|
+
|
|
94
|
+
if __name__ == "__main__":
|
|
95
|
+
main()
|
|
Binary file
|
|
@@ -0,0 +1,60 @@
|
|
|
1
|
+
[build-system]
|
|
2
|
+
requires = ["hatchling"]
|
|
3
|
+
build-backend = "hatchling.build"
|
|
4
|
+
|
|
5
|
+
[project]
|
|
6
|
+
name = "minsnap_trajectories"
|
|
7
|
+
version = "0.2.0"
|
|
8
|
+
description = "Simple minimum-snap trajectory generator"
|
|
9
|
+
readme = "README.md"
|
|
10
|
+
authors = [{ name = "H S Helson Go", email = "hei.go@mail.utoronto.ca" }]
|
|
11
|
+
license = "MIT"
|
|
12
|
+
requires-python = ">=3.11"
|
|
13
|
+
keywords = [
|
|
14
|
+
"trajectory generation",
|
|
15
|
+
"path planning",
|
|
16
|
+
"minimum snap",
|
|
17
|
+
"differential flatness",
|
|
18
|
+
]
|
|
19
|
+
classifiers = [
|
|
20
|
+
"Programming Language :: Python :: 3",
|
|
21
|
+
"Programming Language :: Python :: 3.11",
|
|
22
|
+
"Programming Language :: Python :: 3.12",
|
|
23
|
+
"Programming Language :: Python :: 3.13",
|
|
24
|
+
"License :: OSI Approved :: MIT License",
|
|
25
|
+
"Operating System :: OS Independent",
|
|
26
|
+
"Typing :: Typed",
|
|
27
|
+
]
|
|
28
|
+
dependencies = ["numpy>=2.4.4", "scipy>=1.17.1"]
|
|
29
|
+
|
|
30
|
+
[dependency-groups]
|
|
31
|
+
dev = [
|
|
32
|
+
"hypothesis>=6.152.6",
|
|
33
|
+
"matplotlib>=3.10.9",
|
|
34
|
+
"pytest>=9.0.3",
|
|
35
|
+
"ruff>=0.15.12",
|
|
36
|
+
]
|
|
37
|
+
|
|
38
|
+
[project.urls]
|
|
39
|
+
Homepage = "https://github.com/Hs293Go/minsnap_trajectories"
|
|
40
|
+
|
|
41
|
+
[tool.hatch.build.targets.wheel]
|
|
42
|
+
packages = ["src/minsnap_trajectories"]
|
|
43
|
+
|
|
44
|
+
[tool.pytest.ini_options]
|
|
45
|
+
pythonpath = "src"
|
|
46
|
+
filterwarnings = "ignore::DeprecationWarning"
|
|
47
|
+
|
|
48
|
+
[tool.ruff]
|
|
49
|
+
line-length = 100
|
|
50
|
+
target-version = "py311"
|
|
51
|
+
|
|
52
|
+
[tool.ruff.lint]
|
|
53
|
+
select = ["E", "F", "W", "I", "UP", "B", "SIM", "NPY", "RUF"]
|
|
54
|
+
ignore = [
|
|
55
|
+
"E741", # ambiguous variable names (l, I) — common in linear-algebra code
|
|
56
|
+
"RUF012", # mutable class defaults — not relevant here
|
|
57
|
+
]
|
|
58
|
+
|
|
59
|
+
[tool.ruff.lint.per-file-ignores]
|
|
60
|
+
"tests/*" = ["B011"]
|