Struct Constraints 

pub struct Constraints { /* private fields */ }

Constraint storage and query object used by TOPP/COPP solvers.

Mathematical symbols (with code mapping)

For each path station $s_k$:

• $a_k = \dot{s}_k^2$ (squared path speed), mapped to code symbol a[k].
• $b_k = \ddot{s}_k = \frac{1}{2}\frac{\mathrm{d}a}{\mathrm{d}s}(s_k)$ (path acceleration), mapped to b[k].
• $c_k = \frac{\dddot{s}_k}{\dot{s}_k} = \frac{\mathrm{d}b}{\mathrm{d}s}(s_k)$ (normalized jerk term), mapped to c[k].

Constraint families

• First-order: 0 <= a[k] <= amax[k]
• Second-order: acc_a[k]*a[k] + acc_b[k]*b[k] <= acc_max[k]
• Third-order (nonlinear): sqrt(a[k])*(jerk_a[k]*a[k] + jerk_b[k]*b[k] + jerk_c[k]*c[k] + jerk_d[k]) <= jerk_max[k]
• Third-order (linearized): jerk_a_linear[k]*a[k] + jerk_b[k]*b[k] + jerk_c[k]*c[k] <= jerk_max_linear[k]

Storage model

All station-wise arrays are stored as circular column-major matrices. Logical station index range is [idx_s, idx_s + len), and logical column i maps to physical column (head_col + i) % capacity_col.

API contract

• get_* methods are safe public accessors and return Result<_, ConstraintError>.
• *_unchecked methods are internal fast-path helpers. Callers must satisfy preconditions; debug builds assert them.

Example

The example below constructs low-level constraints directly, without going through Robot.

use copp::constraints::Constraints;
use nalgebra::DMatrix;

let mut constraints = Constraints::with_capacity(2, 8);

let s = [0.0, 0.5, 1.0];
constraints.with_s(s.as_slice())?;

let amax = [1.0, 0.8, 1.0];
constraints.with_constraint_1order(amax.as_slice(), 0)?;

let acc_a = DMatrix::from_row_slice(2, 3, &[
    0.0, 0.0, 0.0,
    0.0, 0.0, 0.0,
]);
let acc_b = DMatrix::from_row_slice(2, 3, &[
    1.0, 1.0, 1.0,
    -1.0, -1.0, -1.0,
]);
let acc_max = DMatrix::from_row_slice(2, 3, &[
    2.0, 2.0, 2.0,
    2.0, 2.0, 2.0,
]);

constraints.with_constraint_2order(
    &acc_a.as_view(),
    &acc_b.as_view(),
    &acc_max.as_view(),
    0,
    false,
)?;

assert_eq!(constraints.len(), 3);
assert_eq!(constraints.get_s(1)?, 0.5);

Implementations

impl Constraints

pub const DEFAULT_CAPACITY: usize = 1000

Default number of constraint stations preallocated for a new container.

pub fn new(dim: usize) -> Self

Construct a new container with default column capacity.

Parameters

• dim: path dimension / DoF.

Notes

Equivalent to with_capacity(dim, DEFAULT_CAPACITY).

pub fn with_capacity(dim: usize, capacity_col: usize) -> Self

Construct a new container with explicit column capacity.

Parameters

• dim: path dimension / DoF.
• capacity_col: initial number of allocated columns.

Initialization policy

• Bound matrices are initialized to neutral values (infinity where applicable).
• Valid-row maps start empty and are progressively populated by with_s().

pub fn len(&self) -> usize

Current number of logical stations stored in the buffer.

pub fn is_empty(&self) -> bool

Whether no logical stations are currently stored.

pub fn get_s(&self, idx_s: usize) -> Result<f64, ConstraintError>

Get station value s[idx_s] with bounds validation.

Errors

Returns ConstraintError::OutOfSBounds if idx_s is outside [idx_s_start(), idx_s_end()).

pub fn amax_rows(&self) -> usize

Number of rows currently allocated for first-order constraints.

Normally this is 1, but the method is intentionally generic.

pub fn acc_rows(&self) -> usize

Number of rows currently allocated for second-order constraints.

pub fn jerk_rows(&self) -> usize

Number of rows currently allocated for third-order constraints.

pub fn s_vec( &self, idx_s_from: usize, idx_s_to: usize, ) -> Result<Vec<f64>, ConstraintError>

Export station values in half-open interval [idx_s_from, idx_s_to).

Parameters

• idx_s_from: global start station id (inclusive).
• idx_s_to: global end station id (exclusive).

