copp\diag/
error.rs

1use clarabel::solver::{SolverError as ClarabelSolverError, SolverStatus as ClarabelSolverStatus};
2use thiserror::Error;
3
4/// The error type for COPP.
5#[derive(Error)]
6pub enum CoppError {
7    /// Some error occurred in filesystem I/O operations, such as opening/creating log files.
8    #[error("I/O error: {0}")]
9    IoError(#[from] std::io::Error),
10    /// Some error occurred in the Constraint struct, such as invalid input or computation failure.
11    #[error("Some error occurred in the Constraint struct: {0}")]
12    ConstraintError(#[from] ConstraintError),
13    /// Some error occurred in the Path struct, such as invalid input or computation failure.
14    #[error("Some error occurred in the Path struct: {0}")]
15    PathError(#[from] PathError),
16    /// The specified optimization problem is reported infeasible by the backend solver.
17    #[error("{0} reported an infeasibility: {1}")]
18    Infeasible(String, String),
19    /// The specified optimization problem is reported unbounded by the backend solver.
20    #[error("{0} reported an unboundedness: {1}")]
21    Unbounded(String, String),
22    /// The solver/backend rejects the given model or data as invalid input.
23    #[error("{0} reported an invalid input: {1}")]
24    InvalidInput(String, String),
25    /// The solver/backend rejects the provided configuration options.
26    #[error("{0} reported an invalid options: {1}")]
27    InvalidOptions(String, String),
28    /// The Clarabel solver returned a concrete internal solver error.
29    #[error("{0} reported an error in Clarabel solver: {1}")]
30    ClarabelSolverError(String, #[source] ClarabelSolverError),
31    /// The Clarabel solver terminated with a non-success status.
32    #[error("{0} reported a failure in Clarabel solver with status {1}")]
33    ClarabelSolverStatus(String, ClarabelSolverStatus),
34    /// A backend-specific or uncategorized runtime error is reported.
35    #[error("{0} reported an error: {1}")]
36    Other(String, String),
37}
38
39/// Error type for constraint storage/query operations.
40///
41/// # Usage recommendation
42/// For public-facing application code, prefer using [`CoppError`](crate::diag::CoppError)
43/// as the unified error type.
44///
45/// [`ConstraintError`] is automatically converted into [`CoppError`] via
46/// `From<ConstraintError> for CoppError`, so `?` can be used directly when
47/// your function returns `Result<_, CoppError>`.
48#[derive(Error, Debug)]
49pub enum ConstraintError {
50    /// Input station sequence is not strictly increasing.
51    #[error("`s` must be strictly increasing; first violation at local index {index}.")]
52    NonIncreasingS { index: usize },
53
54    /// Input matrix/vector dimensions are incompatible with expected shape.
55    #[error("Input dimensions do not match the expected shape.")]
56    NoMatchDimensions,
57
58    /// Input ordering contract is violated.
59    #[error("Input order does not satisfy the expected contract.")]
60    NoMatchOrder,
61
62    /// Signed upper/lower bounds violate strict feasibility contract.
63    #[error(
64        "`{bound_name}` requires strict signed limits at every station: upper bound > 0 and lower bound < 0."
65    )]
66    InvalidSignedBounds { bound_name: &'static str },
67
68    /// Requested station interval is outside currently stored constraints range.
69    #[error("Requested station interval is out of bounds: idx_s={idx_s}, len={len}.")]
70    OutOfSBounds { idx_s: usize, len: usize },
71
72    /// `a` violates positivity / non-negativity preconditions.
73    #[error(
74        "Input `a` violates positivity requirements (must be nonnegative, and strictly positive where required)."
75    )]
76    NonPositiveA,
77
78    /// Linearization floor must be strictly positive.
79    #[error("Linearization floor must be strictly positive.")]
80    NonPositiveLinearizationFloor,
81
82    /// Required path-derivative data is missing in the requested interval.
83    #[error(
84        "Required derivative data (`q`, `dq`, `ddq`, `dddq`) is not fully available in the requested interval."
85    )]
86    NoGivenQInfo,
87
88    /// Linearized jerk constraints are unavailable at the requested station.
89    #[error(
90        "Linearized jerk constraints are unavailable at idx_s={idx_s}; valid range is [{}, {}).",
91        valid_range.0,
92        valid_range.1
93    )]
94    LinearJerkNotAvailable {
95        idx_s: usize,
96        valid_range: (usize, usize),
97    },
98
99    /// Dynamic-model data has not been provided.
100    #[error("Required dynamic-model information is not available.")]
101    NoDynamic,
102
103    /// Reference profile is infeasible under current constraints.
104    #[error("Reference profile is infeasible under current constraints.")]
105    InfeasibleReference,
106
107    /// Requested interval is empty.
108    #[error("Requested interval is empty: {start} <= idx_s < {end}.")]
109    EmptyInterval { start: usize, end: usize },
110}
111
112/// Error type for path construction and path evaluation APIs.
113///
114/// This error is returned by path-related modules such as [`Path`](`crate::path::Path`)
115/// and spline utilities when input data, parameter ranges, or numerical systems
116/// are invalid.
117#[derive(Error, Debug)]
118pub enum PathError {
119    /// Path dimension is invalid (typically zero).
120    #[error("invalid dimension: {dim}")]
121    InvalidDimension { dim: usize },
122    /// Path parameter range is invalid (must satisfy finite `s_min < s_max`).
123    #[error("invalid s range: [{s_min}, {s_max}]")]
124    InvalidRange { s_min: f64, s_max: f64 },
125    /// Spline order is invalid (must satisfy required minimum/order constraints).
126    #[error("invalid spline order: {order}, expected >= 3")]
127    InvalidOrder { order: usize },
128    /// Matrix/tensor shapes are incompatible for the requested operation.
129    #[error("dimension mismatch")]
130    DimensionMismatch,
131    /// Input `s` has invalid matrix shape (must be `1xN` or `Nx1`).
132    #[error("invalid shape for parameter s: ({rows}, {cols}), expected 1xN or Nx1")]
133    InvalidSShape { rows: usize, cols: usize },
134    /// Waypoint sequence is too short to build a valid path.
135    #[error("not enough waypoints: {n}, expected >= 2")]
136    NotEnoughWaypoints { n: usize },
137    /// Query parameter `s` is outside the configured valid interval.
138    #[error("s out of range [{s_min}, {s_max}] at index {index}: {value}")]
139    OutOfRangeS {
140        s_min: f64,
141        s_max: f64,
142        index: usize,
143        value: f64,
144    },
145    /// Boundary conditions are not supported for the requested spline order.
146    #[error("unsupported boundary for order={order}")]
147    UnsupportedBoundary { order: usize },
148    /// Internal linear system is singular and cannot be solved robustly.
149    #[error("singular linear system")]
150    SingularSystem,
151}
152
153/// Force the debug format of ToppError to be the same as the display format, which is more concise and user-friendly.
154impl std::fmt::Debug for CoppError {
155    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
156        write!(f, "{self}")
157    }
158}
159
160/// Check whether a given s_interval is valid.
161#[inline(always)]
162pub(crate) fn check_s_interval_valid(
163    function_name: &str,
164    idx_s_start: usize,
165    idx_s_final: usize,
166) -> Result<(), CoppError> {
167    if idx_s_final < idx_s_start + 2 {
168        Err(CoppError::InvalidInput(
169            function_name.into(),
170            format!(
171                "The final index {idx_s_final} must be at least two positions after the start index {idx_s_start} in Topp2Problem."
172            ),
173        ))
174    } else {
175        Ok(())
176    }
177}
178
179/// Check the positivity of a given tolerance.
180#[inline(always)]
181pub(crate) fn check_abs_rel_tol(
182    function_name: &str,
183    abs_tol_name: &str,
184    abs_tol: f64,
185    rel_tol_name: &str,
186    rel_tol: f64,
187) -> Result<(), CoppError> {
188    check_not_nan_infinite(function_name, abs_tol_name, abs_tol)?;
189    check_not_nan_infinite(function_name, rel_tol_name, rel_tol)?;
190    if abs_tol >= 0.0 && rel_tol >= 0.0 && (abs_tol > f64::EPSILON || rel_tol > f64::EPSILON) {
191        Ok(())
192    } else {
193        Err(CoppError::InvalidOptions(
194            function_name.into(),
195            format!(
196                "At least one of {abs_tol_name} = {abs_tol} and {rel_tol_name} = {rel_tol} must be strictly positive."
197            ),
198        ))
199    }
200}
201
202#[inline(always)]
203pub(crate) fn check_not_nan_infinite(
204    function_name: &str,
205    var_name: &str,
206    var_value: f64,
207) -> Result<(), CoppError> {
208    if var_value.is_nan() {
209        Err(CoppError::InvalidOptions(
210            function_name.into(),
211            format!("{var_name} = {var_value} must not be NaN",),
212        ))
213    } else if var_value.is_infinite() {
214        Err(CoppError::InvalidOptions(
215            function_name.into(),
216            format!("{var_name} = {var_value} must not be infinite",),
217        ))
218    } else {
219        Ok(())
220    }
221}
222
223/// Check the non-negativity.
224#[inline(always)]
225pub(crate) fn check_non_negative(
226    function_name: &str,
227    var_name: &str,
228    var_value: f64,
229) -> Result<(), CoppError> {
230    check_not_nan_infinite(function_name, var_name, var_value)?;
231    if var_value < 0.0 {
232        Err(CoppError::InvalidOptions(
233            function_name.into(),
234            format!("{var_name} = {var_value} must be strictly non-negative"),
235        ))
236    } else {
237        Ok(())
238    }
239}
240
241/// Check the positivity of a given tolerance.
242#[inline(always)]
243pub(crate) fn check_strictly_positive(
244    function_name: &str,
245    var_name: &str,
246    var_value: f64,
247) -> Result<(), CoppError> {
248    check_not_nan_infinite(function_name, var_name, var_value)?;
249    if var_value < f64::EPSILON {
250        Err(CoppError::InvalidOptions(
251            function_name.into(),
252            format!("{var_name} = {var_value} must be strictly positive"),
253        ))
254    } else {
255        Ok(())
256    }
257}
258
259#[inline(always)]
260pub(crate) fn check_boundary_state_copp3_valid(
261    a_boundary: (f64, f64),
262    b_boundary: (f64, f64),
263) -> Result<(), CoppError> {
264    if a_boundary.0 < 0.0 {
265        return Err(CoppError::InvalidInput(
266            "copp3_socp".into(),
267            format!("The initial a = {} must be non-negative.", a_boundary.0),
268        ));
269    }
270    if a_boundary.1 < 0.0 {
271        return Err(CoppError::InvalidInput(
272            "copp3_socp".into(),
273            format!("The terminal a = {} must be non-negative.", a_boundary.1),
274        ));
275    }
276    if a_boundary.0.abs() < f64::EPSILON {
277        // If a[0]==0 but b[0]!=0, then a<0 will occur near s_start
278        if b_boundary.0.abs() >= f64::EPSILON {
279            return Err(CoppError::InvalidInput(
280                "copp3_socp".into(),
281                format!(
282                    "The initial a = {} is zero, so the initial b = {} must also be zero.",
283                    a_boundary.0, b_boundary.0
284                ),
285            ));
286        }
287    }
288    if a_boundary.1.abs() < f64::EPSILON {
289        // If a[n]==0 but b[n]!=0, then a<0 will occur near s_final
290        if b_boundary.1.abs() >= f64::EPSILON {
291            return Err(CoppError::InvalidInput(
292                "copp3_socp".into(),
293                format!(
294                    "The terminal a = {} is zero, so the terminal b = {} must also be zero.",
295                    a_boundary.1, b_boundary.1
296                ),
297            ));
298        }
299    }
300    Ok(())
301}
copp\diag/error.rs

copp\diag/
error.rs
