Struct Path 

pub struct Path { /* private fields */ }

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

Construct via Path::from_parametric, Path::from_waypoints, 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_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( waypoints: &DMatrix<f64>, cfg: SplineConfig, ) -> Result<Self, PathError>

Build a spline path by interpolating a waypoint matrix.

Internally 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(&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_view( waypoints: DMatrixView<'_, f64>, cfg: SplineConfig, ) -> Result<Self, PathError>

Build a spline path from a borrowed waypoint matrix view.

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

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);