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.1 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/topp.git
cd topp
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:
TODO
TODO
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.
TODO
TODO
#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.0], 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.
TODO
TODO
Option B. Build a Spline from Waypoints
If waypoints are available, a spline path can be constructed as follows:
TODO
TODO
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(NormalizedEvaluator3rd, 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_3rd` without dependence on `evaluate_up_to_3rd`.
path = copp.Path.from_evaluator_3rd(Evaluator(), 0.0, 1.0)
TODO
TODO
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:
TODO
TODO
Create the robot model:
Provide the \(s\) grid and path data:
TODO
TODO
// 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)?;
TODO
TODO
// 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:
TODO
TODO
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:
TODO
TODO
Then build solver options and call the solver:
TODO
TODO
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)\):
TODO
TODO
Then invert the timing relation to obtain \(s=s(t)\) and interpolate:
TODO
TODO
// 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:
TODO
TODO
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()?;
TODO
TODO
Using topp3_socp as an example, call the solver as follows:
TODO
TODO
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:
TODO
TODO
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)\):
TODO
TODO
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),
)?;
TODO
TODO
// 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:
TODO
TODO
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.
TODO
TODO
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 following results are from tests/test_random_spline.rs. 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.
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.
TODO
TODO
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. |
TODO
TODO
| 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].