Minsnap Trajectories

Simple Python/Numpy implementation of the Minimum Snap Trajectory Generation algorithm.

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:

• Mellinger and Kumar's original Minimum Snap Trajectory Generator [1]

• Roy and Bry's numerically stable, unconstrained quadratic program [2]

and an important utility in generating quadrotor trajectories:

• Aerodynamic-effects-aware flatness map from flat outputs to quadrotor state and inputs

[1]: D. Mellinger and V. Kumar. Minimum snap trajectory generation and control for quadrotors. In Proc. Int. Conf. on Robotics and Automation, 2011

[2]: C. Richter, A. Bry, and N. Roy. Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments. In Int. Symposium on Robotics Research, 2013

Get Started

Preliminaries

Use the following convention to import this package

import minsnap_trajectories as ms

Derivatives on position are numbered from 0, as shown below

TLDR

Four key names

• ms.Waypoint: Time, position, velocity, etc. waypoint. A sequence of this defines the trajectory
• ms.generate_trajectory: Generates the piecewise-polynomial trajectory
• ms.compute_trajectory_derivatives: Samples the polynomial for position/velocity/acceleration, etc.
• ms.compute_quadrotor_trajectory: Directly compute quadrotor state/inputs along the trajectory

Full usage example

Define a sequence of position (and optionally velocity, acceleration, higher-order) references

refs = [
    ms.Waypoint(
        time=0.0,
        position=np.array([0.0, 0.0, 10.0]),
    ),
    ms.Waypoint(  # Any higher-order derivatives
        time=8.0,
        position=np.array([10.0, 0.0, 10.0]),
        velocity=np.array([0.0, 5.0, 0.0]),
        acceleration=np.array([0.1, 0.0, 0.0]),
    ),
    ms.Waypoint(  # Potentially leave intermediate-order derivatives unspecified
        time=16.0,
        position=np.array([20.0, 0.0, 10.0]),
        jerk=np.array([0.1, 0.0, 0.2]),
    ),
]

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

polys = ms.generate_trajectory(
    refs,
    degree=8,  # Polynomial degree
    idx_minimized_orders=(3, 4),
    num_continuous_orders=4,  # 4 orders: position, velocity, acceleration, jerk
    algorithm="closed-form",  # Or "constrained"
)

# Inspect the output
t = polys.time_reference
dt = polys.durations
cfs = polys.coefficients

Sample the polynomial trajectory to get position, velocity, acceleration (or higher-order) trajectories

t = np.linspace(0, 16, 100)
#  Sample 3 derivative orders (position, velocity, acceleration) -----v
pva = ms.compute_trajectory_derivatives(polys, t, 3)
position = pva[0, ...]
velocity = pva[1, ...]

num_orders is a count, not an index: pass 4 to also include jerk, 5 for snap, etc.

Or directly generate a quadrotor UAV trajectory

t = np.linspace(0, 15, 100)
states, inputs = ms.compute_quadrotor_trajectory(
    polys,
    t,
    vehicle_mass=1.0, # Quadrotor weight
    yaw="velocity", # Align yaw angle to quadrotor velocity
    drag_params=ms.RotorDragParameters(0.1, 0.2, 1.0),
)

Limitations

Tests are not nearly enough!

Existing tests show that this piecewise polynomial planner behaves identically to two of the more approachable MATLAB-based trajectory generator implementations by icsl-Jeon and symao.

1. The polynomial planner is not well-guarded against the case when the polynomial planning problem is overconstrained, i.e.

   • The polynomial degree is too low
   • Too many orders of derivatives are constrained to be continuous

2. The quadrotor trajectory generator is not extensively tested (the rotor-drag effect compensation function is even more so).

Until more extensive tests are available, use the following parameters in polynomial planning (they are the defaults)

• degree: From 5 to 15
• idx_minimized_orders: 4 (Minimum snap)
• num_continuous_orders: 3 (Just keep position/velocity/acceleration continuous)

Migrating to 0.3.0

• Requires Python 3.10+ (previously 3.11+), and the numpy/scipy floors are low enough to run against the versions Ubuntu 22.04 and ROS 2 Humble ship, so installing no longer replaces a distro-provided numpy or scipy.
• The closed-form solver now raises ValueError when degree + 1 is less than 2 * num_continuous_orders. It previously returned a trajectory that did not pass through its waypoints. Either raise degree to at least 2 * num_continuous_orders - 1, lower num_continuous_orders, or use algorithm="constrained", which has no such restriction.

Migrating to 0.2.0

• Requires Python 3.11+.
• 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).
• 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.