Struct Path 

pub struct Path { /* private fields */ }

Unified path abstraction over parametric and spline representations.

Construct via Path::from_parametric or Path::from_waypoints, 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

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 ∈ [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)

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

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), evaluated at s[0]=0.0
assert!((dq[(0, 0)] - 0.0_f64.cos()).abs() < 1e-10);
// dim 1 is cos(s); its third derivative is sin(s), evaluated at s[0]=0.0
assert!((dddq[(1, 0)] - 0.0_f64.sin()).abs() < 1e-10);