# Minimum Snap Waypoint Planning Using OSQP in Peng: Implementation Guide

> Learn minimum snap waypoint planning using OSQP in Peng. This guide details the implementation, minimizing snap via quadratic programming with constraints in Rust.

- Repository: [Yang Zhou/peng](https://github.com/makeecat/peng)
- Tags: how-to-guide
- Published: 2026-03-06

---

**Peng implements minimum snap waypoint planning by formulating a quadratic programming problem solved via OSQP, where the fourth derivative of position (snap) is minimized subject to waypoint constraints and dynamic limits in [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs).**

The Peng repository is a Rust-based quadrotor trajectory planning library that generates smooth, dynamically feasible paths through cluttered environments. At its core, the `QPpolyTrajPlanner` struct in [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs) implements minimum snap waypoint planning using OSQP, an open-source operator splitting QP solver. This approach minimizes the fourth derivative of position to produce trajectories suitable for aggressive aerial maneuvers while respecting physical constraints.

## The Minimum Snap Objective

In aerial robotics, **snap** refers to the fourth derivative of position with respect to time (following velocity, acceleration, and jerk). Minimizing snap produces trajectories that are smooth up to the fourth derivative, making them dynamically feasible for quadrotors that cannot instantaneously change their snap.

Peng configures this optimization through the `min_deriv` parameter in the `QPpolyTrajPlanner` constructor. Setting `min_deriv = 3` (zero-based indexing) targets the fourth derivative:

```rust
let min_deriv = 3; // fourth derivative → snap
let planner = QPpolyTrajPlanner::new(
    waypoints,
    segment_times,
    polyorder,
    min_deriv,      // ← minimizes snap
    smooth_upto,
    max_velocity,
    max_acceleration,
    start_time,
    dt,
).unwrap();

```

As defined in [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs) at lines 1734-1740, this parameter drives the cost function generation throughout the planning pipeline.

## QP Problem Formulation

The planner constructs a standard quadratic programming problem of the form:

\[
\min \frac{1}{2} x^\top Q x \quad \text{s.t.} \quad l \leq A x \leq u
\]

This formulation balances trajectory smoothness against hard constraints on waypoints and dynamics.

### Cost Matrix Generation

The **cost matrix `Q`** is assembled via `generate_q` in [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs) (lines 2030-2031). This function computes the snap cost for each polynomial segment using basis functions up to the fourth derivative, accumulating results into a block-diagonal matrix structure with one block per dimension.

### Equality Constraints

**Equality constraints** enforce waypoint positions and continuity of derivatives up to the specified order. The matrix `A_eq` and vector `b_eq` are constructed in `generate_equality_constraints` (lines 6969-6972), which implements a 5-row-per-segment pattern for snap continuity. The number of constraints per dimension derives directly from `smooth_upto`, ensuring that position, velocity, acceleration, jerk, and snap remain continuous across segment boundaries.

### Inequality Constraints

**Inequality constraints** handle optional velocity and acceleration limits. When `max_velocity` and `max_acceleration` are non-zero, `generate_inequality_constraints` (lines 5252-5255) creates the corresponding bound matrices. If limits are not specified, the planner uses empty matrices to reduce computational overhead.

All constraint matrices are stacked into a single `A` matrix with corresponding lower and upper bound vectors `l` and `u` (lines 7778-7795), preparing the problem for the solver.

## Sparse Matrix Conversion for OSQP

OSQP requires matrices in **Compressed Sparse Column (CSC)** format, while Peng internally uses dense `DMatrix` structures from the nalgebra crate. The `convert_dense_to_sparse` method handles this transformation:

```rust
fn convert_dense_to_sparse(&self, a: &DMatrix<f64>) -> CscMatrix<'_> {
    let (rows, cols) = a.shape();
    let column_major_iter: Vec<f64> = a
        .column_iter()
        .flat_map(|col| col.iter().copied().collect::<Vec<f64>>())
        .collect();
    CscMatrix::from_column_iter(rows, cols, column_major_iter)
}

```

This conversion is applied to both the cost matrix `Q` and the constraint matrix `A` immediately before the OSQP call (lines 1997-2000), ensuring efficient sparse computation.

## Solving with OSQP

With CSC matrices prepared, Peng initializes the OSQP problem with default settings configured for silent operation:

```rust
let mut settings = Settings::default();
settings.verbose = false;
let mut problem = Problem::new(
    q_csc.as_slice(),
    p,
    a_csc.as_slice(),
    l.as_slice(),
    u.as_slice(),
    &settings,
);
problem.solve();
let solution = problem.x(); // optimal coefficient vector

```

The actual solver invocation occurs in [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs) at line 1874. After solving, the optimal coefficient vector is reshaped into per-dimension polynomial coefficients, enabling trajectory evaluation at arbitrary timestamps through the planner's sampling methods.

## Practical Implementation Example

The following complete example demonstrates creating and solving a minimum snap plan:

```rust
use peng::QPpolyTrajPlanner;

// Waypoints (x, y, z, yaw) and timing
let waypoints = vec![
    vec![0.0, 0.0, 0.0, 0.0],
    vec![1.0, 2.0, 1.5, 0.2],
    vec![2.5, 3.0, 0.0, 0.0],
];
let segment_times = vec![2.0, 3.0]; // seconds per segment
let polyorder = 6;                  // 6-coefficient polynomial per segment
let min_deriv = 3;                  // minimize snap
let smooth_upto = 3;                // enforce continuity up to snap
let max_velocity = 2.0;
let max_acceleration = 1.0;
let start_time = 0.0;
let dt = 0.02;                      // sample period

let mut planner = QPpolyTrajPlanner::new(
    waypoints,
    segment_times,
    polyorder,
    min_deriv,
    smooth_upto,
    max_velocity,
    max_acceleration,
    start_time,
    dt,
).unwrap();

planner.solve().expect("OSQP failed");

// Sample the resulting trajectory
let t = 1.5;
let pose = planner.sample(t);
println!("Pose at t={:.2}s → {:?}", t, pose);

```

For debugging the QP structure, you can inspect the generated matrices before solving:

```rust
let (q_csc, _, a_csc, l, u) = planner.prepare_qp_problem();
println!("Q (CSC) NNZ = {}", q_csc.nnz());
println!("A (CSC) rows = {}, cols = {}", a_csc.rows(), a_csc.cols());
println!("Bounds: l[0..5] = {:?}, u[0..5] = {:?}", &l[0..5], &u[0..5]);

```

## Summary

- **Minimum snap** minimizes the fourth derivative of position (`min_deriv = 3`) to generate smooth quadrotor trajectories.
- The `QPpolyTrajPlanner` in [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs) formulates planning as a QP problem with block-diagonal cost matrices and stacked equality/inequality constraints.
- **OSQP** solves the problem in CSC sparse format after Peng converts dense matrices via `convert_dense_to_sparse`.
- The solver respects waypoint positions, derivative continuity up to snap, and optional velocity/acceleration limits.
- Resulting coefficients enable real-time trajectory sampling for autonomous flight.

## Frequently Asked Questions

### What is minimum snap trajectory planning?

Minimum snap trajectory planning minimizes the fourth derivative of position (snap) over the entire trajectory. This produces paths that are smooth up to the snap derivative, ensuring that the required angular velocities and motor commands for quadrotors change gradually rather than abruptly. According to the Peng source code, this is implemented by setting `min_deriv = 3` (zero-indexed) in the `QPpolyTrajPlanner` constructor.

### Why does Peng use OSQP instead of other QP solvers?

Peng uses OSQP because it is an open-source, sparse QP solver based on the alternating direction method of multipliers (ADMM). As implemented in [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs) at line 1874, OSQP efficiently handles the sparse structure of the minimum snap problem, where the cost matrix `Q` is block-diagonal and constraint matrices contain mostly zeros. The solver's CSC format support and warm-starting capabilities make it suitable for real-time robotics applications.

### How do I choose the polynomial order for minimum snap?

The polynomial order must be at least \(2 \times \text{min\_deriv} + 1\) to ensure sufficient degrees of freedom for the optimization. For minimum snap (fourth derivative), Peng typically uses `polyorder = 6` or higher. Higher orders increase trajectory flexibility but also increase computational cost and may lead to numerical instability with very high derivatives. The example in [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs) uses 6th-order polynomials for 4D waypoints (x, y, z, yaw).

### Can I enforce continuity on derivatives lower than snap?

Yes. The `smooth_upto` parameter controls the highest derivative that must be continuous across segment boundaries, independent of the `min_deriv` optimization target. Setting `smooth_upto = 2` enforces continuity through acceleration while still minimizing snap, whereas `smooth_upto = 3` matches the optimization target. This flexibility allows you to optimize for snap while only requiring continuity through jerk or acceleration if your platform dynamics permit discontinuities in higher derivatives.