Returns

A contiguous vector of station values with length idx_s_to - idx_s_from.

Errors

• ConstraintError::EmptyInterval when idx_s_from >= idx_s_to.
• ConstraintError::OutOfSBounds if the interval is outside stored data.

pub fn amax_vec( &self, idx_s_from: usize, idx_s_to: usize, ) -> Result<Vec<f64>, ConstraintError>

Export first-order upper bounds in [idx_s_from, idx_s_to).

Semantics and error behavior are identical to s_vec().

pub fn get_amax(&self, idx_s: usize) -> Result<f64, ConstraintError>

Get first-order upper bound amax[idx_s] with bounds validation.

Errors

Returns ConstraintError::OutOfSBounds if idx_s is invalid.

pub fn idx_s_start(&self) -> usize

Global start station id (inclusive) of current logical window.

pub fn idx_s_end(&self) -> usize

Global end station id (exclusive) of current logical window.

pub fn get_acc_constraints<'a>( &'a self, idx_s: usize, ) -> Result<(InputMatrix<'a>, InputMatrix<'a>, InputMatrix<'a>), ConstraintError>

Get second-order row views at station idx_s.

Returns

(acc_a_col, acc_b_col, acc_max_col) where each matrix is a valid_rows x 1 view into internal storage.

Errors

Returns ConstraintError::OutOfSBounds if idx_s is invalid.

pub fn get_jerk_constraints<'a>( &'a self, idx_s: usize, ) -> Result<(InputMatrix<'a>, InputMatrix<'a>, InputMatrix<'a>, InputMatrix<'a>, InputMatrix<'a>), ConstraintError>

Get nonlinear third-order row views at station idx_s.

Returns

(jerk_a, jerk_b, jerk_c, jerk_d, jerk_max), each a valid_rows x 1 view.

Errors

Returns ConstraintError::OutOfSBounds if idx_s is invalid.

pub fn get_jerk_linear_constraints<'a>( &'a self, idx_s: usize, ) -> Result<(InputMatrix<'a>, InputMatrix<'a>, InputMatrix<'a>, InputMatrix<'a>), ConstraintError>

Get linearized third-order row views at station idx_s.

The linearized rows are prepared by Topp3ProblemBuilder::build_with_linearization or Copp3ProblemBuilder::build_with_linearization. Call this accessor when solver-side code needs to inspect the affine third-order rows generated from the latest reference profile.

Returns

(jerk_a_linear, jerk_b, jerk_c, jerk_max_linear), each a valid_rows x 1 view into internal storage.

Errors

• ConstraintError::OutOfSBounds if idx_s is outside current station window.
• ConstraintError::LinearJerkNotAvailable if idx_s is not covered by the latest linearization interval.

pub fn expand_capacity(&mut self, new_capacity: usize)

Ensure buffer capacity is at least new_capacity columns.

Growth strategy

If expansion is required, target capacity is max(new_capacity, 2 * current_capacity + 1).

Guarantees

• Logical order of existing data is preserved.
• head_col is reset to 0 after re-layout.
• All backing matrices (s, derivative buffers, and constraint buffers) are expanded consistently.

pub fn with_s<T: AsInputMatrix1D + ?Sized>( &mut self, s_new: &T, ) -> Result<&mut Self, ConstraintError>

Append strictly increasing station samples to the logical tail.

Parameters

• s_new: a 1 x N station segment; accepted via AsInputMatrix1D.

For the higher-level robot wrapper, Robot::with_s delegates to this method.

Behavior

• Rejects non-increasing input.
• Rejects overlap with existing tail station (s_new[0] must be greater than current last station when the buffer is non-empty).
• Expands capacity proactively.
• Appends zero-valid-row segments into all validity maps so downstream with_q / with_constraint_* calls can progressively fill data.

Errors

Returns ConstraintError::NonIncreasingS on monotonicity violations.

Returns

Returns &mut Self for chaining on success.

pub fn with_q( &mut self, q_new: &InputMatrix<'_>, dq_new: &InputMatrix<'_>, ddq_new: &InputMatrix<'_>, dddq_new: Option<&InputMatrix<'_>>, idx_s: usize, ) -> Result<&mut Self, ConstraintError>

Write path geometry derivatives on a station interval.

This is the low-level entry point for users who populate Constraints directly. Robot-centric workflows usually call Robot::with_q, Robot::with_q_from_path_2nd, or Robot::with_q_from_path_3rd, which forward into this storage layer.

Parameters

