COPP Documentation
Core Problem
COPP focuses on generating a time-parameterized trajectory from a given geometric path while satisfying user-specified constraints and optimizing a specified objective. In particular, trajectories planned by COPP are generally second-order smooth (bounded acceleration) or third-order smooth (bounded jerk).
Path Parameterization
Given an \(n\)-dimensional robotic system and a sufficiently smooth geometric path:
where \(s\) is a generic path parameter and \(s_\text{f}\) is known. For an order-\(m\) problem, \(m\in\{2,3\}\), the path \(\boldsymbol{q}=\boldsymbol{q}(s)\) is required to be \(\mathcal{C}^m\) continuous in the \(s\) domain. The goal of \(m\)-th order path parameterization is to construct a strictly increasing time parameterization
such that \(\frac{\mathrm{d}^ms}{\mathrm{d}t^m}(t)\) exists and is bounded except at finitely many time instants, where the terminal time \(t_\text{f}\) is unknown. This yields a robot trajectory that strictly follows the prescribed geometric path:
After interpolation, the trajectory can be sent to a low-level servo system as a reference trajectory. If the servo period is \(T_\text{s}\) (for example, \(T_\text{s}=\text{1ms}\)), the data sent to the low-level controller typically includes the interpolated trajectory
and the corresponding feedforward quantities, such as the reference velocity terms \(\{\dot{\boldsymbol{q}}(s(iT_\text{s}))\}_{i\in\mathbb{N}}\) required by PID control, as well as reference acceleration terms \(\{\ddot{\boldsymbol{q}}(s(iT_\text{s}))\}_{i\in\mathbb{N}}\) and torque terms \(\{\boldsymbol{\tau}(s(iT_\text{s}))\}_{i\in\mathbb{N}}\) when needed.
Let \(\dot{\bullet}\) denote differentiation with respect to time \(t\), and let \(\bullet'\) denote differentiation with respect to the path parameter \(s\). Define
In second-order problems, the state is \(a(s)\) and the control is \(b(s)\). In third-order problems, the state is \((a(s),b(s))\) and the control is \(c(s)\).
Optimal Path Parameterization
Compared with standard path parameterization, optimal path parameterization additionally introduces constraints and an optimization objective.
Constraints are used to improve trajectory smoothness and keep the reference trajectory within actuator performance limits, which in practice helps reduce vibration and improve tracking accuracy. First-order constraints can represent composite velocity limits \(\|\boldsymbol{J(s)}\dot{\boldsymbol{q}}(s)\|\leq V(s)\), axis-wise velocity limits \(\dot{\boldsymbol{q}}_\text{min}(s)\leq\dot{\boldsymbol{q}}(s)\leq\dot{\boldsymbol{q}}_\text{max}(s)\), and related bounds. The general form is
Second-order constraints can represent axis-wise acceleration limits \(\ddot{\boldsymbol{q}}_\text{min}(s)\leq\ddot{\boldsymbol{q}}(s)\leq\ddot{\boldsymbol{q}}_\text{max}(s)\), axis-wise torque limits \(\boldsymbol{\tau}_\text{min}(s)\leq\boldsymbol{\tau}(s)\leq\boldsymbol{\tau}_\text{max}(s)\), and related bounds. The general form is
Third-order constraints can represent axis-wise jerk limits \(\dddot{\boldsymbol{q}}_\text{min}(s)\leq\dddot{\boldsymbol{q}}(s)\leq\dddot{\boldsymbol{q}}_\text{max}(s)\), axis-wise torque-derivative limits \(\dot{\boldsymbol{\tau}}_\text{min}(s)\leq\dot{\boldsymbol{\tau}}(s)\leq\dot{\boldsymbol{\tau}}_\text{max}(s)\), and related bounds. The general form is
In these formulations, \(a(s)\), \(b(s)\), and \(c(s)\) are the decision variables; all other quantities are known from the path, model, and physical constraints. The constraints also support piecewise bounds, station-dependent bounds, and asymmetric limits.
Common objectives include the terminal time
thermal energy dissipation
torque total variation
and linear objectives
The most common objective is the terminal time \(t_\text{f}\), which gives Time-Optimal Path Parameterization (TOPP). More generally, one can optimize a linear combination of the objectives above, or other user-defined convex objectives, yielding Convex-Objective Path Parameterization (COPP). Empirical results show that, compared with pure TOPP, the terminal-time-plus-thermal-energy objective \(J=t_\text{f}+\lambda J_\text{th}\) can significantly improve trajectory smoothness with only a minimal increase in traversal time.
In summary, the problem classes supported by copp are:
| Problem class | Second-order constraints (velocity, acceleration, torque) | Third-order constraints (additional jerk, torque-derivative bounds, etc.) |
|---|---|---|
| Time objective | TOPP2 | TOPP3 |
| General convex objective | COPP2 | COPP3 |
Installation
The copp crate is published on crates.io. Add the following dependency to the root Cargo.toml:
copp v0.2.2 requires Rust 1.88 or newer. We strongly recommend building in Release mode for substantially better computational performance.
The Python package is published on PyPI: copp-py. Install it with:
The import package name is copp_py, typically used as:
To build from local source, you need Python 3.9 or newer, Rust/Cargo, maturin, and the platform C/C++ compiler toolchain: Visual Studio Build Tools with the C++ workload on Windows, GCC/Clang on Linux, or Xcode Command Line Tools on macOS. Start with the repository:
git clone https://github.com/TOPP-THU/copp.git
cd copp
python -m pip install -U pip
python -m pip install -U maturin numpy jax
maturin develop --release --features python
maturin develop installs the Rust extension module directly into the current Python environment. Verify the build with:
Prebuilt MATLAB toolboxes are published with every release on the COPP GitHub Releases page. Each release ships one .mltbx per platform, named copp-matlab-<version>-<platform>.mltbx, where <platform> is one of windows-x86_64, linux-x86_64, macos-aarch64, or macos-x86_64. Each .mltbx is accompanied by a .sha256 checksum file. MATLAB R2024b or newer is required.
Download the asset matching your platform. If you are unsure which one that is, run computer("arch") in MATLAB and use the table below:
computer("arch") |
Release asset platform |
|---|---|
win64 |
windows-x86_64 |
glnxa64 |
linux-x86_64 |
maca64 |
macos-aarch64 |
maci64 |
macos-x86_64 |
Pick the macOS asset carefully. MATLAB toolboxes only distinguish Windows, macOS, and Linux, so both macOS packages declare plain macOS support and MATLAB will not stop you from installing the Apple-silicon package on an Intel Mac; the mismatch would only surface later, when the MEX gateway fails to load.
Install the downloaded toolbox by double-clicking it in MATLAB, or from the Command Window:
copp.version() reads the version out of the linked native library through the MEX gateway, so a successful call also confirms that the toolbox loaded correctly. Do not unpack .mltbx files manually: they are MATLAB toolbox installers, and installing them manages the MATLAB path for you.
If you have the repository checked out, bindings/matlab/install_copp.m automates the same steps. It detects the platform, downloads the matching release asset, and installs it:
cd bindings/matlab
install_copp() % latest release
install_copp(Version="v0.2.2") % a specific release
To build from source instead, you additionally need Rust/Cargo and a MATLAB-supported C++ compiler. Start with the repository:
git clone https://github.com/TOPP-THU/copp.git
cd copp
cargo build --release --lib --features matlab
The Cargo matlab feature reuses the C ABI internally, so it does not enable MATLAB code in default pure-Rust builds. Then, in the MATLAB Command Window:
cd bindings/matlab
mex -setup C++ % only needed the first time, or when changing compiler
build()
addpath(pwd)
rehash
copp.version()
build() links statically by default, which is the recommended build for ordinary use. build(Linkage="dynamic") produces a smaller MEX file but requires the native library to be discoverable at runtime.
Two path constructors need optional dependencies: copp.Path.from_symbolic requires the Symbolic Math Toolbox, and copp.Path.from_casadi requires CasADi. Everything else works with base MATLAB.
The C++ SDK suits projects that prefer RAII, namespace copp, std::vector / copp::Matrix, and an optional Eigen adapter. Prebuilt C++ SDKs for released versions are available from GitHub Releases. C++ currently has no single de-facto official distribution platform comparable to crates.io for Rust or PyPI for Python; the common choices are GitHub Releases plus a CMake package, or a vcpkg / Conan recipe provided later.
To build from source, start with the repository:
You need Cargo, CMake, and a C++17-capable compiler toolchain: Visual Studio Build Tools with the C++ workload on Windows, GCC/Clang on Linux, or Xcode Command Line Tools on macOS. If you use copp/eigen.hpp, Eigen is required as well; if you only use the core C++ API, the Eigen adapter can be turned off in CMake.
The C++ facade is gated behind the Cargo cpp feature. First build the native library from the repository root:
Then configure and build the CMake project:
To install it as a package discoverable by downstream CMake projects:
Replace <install-prefix> with your own installation path, for example C:/copp-install or $PWD/copp-install.
Use it from a downstream project as follows:
find_package(copp CONFIG REQUIRED)
add_executable(app main.cpp)
target_link_libraries(app PRIVATE copp::copp)
Pass the installation prefix to CMake when configuring the downstream project:
cmake -S app -B app/build -DCMAKE_PREFIX_PATH=<install-prefix>
cmake --build app/build --config Release
If static linking is desired, add -DCOPP_LINK_STATIC=ON to the corresponding COPP CMake configure command above. If you prefer a shared library, add -DCOPP_LINK_STATIC=OFF instead and make sure copp.dll, libcopp.so, or libcopp.dylib is discoverable at runtime.
The C ABI is suitable for C/C++ projects, downstream language bindings, and existing robotics software stacks. Prebuilt C ABI SDKs for released versions are available from GitHub Releases. To build from source, start with the repository:
You need Cargo, CMake, and a C compiler toolchain: Visual Studio Build Tools with the C++ workload on Windows, GCC/Clang on Linux, or Xcode Command Line Tools on macOS. The C ABI is gated behind the Cargo c feature. First build the native library from the repository root:
Then configure and build the CMake project:
To install it as a package discoverable by downstream CMake projects:
Replace <install-prefix> with your own installation path, for example C:/copp-install or $PWD/copp-install.
Use it from a downstream project as follows:
find_package(copp CONFIG REQUIRED)
add_executable(app main.c)
target_link_libraries(app PRIVATE copp::copp)
Pass the installation prefix to CMake when configuring the downstream project:
cmake -S app -B app/build -DCMAKE_PREFIX_PATH=<install-prefix>
cmake --build app/build --config Release
If static linking is desired, add -DCOPP_LINK_STATIC=ON to the corresponding COPP CMake configure command above.
To regenerate the C headers, install cbindgen and run the PowerShell header-generation script. Most users can directly use the generated headers under bindings/c/include/copp/.
Algorithm Selection
Algorithm selection in copp mainly depends on three factors: whether the problem is second-order or third-order, whether the objective is time-optimal or a general convex objective, and whether the higher-performance PRO algorithms are needed.
| Problem class | Algorithm | Availability | Notes |
|---|---|---|---|
| TOPP2 | TOPP2-RA | Open-source | Ultra-fast reachability-analysis-based method. In common benchmarks, its relative error versus global optimization baselines is typically below \(10^{-4}\), making it a good default entry point for second-order time-optimal problems. |
| COPP2 | COPP2-SOCP | Open-source | Formulated as an SOCP and solved with clarabel. It is globally optimal under the convex formulation, but usually costs substantially more runtime than RA-style methods. |
| COPP2 | COPP2-RDDP | PRO | Fast original method that preserves global-optimal solution quality while running substantially faster than COPP2-SOCP. |
| TOPP3 | TOPP3-SOCP | Open-source | clarabel-based conic formulation that usually returns high-quality KKT solutions; computational cost may be high on some datasets. |
| TOPP3 | TOPP3-LP | Open-source | Linear-objective approximation of TOPP3-SOCP. It is usually faster, but can become sub-optimal under tight jerk constraints; it is mainly recommended when jerk bounds are loose. |
| TOPP3 | TOPP3-RA | PRO | Ultra-fast reachability-analysis-based third-order method. It can be sub-optimal under tight jerk constraints and is mainly recommended when jerk bounds are loose. |
| COPP3 | COPP3-SOCP | Open-source | clarabel-based conic formulation with strong practical optimality, at relatively high computational cost. |
| COPP3 | COPP3-RDDP | PRO | Fast original method that returns KKT-quality solutions comparable to TOPP3-SOCP while running substantially faster than TOPP3-SOCP, TOPP3-LP, and COPP3-SOCP. COPP3-RDDP can also be used as a high-quality TOPP3 solver. On long-path problems, it may provide better practical optimality and numerical stability than COPP3-SOCP. |
More specifically, choose algorithms as follows:
| Scenario / primary priority | Recommended algorithm | Availability | Why this is recommended | Typical caveat | Alternative |
|---|---|---|---|---|---|
| Second-order, time-optimal planning with very low runtime | TOPP2-RA | Open-source | Excellent speed-performance trade-off; near-global-optimal in typical benchmarks. | Objective is fixed to minimum time. | |
| Second-order, convex objective with strong global solution quality | COPP2-SOCP | Open-source | Convex conic formulation with global optimality under model assumptions. | Higher runtime than RA/RDDP methods. | COPP2-RDDP for major speedup. |
| Second-order, convex objective with maximum computational efficiency | COPP2-RDDP | PRO | Preserves global-optimal solution quality while substantially improving runtime. | PRO license required. | COPP2-SOCP. |
| Third-order problems requiring the strongest open-source optimality quality | TOPP3-SOCP / COPP3-SOCP | Open-source | Strong KKT-quality solutions and broad applicability. | On specific datasets, computational cost may be high. | COPP3-RDDP for major speedup. |
| Third-order, time-optimal planning with a faster open-source approximation | TOPP3-LP | Open-source | Use it when your own path dataset shows better runtime/performance. | May become sub-optimal under tight jerk bounds. | TOPP3-SOCP or COPP3-RDDP. |
| Third-order, time-optimal planning with loose jerk bounds and very low runtime | TOPP3-RA | PRO | Very low computational cost. | May become sub-optimal under tight jerk bounds. | COPP3-RDDP or TOPP3-SOCP. |
| Third-order, high-quality and high-stability planning for difficult long paths | COPP3-RDDP | PRO | Strong practical optimality, high speed, and often better robustness on long horizons. | PRO license required. | COPP3-SOCP. |
Step-by-Step Workflow
We use time-optimal parameterization of a two-dimensional path as a simple example. Complete runnable examples are available under the examples directory for each language, and advanced APIs are described in the documentation and architecture section.
#include <math.h>
#include <stddef.h>
#include <stdio.h>
#include "copp/copp.h"
enum { DIM = 2 };
static int check(enum CoppStatus status, const char *call)
{
if (status == COPP_STATUS_OK) {
return 0;
}
fprintf(stderr, "%s failed: %s\n", call, copp_status_message(status));
fprintf(stderr, "%s\n", copp_last_error_message());
return 1;
}
Step 1. Construct the Geometric Path
Strictly speaking, the geometric path \(\boldsymbol{q}=\boldsymbol{q}(s)\) is supplied by the user. The copp library provides path-related modules as convenient helpers.
Option A. Analytic Expression with Automatic Differentiation
The simplest approach is to construct the path from an analytic expression, for example:
import jax
import jax.numpy as jnp
jax.config.update("jax_enable_x64", True)
def q_fn(s):
freq = jnp.array([2.0 * jnp.pi, 2.0 * jnp.pi], dtype=jnp.float64)
phase = jnp.array([0.0, 0.5 * jnp.pi], dtype=jnp.float64)
return jnp.sin(freq * s + phase)
path = copp.Path.from_jax(q_fn, 0.0, 1.0)
Python automatic differentiation supports jax, autograd, casadi, and sympy. Choose one and install the corresponding dependency as needed.
from_parametric uses the MATLAB-side Jet3 automatic differentiation; the length of the vector returned by the formula is the path dimension dim.
static enum CoppStatus evaluate_parametric_path(
void *user_data,
size_t dim,
struct CoppJet3 s,
struct CoppJet3 *q)
{
(void)user_data;
if (dim != DIM || q == NULL) {
return COPP_STATUS_INVALID_ARGUMENT;
}
// `s` arrives seeded with ds/ds = 1, and the inline Jet3 helpers in
// `copp/path.h` propagate derivatives up to third order. Only q(s)
// has to be written; dq, ddq, and dddq come out of the helpers.
const struct CoppJet3 w = copp_mul_f64(s, 6.28318530717958647692);
q[0] = copp_sin(w);
q[1] = copp_cos(w);
return COPP_STATUS_OK;
}
struct CoppPath *path = NULL;
// On success, release `path` later with `copp_path_free(path)`.
if (check(
copp_path_from_parametric(DIM, 0.0, 1.0, evaluate_parametric_path, NULL, &path),
"copp_path_from_parametric")) {
return 1;
}
copp_path_from_parametric and CoppJet3 require v0.2.2 or newer. Besides copp_sin, copp_cos, and copp_mul_f64, the header provides copp_add, copp_sub, copp_mul, copp_div, copp_neg, copp_exp, copp_log, copp_sqrt, copp_powi, copp_constant, and their scalar variants. On earlier versions, supply the derivatives manually through copp_path_from_evaluator_3rd, as shown in Option C below.
Option B. Build a Spline from Waypoints
If waypoints are available, a spline path can be constructed as follows:
enum { NUM_WAYPOINTS = 5 };
// Column-major matrix with shape DIM x NUM_WAYPOINTS.
// Each column is one waypoint.
double waypoints[DIM * NUM_WAYPOINTS] = {
0.0, 0.0,
0.25, 0.1,
0.5, -0.1,
0.75, 0.2,
1.0, 0.0,
};
struct CoppPathOptions path_options;
struct CoppPath *path = NULL;
// On success, release `path` later with `copp_path_free(path)`.
if (check(copp_path_default_options(0.0, 1.0, &path_options), "copp_path_default_options")) {
return 1;
}
if (check(
copp_path_from_waypoints(
COPP_MATRIX_VIEW_F64_COLUMN_MAJOR(waypoints, DIM, NUM_WAYPOINTS),
path_options,
&path),
"copp_path_from_waypoints")) {
return 1;
}
Option C. User-Provided Derivatives
In the most general case, users can provide derivatives manually. For TOPP2/COPP2, the evaluator should provide \(\boldsymbol{q}(s)\), \(\boldsymbol{q}'(s)\), and \(\boldsymbol{q}''(s)\) at the requested \(s\) values:
use copp::diag::PathError;
use copp::path::{Path, PathEvaluator2nd, PathEvaluator3rd};
struct NormalizedEvaluator;
impl PathEvaluator2nd for NormalizedEvaluator {
fn dim(&self) -> usize {
2
}
fn evaluate_up_to_2nd(
&self,
s: &[f64],
q: &mut [f64],
dq: &mut [f64],
ddq: &mut [f64],
) -> Result<(), PathError> {
const PI2 : f64 = 2.0 * PI;
for (col, &sj) in s.iter().enumerate() {
let row0 = 2 * col; // dim = 2
let sin = (PI2 * sj).sin();
let cos = (PI2 * sj).cos();
q[row0] = sin;
q[row0 + 1] = cos;
dq[row0] = PI2 * cos;
dq[row0 + 1] = -PI2 * sin;
ddq[row0] = -PI2 * PI2 * sin;
ddq[row0 + 1] = -PI2 * PI2 * cos;
}
Ok(())
}
}
// If only TOPP2/COPP2 is required and TOPP3/COPP3 is not called, then `PathEvaluator3rd` can be removed.
impl PathEvaluator3rd for NormalizedEvaluator {
fn evaluate_up_to_3rd(
&self,
s: &[f64],
q: &mut [f64],
dq: &mut [f64],
ddq: &mut [f64],
dddq: &mut [f64],
) -> Result<(), PathError> {
const PI2 : f64 = 2.0 * PI;
for (col, &sj) in s.iter().enumerate() {
let row0 = 2 * col; // dim = 2
let sin = (PI2 * sj).sin();
let cos = (PI2 * sj).cos();
q[row0] = sin;
q[row0 + 1] = cos;
dq[row0] = PI2 * cos;
dq[row0 + 1] = -PI2 * sin;
ddq[row0] = -PI2 * PI2 * sin;
ddq[row0 + 1] = -PI2 * PI2 * cos;
dddq[row0] = -PI2 * PI2 * PI2 * cos;
dddq[row0 + 1] = PI2 * PI2 * PI2 * sin;
}
Ok(())
}
}
// If only TOPP2/COPP2 is required and TOPP3/COPP3 is not called, then the path can be constructed by `from_evaluator_2nd` without dependence on `PathEvaluator3rd`.
let path = Path::from_evaluator_3rd(NormalizedEvaluator, 0.0, 1.0)?;
class Evaluator:
dim = 2
def evaluate_q(self, s):
q = np.empty((s.size, self.dim), dtype=np.float64)
pi2 = 2.0 * np.pi
q[:, 0] = np.sin(pi2 * s)
q[:, 1] = np.cos(pi2 * s)
return q
def evaluate_up_to_2nd(self, s):
q = self.evaluate_q(s)
pi2 = 2.0 * np.pi
sin = np.sin(pi2 * s)
cos = np.cos(pi2 * s)
dq = np.empty_like(q)
dq[:, 0] = pi2 * cos
dq[:, 1] = -pi2 * sin
ddq = np.empty_like(q)
ddq[:, 0] = -(pi2**2) * sin
ddq[:, 1] = -(pi2**2) * cos
return q, dq, ddq
# If only TOPP2/COPP2 is required and TOPP3/COPP3 is not called, then `evaluate_up_to_3rd` can be removed.
def evaluate_up_to_3rd(self, s):
q, dq, ddq = self.evaluate_up_to_2nd(s)
pi2 = 2.0 * np.pi
sin = np.sin(pi2 * s)
cos = np.cos(pi2 * s)
dddq = np.empty_like(q)
dddq[:, 0] = -(pi2**3) * cos
dddq[:, 1] = (pi2**3) * sin
return q, dq, ddq, dddq
# If only TOPP2/COPP2 is required and TOPP3/COPP3 is not called, then the path can be constructed by `from_evaluator_2nd` without dependence on `evaluate_up_to_3rd`.
path = copp.Path.from_evaluator_3rd(Evaluator(), 0.0, 1.0)
eval3 = @(s) deal( ...
[sin(2*pi*s); cos(2*pi*s)], ...
[2*pi*cos(2*pi*s); -2*pi*sin(2*pi*s)], ...
[-(2*pi)^2*sin(2*pi*s); -(2*pi)^2*cos(2*pi*s)], ...
[-(2*pi)^3*cos(2*pi*s); (2*pi)^3*sin(2*pi*s)]);
% If only TOPP2/COPP2 is required and TOPP3/COPP3 is not called, then
% the path can be constructed by Path.from_evaluator_2nd without dddq.
path = copp.Path.from_evaluator_3rd( ...
eval3, dim=DIM, s_range=[0, 1]);
The MATLAB evaluator is a batch callback: the input s is always 1 x N, and the outputs q,dq,ddq,dddq are always dim x N.
auto path = copp::Path::from_evaluator_3rd(
DIM,
0.0,
1.0,
[](copp::Span<const double> s,
copp::MatrixRef q,
copp::MatrixRef dq,
copp::MatrixRef ddq,
copp::MatrixRef dddq) {
const double pi2 = 2.0 * 3.14159265358979323846;
const double pi2_sq = pi2 * pi2;
const double pi2_cu = pi2_sq * pi2;
for (std::size_t col = 0; col < s.size(); ++col) {
const double sj = s[col];
const double sin_v = std::sin(pi2 * sj);
const double cos_v = std::cos(pi2 * sj);
q(0, col) = sin_v;
q(1, col) = cos_v;
dq(0, col) = pi2 * cos_v;
dq(1, col) = -pi2 * sin_v;
ddq(0, col) = -pi2_sq * sin_v;
ddq(1, col) = -pi2_sq * cos_v;
dddq(0, col) = -pi2_cu * cos_v;
dddq(1, col) = pi2_cu * sin_v;
}
});
// If only TOPP2/COPP2 is required, use `Path::from_evaluator_2nd` instead
// and omit the `dddq` output.
static enum CoppStatus evaluate_path_2nd(
void *user_data,
size_t dim,
size_t n,
const double *s,
double *q,
double *dq,
double *ddq)
{
(void)user_data;
if (dim != DIM) {
return COPP_STATUS_INVALID_ARGUMENT;
}
if (n > 0 && (s == NULL || q == NULL || dq == NULL || ddq == NULL)) {
return COPP_STATUS_NULL_POINTER;
}
const double pi2 = 6.28318530717958647692;
for (size_t col = 0; col < n; ++col) {
const double sin_v = sin(pi2 * s[col]);
const double cos_v = cos(pi2 * s[col]);
const size_t row0 = col * dim;
q[row0] = sin_v;
q[row0 + 1] = cos_v;
dq[row0] = pi2 * cos_v;
dq[row0 + 1] = -pi2 * sin_v;
ddq[row0] = -(pi2 * pi2) * sin_v;
ddq[row0 + 1] = -(pi2 * pi2) * cos_v;
}
return COPP_STATUS_OK;
}
static enum CoppStatus evaluate_path_3rd(
void *user_data,
size_t dim,
size_t n,
const double *s,
double *q,
double *dq,
double *ddq,
double *dddq)
{
(void)user_data;
if (dim != DIM) {
return COPP_STATUS_INVALID_ARGUMENT;
}
if (n > 0 && (s == NULL || q == NULL || dq == NULL || ddq == NULL || dddq == NULL)) {
return COPP_STATUS_NULL_POINTER;
}
const double pi2 = 6.28318530717958647692;
const double pi2_sq = pi2 * pi2;
const double pi2_cu = pi2_sq * pi2;
for (size_t col = 0; col < n; ++col) {
const double sin_v = sin(pi2 * s[col]);
const double cos_v = cos(pi2 * s[col]);
const size_t row0 = col * dim;
q[row0] = sin_v;
q[row0 + 1] = cos_v;
dq[row0] = pi2 * cos_v;
dq[row0 + 1] = -pi2 * sin_v;
ddq[row0] = -pi2_sq * sin_v;
ddq[row0 + 1] = -pi2_sq * cos_v;
dddq[row0] = -pi2_cu * cos_v;
dddq[row0 + 1] = pi2_cu * sin_v;
}
return COPP_STATUS_OK;
}
// If only TOPP2/COPP2 is required and TOPP3/COPP3 is not called, then
// `copp_path_from_evaluator_2nd` can be used without `evaluate_path_3rd`.
struct CoppPath *path = NULL;
// On success, release `path` later with `copp_path_free(path)`.
if (check(
copp_path_from_evaluator_3rd(
DIM,
0.0,
1.0,
evaluate_path_2nd,
evaluate_path_3rd,
NULL,
&path),
"copp_path_from_evaluator_3rd")) {
return 1;
}
Step 2. Discretize Path Data
Path-parameterization problems are solved on a discrete \(s\) grid, for example:
Create the robot model:
Provide the \(s\) grid and path data:
If only TOPP2/COPP2 is solved, set_q_from_path_2nd can be used instead.
// If only TOPP2/COPP2 is required and TOPP3/COPP3 is not called, then
// `copp_robot_sample_path_3rd` should be replaced by `copp_robot_sample_path_2nd`.
if (check(copp_robot_append_s(robot, (struct CoppSliceF64){s, n}), "copp_robot_append_s")) {
return 1;
}
if (check(copp_robot_sample_path_3rd(robot, path, 0, n), "copp_robot_sample_path_3rd")) {
return 1;
}
For more flexible use cases, paths can be added or removed online, and the robot model can include inverse-kinematics information. These advanced APIs are described in the documentation and architecture section.
Step 3. Construct Constraints
In most cases, we recommend using high-level APIs with clear physical meaning, for example:
// The axial velocity is -1 <= vel <= 1 for each axis in this example
let vel_max = vec![1.0; DIM];
let vel_min = vec![-1.0; DIM];
// The axial acceleration is -1 <= acc <= 1 for each axis in this example.
let acc_max = vec![1.0; DIM];
let acc_min = vec![-1.0; DIM];
robot
.with_axial_velocity((vel_max.as_slice(), n), (vel_min.as_slice(), n), 0)?
.with_axial_acceleration((acc_max.as_slice(), n), (acc_min.as_slice(), n), 0)?;
// The axial velocity is -1 <= vel <= 1 for each axis.
std::vector<double> vel_max(DIM, 1.0);
std::vector<double> vel_min(DIM, -1.0);
// The axial acceleration is -1 <= acc <= 1 for each axis.
std::vector<double> acc_max(DIM, 1.0);
std::vector<double> acc_min(DIM, -1.0);
robot.add_velocity_limits(vel_max, vel_min, 0, n)
.add_acceleration_limits(acc_max, acc_min, 0, n);
// The axial velocity/acceleration is -1 <= value <= 1 for each axis.
double upper[DIM] = {1.0, 1.0};
double lower[DIM] = {-1.0, -1.0};
if (check(
copp_add_axial_velocity_limits(
robot,
0,
n,
(struct CoppSliceF64){upper, DIM},
(struct CoppSliceF64){lower, DIM}),
"copp_add_axial_velocity_limits")) {
return 1;
}
if (check(
copp_add_axial_acceleration_limits(
robot,
0,
n,
(struct CoppSliceF64){upper, DIM},
(struct CoppSliceF64){lower, DIM}),
"copp_add_axial_acceleration_limits")) {
return 1;
}
For third-order trajectories, additional third-order constraints are needed, for example:
Lower-level and more flexible constraint-construction APIs are described in the documentation and architecture section.
Step 4. Call the Solver
Here we solve a TOPP2 problem with topp2_ra. First define the problem:
Then build solver options and call the solver:
This gives the profile \(a=a(s)\).
Step 5. Post-Process the Second-Order Trajectory (Second-Order Only)
Next we convert the result into the actual trajectory \(\boldsymbol{q}=\boldsymbol{q}(t)\), in particular an interpolated trajectory suitable for tracking by a low-level servo controller. First compute \(t=t(s)\):
Then invert the timing relation to obtain \(s=s(t)\) and interpolate:
// s_t is a uniform time grid of s(t) with dt = 1e-3s. This is useful for plotting and downstream control.
const double dt = 1e-3;
struct CoppVecF64 s_t = {0};
// On success, release `s_t` later with `copp_vec_f64_free(s_t)`.
if (check(
copp_t_to_s_uniform_2nd(
(struct CoppSliceF64){s, n},
(struct CoppSliceF64){a_ra.data, a_ra.len},
(struct CoppSliceF64){t_s.data, t_s.len},
0.0,
dt,
true,
&s_t),
"copp_t_to_s_uniform_2nd")) {
return 1;
}
The interpolation routines also support non-uniform time samples; see the documentation and architecture section for details. Finally, evaluate the interpolated trajectory \(\boldsymbol{q}=\boldsymbol{q}(t)\). A simple approach is:
struct CoppMatrixF64 q_t = {0};
struct CoppMatrixF64 dq_t = {0};
struct CoppMatrixF64 ddq_t = {0};
// On success, release returned matrices with `copp_matrix_f64_free`.
if (check(
copp_path_evaluate_up_to_2nd(
path,
(struct CoppSliceF64){s_t.data, s_t.len},
&q_t,
&dq_t,
&ddq_t),
"copp_path_evaluate_up_to_2nd")) {
return 1;
}
// q_t is a column-major DIM x s_t.len matrix: q_t.data[row + col * q_t.rows].
copp_matrix_f64_free(ddq_t);
copp_matrix_f64_free(dq_t);
copp_matrix_f64_free(q_t);
This completes the full second-order trajectory pipeline. For a third-order trajectory, skip the second-order post-processing step and continue with the following workflow.
Step 6. Construct and Solve the Third-Order Problem (Third-Order Only)
Construct the third-order problem as follows. The non-convex third-order constraints are linearized using the previously computed second-order profile a_ra:
use copp::solver::topp3_socp::Topp3ProblemBuilder;
// Note that in TOPP3Problem, the non-convex jerk constraints should be linearized into a convex one.
// More details can be found in the documentation of `Topp3ProblemBuilder::build_with_linearization`.
let topp3_problem =
Topp3ProblemBuilder::new(&mut robot, idx_s_interval.0, &a_ra, (0.0, 0.0), (0.0, 0.0))
.build_with_linearization()?;
problem = copp.solver.topp3_socp.Problem( ...
robot, ...
a_ra, ...
idx_s_start=1, ...
a_boundary=[0, 0], ...
b_boundary=[0, 0]);
The third-order MATLAB Problem lazily linearizes around a_ra when solve is called.
Using topp3_socp as an example, call the solver as follows:
struct CoppClarabelOptions options_socp;
struct CoppProfile3rd profile = {0};
// On success, release `profile` later with `copp_profile_3rd_free(profile)`.
if (check(copp_clarabel_default_options(&options_socp), "copp_clarabel_default_options")) {
return 1;
}
options_socp.allow_almost_solved = true;
if (check(topp3_socp(topp3_problem, options_socp, &profile), "topp3_socp")) {
return 1;
}
This already produces a feasible, near-optimal third-order trajectory \(a_1(s),b_1(s)\) and can be used directly in the next step. If more computational budget is available, use \(a_1(s)\) as the next linearization point and solve a new linearized third-order problem:
struct Topp3Problem topp3_problem_next = {
robot,
0,
(struct CoppSliceF64){profile.a.data, profile.a.len},
0.0,
0.0,
0.0,
0.0,
1,
1,
1e-10,
};
struct CoppProfile3rd profile_next = {0};
// On success, release `profile_next` with `copp_profile_3rd_free(profile_next)`,
// or move it into `profile` as below and release `profile` once at cleanup.
if (check(
topp3_socp(topp3_problem_next, options_socp, &profile_next),
"topp3_socp second iteration")) {
return 1;
}
copp_profile_3rd_free(profile);
profile = profile_next;
profile_next = (struct CoppProfile3rd){0};
When computational resources allow, the above procedure can be repeated: linearize the third-order non-convex problem at \(a_k(s)\) and solve for \(a_{k+1}(s),b_{k+1}(s)\). Under non-degenerate conditions, this process can converge to a KKT solution. For practical online use, one to two linearization iterations are usually sufficient.
Step 7. Post-Process the Third-Order Trajectory (Third-Order Only)
Next we convert the third-order profile into the actual trajectory \(\boldsymbol{q}=\boldsymbol{q}(t)\), in particular an interpolated trajectory suitable for tracking by a low-level servo controller. First compute \(t=t(s)\):
double t_final = 0.0;
struct CoppVecF64 t_s = {0};
// On success, release `t_s` later with `copp_vec_f64_free(t_s)`.
if (check(
copp_s_to_t_3rd(
(struct CoppSliceF64){s, n},
(struct CoppSliceF64){profile.a.data, profile.a.len},
(struct CoppSliceF64){profile.b.data, profile.b.len},
profile.num_stationary_start,
profile.num_stationary_end,
0.0,
&t_final,
&t_s),
"copp_s_to_t_3rd")) {
return 1;
}
Then invert the timing relation to obtain \(s=s(t)\) and interpolate:
use copp::solver::topp3_socp::t_to_s_topp3;
use copp::InterpolationMode;
// s_t is a uniform time grid of s(t) with dt = 1e-3s. This is useful for plotting and downstream control.
let dt = 1e-3;
let s_t = t_to_s_topp3(
&s,
profile.as_parts(),
&t_s,
InterpolationMode::UniformTimeGrid(0.0, dt, true),
)?;
// s_t is a uniform time grid of s(t) with dt = 1e-3s. This is useful for plotting and downstream control.
const double dt = 1e-3;
struct CoppVecF64 s_t = {0};
// On success, release `s_t` later with `copp_vec_f64_free(s_t)`.
if (check(
copp_t_to_s_uniform_3rd(
(struct CoppSliceF64){s, n},
(struct CoppSliceF64){profile.a.data, profile.a.len},
(struct CoppSliceF64){profile.b.data, profile.b.len},
profile.num_stationary_start,
profile.num_stationary_end,
(struct CoppSliceF64){t_s.data, t_s.len},
0.0,
dt,
true,
&s_t),
"copp_t_to_s_uniform_3rd")) {
return 1;
}
The interpolation routines also support non-uniform time samples; see the documentation and architecture section for details. Finally, evaluate the interpolated trajectory \(\boldsymbol{q}=\boldsymbol{q}(t)\). A simple approach is:
All output matrices have shape dim x numel(s_t).
struct CoppMatrixF64 q_t = {0};
struct CoppMatrixF64 dq_t = {0};
struct CoppMatrixF64 ddq_t = {0};
struct CoppMatrixF64 dddq_t = {0};
// On success, release returned matrices with `copp_matrix_f64_free`.
if (check(
copp_path_evaluate_up_to_3rd(
path,
(struct CoppSliceF64){s_t.data, s_t.len},
&q_t,
&dq_t,
&ddq_t,
&dddq_t),
"copp_path_evaluate_up_to_3rd")) {
return 1;
}
// q_t is a column-major DIM x s_t.len matrix: q_t.data[row + col * q_t.rows].
copp_matrix_f64_free(dddq_t);
copp_matrix_f64_free(ddq_t);
copp_matrix_f64_free(dq_t);
copp_matrix_f64_free(q_t);
This completes the full third-order trajectory pipeline.
Resource Management
The Rust API does not require manual resource release. Objects are released automatically when they leave scope.
The Python API does not require manual resource release. Objects are managed by the Python runtime.
MATLAB objects release their native handle on delete, so ordinary scripts usually do not need to free anything manually. Long scripts and tests can release explicitly, or use onCleanup:
The C++ API does not require manual resource release. Path, Robot, Constraints, Profile3rd, Matrix, and similar objects follow RAII and release both the Rust-side handle and the C++-side storage when they leave scope. Span, MatrixView, and MatrixRef are borrowed views that do not own memory and do not need to be released.
In the C ABI, CoppSliceF64 and CoppMatrixViewF64 only borrow user-owned memory and do not need to be released. All owned objects returned by COPP, including CoppVecF64, CoppMatrixF64, CoppProfile3rd, and the CoppPath and CoppRobot handles, must be released with their matching free functions. In real projects, we recommend initializing owned objects to {0} or NULL and using a centralized cleanup block so that resources already created are still released after an early error:
cleanup:
// Path evaluation outputs.
copp_matrix_f64_free(dddq_t);
copp_matrix_f64_free(ddq_t);
copp_matrix_f64_free(dq_t);
copp_matrix_f64_free(q_t);
// Interpolation outputs and solver profiles.
copp_vec_f64_free(s_t);
copp_vec_f64_free(t_s);
copp_profile_3rd_free(profile);
copp_vec_f64_free(a_ra);
// Long-lived handles should usually be released last.
copp_robot_free(robot);
copp_path_free(path);
return rc;
For second-order trajectories, objects such as profile and dddq_t are not present. If multiple third-order iteration results are retained, such as profile_qp1 and profile_qp2, each CoppProfile3rd whose ownership has not been moved must be released once with copp_profile_3rd_free. The C examples in the repository follow the same centralized cleanup style.
Step-by-Step Summary
Overall, a minimal closed loop consists of: construct the path \(\boldsymbol{q}(s)\); build a Robot and constraints on a discrete grid; select the corresponding problem builder and solver; obtain \(a(s)\) or \((a(s),b(s))\); convert back to the time domain with s_to_t_* and t_to_s_*; and finally resample the original path at \(s(t)\). The repository also provides runnable examples for TOPP2, COPP2, TOPP3, and COPP3.
Benchmark
The open-source rows are reproducible from tests/test_random_spline.rs in the repository. The PRO rows (COPP2-RDDP, TOPP3-RA, COPP3-RDDP) come from the same benchmark run on the PRO release and are therefore not part of the open-source test file. Test conditions:
release, --include-ignored- CPU: Intel(R) Core(TM) Ultra 9 285K.
- Dataset: 100 random 7-DOF spline paths, each discretized into 1000 intervals.
All metrics are reported as mean ± std. Computation times are machine- and run-dependent, so absolute values will differ from your own runs; the relative comparison between methods is the meaningful part.
Time-Optimal
| Method | Availability | Computation time (ms) | Traversal time (s) |
|---|---|---|---|
| TOPP2-RA | Open-source | 0.615425 ± 0.244409 | 40.903420 ± 1.378671 |
| COPP2-SOCP | Open-source | 149.969964 ± 9.364334 | 40.900039 ± 1.378613 |
| COPP2-RDDP | PRO | 5.436142 ± 0.465495 | 40.900135 ± 1.378613 |
| TOPP3-LP | Open-source | 327.074029 ± 28.893341 | 41.422945 ± 1.381874 |
| TOPP3-SOCP | Open-source | 289.654071 ± 12.862133 | 41.418608 ± 1.381202 |
| COPP3-SOCP | Open-source | 285.004302 ± 13.471264 | 41.418608 ± 1.381202 |
| TOPP3-RA (Iteration 1) | PRO | 10.571045 ± 0.857653 | 41.499200 ± 1.385735 |
| TOPP3-RA (Iteration 2) | PRO | 20.300932 ± 1.237908 | 41.399867 ± 1.386791 |
Convex-Objective
In this test, TOPP methods still use traversal time as the optimization objective.
| Method | Availability | Computation time (ms) | Objective value |
|---|---|---|---|
| TOPP2-RA | Open-source | 0.534700 ± 0.069296 | 217.444861 ± 12.462360 |
| COPP2-SOCP | Open-source | 270.059250 ± 52.073677 | 96.517354 ± 3.641154 |
| COPP2-RDDP | PRO | 12.667700 ± 0.429214 | 96.525785 ± 3.639733 |
| TOPP3-LP | Open-source | 348.000000 ± 9.326314 | 211.611085 ± 12.367224 |
| TOPP3-SOCP | Open-source | 301.227000 ± 12.938498 | 211.974066 ± 12.323865 |
| COPP3-SOCP | Open-source | 301.227000 ± 12.938498 | 96.634962 ± 3.613264 |
| COPP3-RDDP | PRO | 65.823050 ± 0.087893 | 98.708998 ± 3.354004 |
Documentation and Architecture
Documentation
We recommend using the latest docs.rs documentation. Local documentation is also available:
To inspect unreleased updates on the main branch, we recommend generating the documentation locally from the copp repository root:
The generated documentation includes mathematical foundations, path and constraint construction methods, logging and output conventions, error definitions, and solver interfaces.
Python documentation:
To generate the Python documentation locally, install Sphinx and build the HTML pages from the copp repository root:
python -m pip install -U sphinx
python -m sphinx -E -b html bindings/python/docs/source bindings/python/docs/build/html
The generated entry page is bindings/python/docs/build/html/index.html.
MATLAB documentation:
It can also be generated locally through publish. Run these commands in the MATLAB Command Window, not in a system shell:
build_docs adds bindings/matlab to the MATLAB path itself, so no manual addpath is needed. It does, however, execute the code in the documentation pages (EvalCode defaults to true), and those pages call the COPP API, so the MEX gateway must already be built. Check it first:
If that errors, build the MEX before generating the documentation, as described in the installation section.
To generate the documentation without leaving a system shell, use MATLAB batch mode:
The generated entry page is bindings/matlab/docs/html/index.html.
C++ documentation:
The C++ API documentation is generated with Doxygen from bindings/cpp/include/copp/*.hpp, bindings/cpp/docs/*.md, and the example code. To inspect the C++ interface on the development branch, generate it locally from the copp repository root:
The generated entry page is bindings/cpp/docs/html/index.html. Rendering include graphs and call graphs additionally requires Graphviz; if formulas do not display correctly, check the Doxygen MathJax configuration first.
C documentation:
To generate the C documentation locally, install Doxygen, Graphviz, and PowerShell 7 (pwsh, used to regenerate headers), then run the corresponding command from the copp repository root:
The generated entry page is bindings/c/docs/html/index.html.
Project Architecture
| Module | Responsibility |
|---|---|
path |
Path construction, parameter range, second/third derivatives. |
robot |
Robot dimension, inverse dynamics, and physical constraint entry points. |
constraints |
Lower-level constraint interfaces. |
solver |
Solvers for each problem class. |
diag |
Error types, logging verbosity, and diagnostic information. |
| Module | Responsibility |
|---|---|
copp_py |
Top-level package exporting common types, functions, and submodule entry points. |
copp_py.path |
Path, SplineConfig, path evaluation, waypoint splines, automatic differentiation, and user-evaluator paths. |
copp_py.robot |
Robot, path sampling, velocity/acceleration/jerk/torque constraints, advanced physical constraint entry points, and inverse-dynamics callbacks. |
copp_py.constraints |
Raw Constraints buffers, first/second/third-order low-level constraints, amax_substitute, and sliding-window operations. |
copp_py.solver |
Solver namespace containing topp2_ra, reach_set2, copp2_socp, topp3_lp, topp3_socp, and copp3_socp. |
copp_py.interpolation |
Profile3rd, second/third-order s_to_t_*, t_to_s_*, and a_to_b_topp2 trajectory post-processing. |
copp_py.objective |
COPP2/COPP3 objective descriptions, including time, linear, thermal-energy, and torque-total-variation objectives. |
copp_py.clarabel |
Clarabel SOCP options, settings, solver status, and expert diagnostic results. |
copp_py.core |
Version, error types, enums, matrix layouts, and common type conventions. |
| Module | Responsibility |
|---|---|
copp |
Top-level package providing version, Path, Robot, Profile3rd. |
copp.Path |
Waypoint, evaluator, parametric, symbolic, and CasADi paths. |
copp.Robot |
Station grid, path sampling, velocity/acceleration/jerk/torque limits. |
copp.objective |
COPP objectives: time, linear, thermal energy, torque total variation. |
copp.solver |
topp2_ra, reach_set2, copp2_socp, topp3_*, copp3_*. |
copp.interpolation |
Second/third-order s_to_t_*, t_to_s_*, and a_to_b_topp2. |
copp.clarabel |
Clarabel solver options, settings, and direct solve method. |
copp.diag |
MATLAB exception facade, verbosity, and MEX last-error snapshots. |
| Header / Namespace | Responsibility |
|---|---|
copp/copp.hpp |
Umbrella header containing the C++ public facade. |
copp/core.hpp |
Core types such as Matrix, Span, Error, and Expected. |
copp/path.hpp |
Waypoint / parametric / evaluator path construction and derivatives. |
copp/robot.hpp |
Robot, Constraints, inverse-dynamics callbacks, and physical constraints. |
copp/interpolation.hpp |
TOPP2 / TOPP3 profiles and time interpolation. |
copp/solver/*.hpp |
TOPP/COPP solver namespaces with problem/options/result types. |
copp/eigen.hpp |
Optional Eigen adapter; the core headers do not depend on Eigen. |
CMake target copp::copp |
Link entry point for downstream C++ projects; the C ABI is also available as copp::c_abi. |
| Header | Responsibility |
|---|---|
copp/copp.h |
Umbrella header containing the complete C ABI. |
copp/core.h |
Status codes, last error, matrix/vector views, and Clarabel options. |
copp/path.h |
Path handles, spline paths, callback paths, and path evaluation. |
copp/robot.h |
Robot handles, path sampling, physical constraints, and inverse-dynamics callbacks. |
copp/formulation.h |
TOPP/COPP problem descriptors, objectives, and profile types. |
copp/interpolation.h |
Second/third-order trajectory post-processing and interpolation. |
copp/topp2.h |
TOPP2-RA and ReachSet2 interfaces. |
copp/copp2.h |
COPP2-SOCP interface. |
copp/topp3.h |
TOPP3-LP and TOPP3-SOCP interfaces. |
copp/copp3.h |
COPP3-SOCP interface. |
Citing
If your work uses the open-source TOPP3/COPP3 functionality, please cite the following paper, as even the basic interval-wise profile template uses contributions from this work:
@article{wang2026online,
title={Online time-optimal trajectory planning along parametric toolpaths with strict constraint satisfaction and certifiable feasibility guarantee},
author={Wang, Yunan and Hu, Chuxiong and Li, Yuanshenglong and Yu, Jichuan and Yan, Jizhou and Liang, Yixuan and Jin, Zhao},
journal={International Journal of Machine Tools and Manufacture},
volume={215},
pages={104355},
year={2026}
}
If your work uses the TOPP3-RA, COPP2-RDDP, or COPP3-RDDP methods from the PRO release, please cite the following paper. TOPP3-RA is also improved based on this work:
@article{wang2026reachability,
title={Reachability-augmented dual dynamic programming for optimal path parameterization},
author={Yunan Wang and Jizhou Yan and Chuxiong Hu and Zeyang Li},
journal={arXiv preprint arXiv:2605.19089},
year={2026}
}
For other use cases, please cite the COPP library or the corresponding paper:
@misc{thu2026copp,
title = {COPP: Convex-Objective Path Parameterization},
author = {Wang, Yunan and He, Suqin and Lin, Shize and Hu, Chuxiong},
year = {2026},
publisher = {GitHub},
howpublished = {\url{https://github.com/TOPP-THU/copp}}
}
Contact
For COPP PRO licensing, commercial collaboration, technical consulting, or general inquiries, please contact [email protected].