Struct Path 

pub struct Path { /* private fields */ }

Unified path abstraction over parametric, spline, and evaluator representations.

Construct via Path::from_parametric, Path::from_waypoints_interpolating, Path::from_waypoints_fitting, Path::from_evaluator_2nd, or Path::from_evaluator_3rd, then query a batch of parameter values with the evaluate_* family of methods.

The valid parameter domain is [s_min, s_max] (set at construction time). Out-of-range behaviour is controlled by OutOfRangeMode: the default is to return an error; it can be changed to silent clamping.

Implementations

impl Path

pub fn from_waypoints_fitting( waypoints: &DMatrix<f64>, cfg: SmoothingConfig, ) -> Result<Self, PathError>

Construct a tolerance-bounded C4 quintic approximation of a waypoint polyline.

This constructor is intended for sampled or programmed paths whose selected coordinates may be denoised or rounded within explicit absolute tolerances. Use Path::from_waypoints_interpolating when every waypoint must be interpolated. For a borrowed compatible strided column-major matrix, use Path::from_waypoints_fitting_view.

API stability

This constructor is currently unstable. Its name, signature, configuration, and algorithm-selection interface may change as additional waypoint-fitting methods are introduced.

Reference path and tolerances

waypoints has shape (dim, n_points) and each column is one ordered waypoint. A common parameter is assigned to every column by SmoothingConfig::parameters, or uniformly on [0, 1] by default. Adjacent columns are linearly interpolated at that common parameter to form the reference polyline.

For each selected axis j, successful construction numerically audits the complete-domain contract

$$ \max_s |q_j(s)-r_j(s)| \le \varepsilon_j. $$

This is a same-parameter, per-axis absolute bound in input-coordinate units. It is not merely a waypoint check, a nearest-distance/Hausdorff bound, or a Cartesian error after forward kinematics.

Continuity and derivatives

Selected axes use one adaptive nonuniform quintic B-spline with simple interior knots, giving C4 continuity. Their first and last positions are fixed to the input, while endpoint derivatives are determined by the fit. Unselected axes use a separate quintic C4 interpolant through all input columns, with zero first and second parameter derivatives at both endpoints. Positional endpoint equalities are subject only to ordinary floating-point roundoff.

Returned derivatives are with respect to the configured path parameter s, not time. Smoothing therefore does not replace time parameterization or impose physical velocity, acceleration, or jerk limits.

Construction method

1. Parameters are mapped to [0, 1]; every selected coordinate is shifted by its first value and divided by its tolerance.
2. A straight-chord fast path is tried first. Otherwise a shared slope corridor seeds nonuniform knots near changes in direction.
3. An open degree-five B-spline is fitted to the reference polyline with a span-scaled third-parameter-derivative penalty. Compact banded normal equations exploit the six-function local support.
4. Every spline-span/reference-segment intersection is converted to a degree-five Bernstein error polynomial. Its coefficients provide an exact-arithmetic sufficient bound over the complete intersection.
5. Failed regions receive new knots and local control-point refits until the computed bounds pass or a configured budget is exhausted.
6. Coordinates are restored to physical units and the entire domain is audited again before the path is returned.

The least-squares fit does not directly impose hard error constraints; the tolerance behavior comes from this audit/refinement/rejection loop. Bernstein bounds are computed with ordinary f64 arithmetic and are therefore numerical audit results, not outward-rounded formal proofs.

Arguments

• waypoints: finite (dim, n_points) matrix with dim > 0 and n_points >= 2; each column is one waypoint;
• cfg: selected axes, their tolerances, common waypoint parameters, refinement budgets, and out-of-range query behavior.

Returns

A unified Path that supports position and derivatives through Path::evaluate_up_to_3rd. Construction diagnostics and final error bounds are available from Path::smoothing_report.

Errors

Returns PathError::Smoothing when:

• waypoint values, dimensions, axis indices, tolerances, or parameters violate their input contracts;
• the selected-axis span count exceeds the configured segment budget;
• the banded fit encounters an unsupported or non-finite numerical system;
• refinement exhausts its pass budget or parameter resolution; or
• the final physical-unit whole-interval audit does not satisfy tolerance.

An unchecked or silently relaxed approximation is never returned.

Limitations

The adaptive knot layout is heuristic and does not guarantee the minimum span count. Bernstein bounds use ordinary floating-point arithmetic, not outward-rounded interval arithmetic. No guarantee is made for forward kinematics, collision avoidance, monotonicity, dynamics, or traversal time.

Example

use copp::path::{Path, SmoothingConfig, SmoothingTolerance};
use nalgebra::DMatrix;