• q_new: configuration values (dim x N).
• dq_new: first derivatives (dim x N).
• ddq_new: second derivatives (dim x N).
• dddq_new: optional third derivatives (dim x N).
• idx_s: global start station id (inclusive).

Behavior

• Performs shape checks and bounds checks.
• Overwrites corresponding circular-buffer ranges.
• Marks second-order derivative data as fully valid (n_rows = dim) over the updated range.
• If dddq_new is provided, writes it and marks third-order derivative data as valid over the updated range.
• If dddq_new is None, clears third-order derivative availability over the updated range.

Errors

• ConstraintError::NoMatchDimensions on shape mismatch.
• ConstraintError::OutOfSBounds if target interval is invalid.

Returns

Returns &mut Self for chaining on success.

Example

The example below writes one-dimensional path geometry into a directly constructed constraint container.

use copp::constraints::Constraints;
use nalgebra::DMatrix;

let mut constraints = Constraints::with_capacity(2, 3);
let s = [0.0, 0.5, 1.0];
constraints.with_s(s.as_slice())?;

let q = DMatrix::from_row_slice(2, 3, &[
    0.0, 0.125, 0.5,
    1.0, 1.0, 1.0,
]);
let dq = DMatrix::from_row_slice(2, 3, &[
    0.0, 0.5, 1.0,
    0.0, 0.0, 0.0,
]);
let ddq = DMatrix::from_row_slice(2, 3, &[
    1.0, 1.0, 1.0,
    0.0, 0.0, 0.0,
]);

constraints.with_q(&q.as_view(), &dq.as_view(), &ddq.as_view(), None, 0)?;
assert_eq!(constraints.len(), 3);

pub fn with_constraint_1order<T: AsInputMatrix1D + ?Sized>( &mut self, amax_new: &T, idx_s: usize, ) -> Result<&mut Self, ConstraintError>

Add / tighten first-order bound amax over an interval.

Parameters

• amax_new: candidate upper bounds as R x N; each column is reduced to its minimum before being fused into storage.
• idx_s: global start station id.

Fusion rule

Stored value is updated as self.amax = min(self.amax, amax_new_reduced).

Errors

• ConstraintError::OutOfSBounds if interval is invalid.
• ConstraintError::NonPositiveA if any reduced bound is non-positive.

Returns

Returns &mut Self for chaining on success.

