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 `+鈭瀈; 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.

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.