Minimum Snap Waypoint Planning Using OSQP in Peng: Implementation Guide

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.

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 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:

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 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 (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:

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:

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 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:

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:

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 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 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 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.

{
  "mcpServers": {
    "instagit": {
      "command": "npx",
      "args": ["-y", "instagit@latest"]
    }
  }
}