let waypoints = DMatrix::from_row_slice(
    2,
    3,
    &[
        0.0, 0.5, 1.0,
        0.0, 1.0, 2.0,
    ],
);
let config = SmoothingConfig {
    axes: Some(vec![0, 1]),
    tolerance: SmoothingTolerance::PerAxis(vec![0.001, 0.01]),
    parameters: Some(vec![2.0, 3.0, 4.0]),
    ..Default::default()
};

let path = Path::from_waypoints_fitting(&waypoints, config)?;
let values = path.evaluate_up_to_3rd(&[2.0, 3.0, 4.0])?;
let report = path.smoothing_report().unwrap();

assert_eq!(values.q.shape(), (2, 3));
assert_eq!(report.axes, vec![0, 1]);
assert!(report.max_errors[0] <= 0.001);
assert!(report.max_errors[1] <= 0.01);

pub fn from_waypoints_fitting_view( waypoints: DMatrixView<'_, f64>, cfg: SmoothingConfig, ) -> Result<Self, PathError>

Construct a tolerance-bounded C4 quintic approximation from a borrowed waypoint matrix view.

This accepts nalgebra views such as waypoints.as_view() and compatible strided column-major views. Construction reads the view and stores an owned fitted representation in the returned Path, which does not borrow the input afterwards. See Path::from_waypoints_fitting for the complete fitting semantics, configuration, errors, and limitations.

API stability

This view constructor has the same unstable API status as Path::from_waypoints_fitting.

pub fn smoothing_report(&self) -> Option<&SmoothingReport>

Return construction diagnostics for a tolerance-fitted waypoint path.

Returns

• Some(report) for a path successfully constructed by either Path::from_waypoints_fitting or Path::from_waypoints_fitting_view;
• None for analytic, strictly interpolated, and user-evaluator paths.

SmoothingReport::max_errors contains Bernstein-derived whole-domain numerical bounds in physical input units and follows the order of SmoothingReport::axes. refinements, fitting_rows, and checked_intervals are cumulative work counters; axis and segment fields describe the final representation. None of these values is an optimality or run-time guarantee.

pub fn from_parametric<F>( q_fn: F, s_min: f64, s_max: f64, ) -> Result<Self, PathError>

where F: Fn(Jet3) -> Vec<Jet3> + Send + Sync + 'static,

Build a parametric path from an analytic closure.

Derivatives up to third order are computed automatically via Jet3 forward-mode AD. The closure only needs to express q(s) symbolically; no manual differentiation is required.

Arguments

• q_fn : closure mapping scalar s to a dim-dimensional position vector
• s_min : lower bound of the path parameter
• s_max : upper bound of the path parameter (s_max > s_min required)

Errors

• PathError::InvalidRange : s_min >= s_max or either value is non-finite
• PathError::InvalidDimension : closure returned an empty vector

Example

The example below builds a two-dimensional analytic path and evaluates derivatives produced by automatic differentiation.

use copp::path::autodiff::Jet3;
use copp::path::{cos, sin, Path};

let path = Path::from_parametric(|s: Jet3| vec![sin(s), cos(s)], 0.0, 1.0)?;

let s = [0.0, 0.25, 0.5, 0.75, 1.0];
let out = path.evaluate_up_to_3rd(s.as_slice())?;
assert_eq!(out.q.shape(), (2, 5));
assert!(out.dddq.is_some());

pub fn from_evaluator_2nd<E>( evaluator: E, s_min: f64, s_max: f64, ) -> Result<Self, PathError>

where E: PathEvaluator2nd + 'static,

Build a path from an evaluator that provides explicit derivatives up to second order.

Use this constructor when derivatives are already available from an external source and automatic differentiation is not desired. The evaluator is owned by the returned Path through an internal Arc, so the path can be passed around without borrowing the original value.

Arguments

• evaluator: object that writes column-major derivative buffers
• s_min: lower bound of the path parameter
• s_max: upper bound of the path parameter (s_max > s_min required)

Errors

• PathError::InvalidRange: s_min >= s_max or either value is non-finite
• PathError::InvalidDimension: evaluator dimension is zero

Example

The example below wraps a two-dimensional external evaluator that writes explicit q/dq/ddq buffers in column-major order.

use copp::diag::PathError;
use copp::path::{Path, PathEvaluator2nd};

struct NormalizedEvaluator2nd;

impl PathEvaluator2nd for NormalizedEvaluator2nd {
    fn dim(&self) -> usize {
        2
    }

    fn evaluate_up_to_2nd(
        &self,
        s: &[f64],
        q: &mut [f64],
        dq: &mut [f64],
        ddq: &mut [f64],
    ) -> Result<(), PathError> {
        for (col, &sj) in s.iter().enumerate() {
            let row0 = 2 * col;
            q[row0] = 0.5 * sj * sj;
            q[row0 + 1] = sj;
            dq[row0] = sj;
            dq[row0 + 1] = 1.0;
            ddq[row0] = 1.0;
            ddq[row0 + 1] = 0.0;
        }
        Ok(())
    }
}

let path = Path::from_evaluator_2nd(NormalizedEvaluator2nd, 0.0, 1.0)?;
let out = path.evaluate_up_to_2nd(&[0.0, 0.5])?;
assert_eq!(out.q.shape(), (2, 2));
assert_eq!(out.q[(0, 1)], 0.125);
assert!(out.dddq.is_none());

pub fn from_shared_evaluator_2nd( evaluator: Arc<dyn PathEvaluator2nd>, s_min: f64, s_max: f64, ) -> Result<Self, PathError>

Build a path from a shared explicit-derivative evaluator up to second order.

This is the same representation as Path::from_evaluator_2nd, but accepts an already shared evaluator. It is useful when multiple paths or application components need to hold the same evaluator object.

Errors

• PathError::InvalidRange: s_min >= s_max or either value is non-finite
• PathError::InvalidDimension: evaluator dimension is zero

pub fn from_evaluator_3rd<E>( evaluator: E, s_min: f64, s_max: f64, ) -> Result<Self, PathError>

where E: PathEvaluator3rd + 'static,

Build a path from an evaluator that provides explicit derivatives up to third order.

This is the constructor to use when the path will be evaluated by third-order APIs such as TOPP3/COPP3 sampling. For TOPP2/COPP2-only usage, Path::from_evaluator_2nd avoids requiring a third derivative implementation.

Errors

• PathError::InvalidRange: s_min >= s_max or either value is non-finite
• PathError::InvalidDimension: evaluator dimension is zero

Example

The example below extends a two-dimensional explicit evaluator to third order by writing jerk samples into the dddq buffer.

use copp::diag::PathError;
use copp::path::{Path, PathEvaluator2nd, PathEvaluator3rd};

struct NormalizedEvaluator3rd;

impl PathEvaluator2nd for NormalizedEvaluator3rd {
    fn dim(&self) -> usize {
        2
    }

    fn evaluate_up_to_2nd(
        &self,
        s: &[f64],
        q: &mut [f64],
        dq: &mut [f64],
        ddq: &mut [f64],
    ) -> Result<(), PathError> {
        for (col, &sj) in s.iter().enumerate() {
            let row0 = 2 * col;
            q[row0] = 0.5 * sj * sj;
            q[row0 + 1] = sj;
            dq[row0] = sj;
            dq[row0 + 1] = 1.0;
            ddq[row0] = 1.0;
            ddq[row0 + 1] = 0.0;
        }
        Ok(())
    }
}

impl PathEvaluator3rd for NormalizedEvaluator3rd {
    fn evaluate_up_to_3rd(
        &self,
        s: &[f64],
        q: &mut [f64],
        dq: &mut [f64],
        ddq: &mut [f64],
        dddq: &mut [f64],
    ) -> Result<(), PathError> {
        self.evaluate_up_to_2nd(s, q, dq, ddq)?;
        dddq.fill(0.0);
        Ok(())
    }
}

let path = Path::from_evaluator_3rd(NormalizedEvaluator3rd, 0.0, 1.0)?;
let out = path.evaluate_up_to_3rd(&[0.0, 0.5])?;
assert_eq!(out.q.shape(), (2, 2));
assert_eq!(out.q[(0, 1)], 0.125);
assert!(out.dddq.is_some());

pub fn from_shared_evaluator_3rd( evaluator: Arc<dyn PathEvaluator3rd>, s_min: f64, s_max: f64, ) -> Result<Self, PathError>

Build a path from a shared explicit-derivative evaluator up to third order.

Errors

• PathError::InvalidRange: s_min >= s_max or either value is non-finite
• PathError::InvalidDimension: evaluator dimension is zero

pub fn from_evaluator<E>( evaluator: E, s_min: f64, s_max: f64, ) -> Result<Self, PathError>

where E: PathEvaluator3rd + 'static,

Build a path from a third-order explicit-derivative evaluator.

This compatibility constructor is equivalent to Path::from_evaluator_3rd. New code should prefer Path::from_evaluator_2nd or Path::from_evaluator_3rd to make the supported derivative order explicit.

pub fn from_shared_evaluator( evaluator: Arc<dyn PathEvaluator3rd>, s_min: f64, s_max: f64, ) -> Result<Self, PathError>

Build a path from a shared third-order explicit-derivative evaluator.

This compatibility constructor is equivalent to Path::from_shared_evaluator_3rd.

pub fn from_waypoints_interpolating( waypoints: &DMatrix<f64>, cfg: SplineConfig, ) -> Result<Self, PathError>

Build a spline path that interpolates every waypoint in a matrix.

Every input column is a hard positional interpolation condition: at that waypoint’s assigned parameter, the returned path passes through the column up to floating-point roundoff. This differs from Path::from_waypoints_fitting, which may deviate from interior waypoints within configured tolerances.

API stability

This constructor is currently unstable. Its name, signature, configuration, and algorithm-selection interface may change as additional waypoint-interpolation methods are introduced.

Internally, the current algorithm solves the Hermite spline system with an O(N) block-Thomas algorithm; all dimensions are solved in parallel. The default configuration (SplineConfig::default) uses a quintic (order-5) spline with s in [0, 1].

Arguments

• waypoints : matrix of shape (dim, n_points); each column is one waypoint
• cfg : spline configuration (order, parameter range, boundary derivatives, out-of-range mode)

Errors

• PathError::InvalidDimension : waypoints has zero rows
• PathError::NotEnoughWaypoints : fewer than 2 columns
• PathError::InvalidOrder : order < 3
• PathError::InvalidRange : invalid parameter range
• PathError::SingularSystem : spline system is singular (extremely rare)

Example

The example below builds a spline through two-dimensional waypoints and evaluates position samples.

use copp::path::{Path, SplineConfig};
use nalgebra::DMatrix;

let waypoints = DMatrix::from_row_slice(
    2,
    5,
    &[
        0.0, 0.25, 0.5, 0.75, 1.0,
        0.0, 0.1, -0.1, 0.2, 0.0,
    ],
);
let path = Path::from_waypoints_interpolating(&waypoints, SplineConfig::default())?;

let s = [0.0, 0.5, 1.0];
let out = path.evaluate_q(s.as_slice())?;
assert_eq!(out.q.shape(), (2, 3));
assert!(out.dq.is_none());

pub fn from_waypoints_interpolating_view( waypoints: DMatrixView<'_, f64>, cfg: SplineConfig, ) -> Result<Self, PathError>

Build a waypoint-interpolating spline from a borrowed matrix view.

This accepts nalgebra views such as waypoints.as_view() and compatible strided column-major views. See Path::from_waypoints_interpolating for the full interpolation semantics, configuration, error conditions, and unstable API status.

pub fn dim(&self) -> usize

Returns the spatial dimension (number of joints) of the path.

pub fn s_range(&self) -> (f64, f64)

Returns the valid parameter range (s_min, s_max).

pub fn evaluate_q(&self, s: &[f64]) -> Result<PathDerivatives, PathError>

Evaluate position q only at the query points (cheapest; no derivatives).

Arguments

• s : one-dimensional parameter samples (length N)

Returns

PathDerivatives with dq / ddq / dddq all None; q has shape (dim, N).

Errors

• PathError::OutOfRangeS : a query value is out of range (only in Error mode)

pub fn evaluate_up_to_2nd( &self, s: &[f64], ) -> Result<PathDerivatives, PathError>

Evaluate position, velocity, and acceleration (q, dq, ddq); jerk is not computed.

Arguments

• s : one-dimensional parameter samples (length N)

Returns

PathDerivatives with dddq = None; q / dq / ddq each have shape (dim, N).

pub fn evaluate_up_to_3rd( &self, s: &[f64], ) -> Result<PathDerivatives, PathError>

Evaluate position and all three derivative orders (q, dq, ddq, dddq).

This is the most expensive evaluation method. If jerk is not needed, prefer evaluate_up_to_2nd.

Arguments

• s : one-dimensional parameter samples (length N)

Returns

PathDerivatives with all four fields populated; each matrix has shape (dim, N).

Errors

• PathError::OutOfRangeS : a query value is out of range

Example

The example below evaluates an analytic path up to jerk using automatic differentiation.

use copp::path::{Path, sin, cos};
use copp::path::autodiff::Jet3;

let path = Path::from_parametric(
    |s: Jet3| vec![sin(s), cos(s)],
    0.0, 1.0,
).unwrap();

let s = [0.0, 0.25, 0.5, 0.75, 1.0];
let out = path.evaluate_up_to_3rd(&s).unwrap();

let dq   = out.dq.as_ref().unwrap();
let dddq = out.dddq.as_ref().unwrap();
// dim 0 is sin(s); its first derivative is cos(s)
assert!((dq[(0, 0)] - 1.0_f64.cos()).abs() < 1e-10);
// dim 1 is cos(s); its third derivative is sin(s)
assert!((dddq[(1, 0)] - 0.0_f64.sin()).abs() < 1e-10);