pub fn with_constraint_2order( &mut self, acc_a_new: &InputMatrix<'_>, acc_b_new: &InputMatrix<'_>, acc_max_new: &InputMatrix<'_>, idx_s: usize, is_negative: bool, ) -> Result<&mut Self, ConstraintError>

Append second-order inequality rows over station interval starting at idx_s.

Model

For each station column, rows satisfy: acc_a * a + acc_b * b <= acc_max.

Parameters

• acc_a_new, acc_b_new, acc_max_new: same-shape matrices (R x N).
• idx_s: global start station id.
• is_negative: whether to negate inserted rows (used to build symmetric upper/lower bounds from one physical expression).

Behavior

• Increases row counts by R on affected stations.
• Appends new rows below existing rows per station.
• Merges adjacent validity intervals when row counts match.

Errors

• ConstraintError::NoMatchDimensions for shape mismatch.
• ConstraintError::OutOfSBounds for invalid interval.

Returns

Returns &mut Self for chaining on success.

pub fn with_constraint_3order( &mut self, jerk_a_new: &InputMatrix<'_>, jerk_b_new: &InputMatrix<'_>, jerk_c_new: &InputMatrix<'_>, jerk_d_new: &InputMatrix<'_>, jerk_max_new: &InputMatrix<'_>, idx_s: usize, is_negative: bool, ) -> Result<&mut Self, ConstraintError>

Append third-order nonlinear inequality rows over station interval.

Model

sqrt(a) * (jerk_a*a + jerk_b*b + jerk_c*c + jerk_d) <= jerk_max

Parameters

• jerk_*_new: same-shape matrices (R x N).
• idx_s: global start station id.
• is_negative: if true, inserted rows are sign-flipped.

Side effects

If the inserted interval overlaps current valid_ids_linear_jerk, the linearization-valid interval is cleared because source nonlinear rows changed.

Errors

• ConstraintError::NoMatchDimensions for shape mismatch.
• ConstraintError::OutOfSBounds for invalid interval.

Returns

Returns &mut Self for chaining on success.

Example

The example below installs a single jerk upper-bound row at three stations. After changing third-order rows, build a TOPP3/COPP3 problem with build_with_linearization() before reading linearized jerk rows.

use copp::constraints::Constraints;
use nalgebra::DMatrix;

let mut constraints = Constraints::with_capacity(2, 3);
let s = [0.0, 0.5, 1.0];
constraints.with_s(s.as_slice())?;

let zero = DMatrix::from_row_slice(2, 3, &[
    0.0, 0.0, 0.0,
    0.0, 0.0, 0.0,
]);
let jerk_c = DMatrix::from_row_slice(2, 3, &[
    1.0, 1.0, 1.0,
    -1.0, -1.0, -1.0,
]);
let jerk_max = DMatrix::from_row_slice(2, 3, &[
    5.0, 5.0, 5.0,
    5.0, 5.0, 5.0,
]);

constraints.with_constraint_3order(
    &zero.as_view(),
    &zero.as_view(),
    &jerk_c.as_view(),
    &zero.as_view(),
    &jerk_max.as_view(),
    0,
    false,
)?;
assert_eq!(constraints.get_jerk_constraints(1)?.0.nrows(), 2);

pub fn pop_front(&mut self, mode: ModePopConstraints)

Remove a prefix of logical stations from the front.

Modes

• ModePopConstraints::CutAtIdxS(cut): keep stations with id >= cut.
• ModePopConstraints::PopNCols(n): remove first n logical stations.

Notes

• amax values in removed columns are reset to +inf.
• Valid-row maps are trimmed and re-anchored.
• idx_s increases and head_col advances accordingly.

pub fn pop_back(&mut self, mode: ModePopConstraints)

Remove a suffix of logical stations from the back.

Modes

• ModePopConstraints::CutAtIdxS(cut): keep stations with id < cut.
• ModePopConstraints::PopNCols(n): remove last n logical stations.

Notes

• amax values in removed columns are reset to +inf.
• Valid-row maps are trimmed to new right boundary.
• idx_s is unchanged; only len shrinks.

pub fn clear(&mut self, keep_idx_s: bool)

Reset logical content and validity maps.

Parameters

• keep_idx_s: when true, preserve current global station origin; otherwise reset it to 0.

Notes

amax is reinitialized to +inf; other matrices are kept allocated and may retain old values outside the active logical window.

pub fn amax_substitute( &mut self, amax_new: &[f64], idx_from: usize, ) -> Result<(), ConstraintError>

Overwrite first-order bounds in [idx_from, idx_from + amax_new.len()).

Parameters

• amax_new: replacement values.
• idx_from: global start station id.

Errors

Returns ConstraintError::OutOfSBounds if target range is invalid.

pub fn exceed_topp2(&self, idx_s_start: usize, a_profile: &[f64]) -> (f64, f64)

Evaluate maximum violation magnitudes of the TOPP2 constraints for a profile.

a_profile holds a = s_dot^2 on the stations starting at idx_s_start. The path acceleration of each interval is reconstructed from it by the TOPP2 relation b[k] = (a[k+1] - a[k]) / (2 * ds[k]), and the acceleration rows of both endpoint stations are checked against that interval’s b.

Returns

(exceed_1order, exceed_2order), each <= 0 when fully feasible and positive when violated. The first-order term covers 0 <= a[k] <= amax[k]. Returns NaNs if the station range is unavailable or b cannot be reconstructed from a_profile, e.g. because it contains a non-finite value.

pub fn exceed_topp3( &self, idx_s_start: usize, a_profile: &[f64], b_profile: &[f64], num_stationary: (usize, usize), ) -> (f64, f64, f64)

Evaluate maximum violation magnitudes of the TOPP3 constraints for a profile.

The third-order term uses the original sqrt(a) form, not the linearized one, so this certifies the profile that is actually delivered rather than the model the solver optimized. Run it after any post-processing such as force_positive_a, which rewrites a and b without knowing about the acceleration and jerk limits.

The third-order term covers only the intervals the solver models with a constant c, i.e. it skips the two stationary blocks. There b follows the sdddot-constant law and c = 2a/(9 d^2), so a finite difference of b across a block edge overestimates c by a factor of three and would report a violation that the model never claimed.

Returns

(exceed_1order, exceed_2order, exceed_3order), each <= 0 when fully feasible and positive when violated. Returns NaNs if the station range is unavailable, the two profiles disagree in length, or either profile contains a non-finite value.

Trait Implementations

impl Clone for Constraints

fn clone(&self) -> Constraints

Returns a duplicate of the value.

fn clone_from(&mut self, source: &Self)

Performs copy-assignment from source.