nabled-dynamics 0.0.11

RNEA, CRBA, and forward dynamics (serial + tree) for nabled Physical AI
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87

//! Robot dynamics.

#![allow(clippy::missing_errors_doc, clippy::too_many_arguments, clippy::similar_names)]

use nabled_core::errors::{IntoNabledError, NabledError, ShapeError};

pub mod config;
pub mod crba;
pub mod fd;
pub mod id;
pub mod rnea;
pub mod spatial;
pub mod tree;

pub use config::{DynamicsConfig, ForwardDynamicsMethod};

#[derive(Debug, Clone, PartialEq)]
pub enum DynamicsError {
    EmptyModel,
    DimensionMismatch,
    InvalidInput(String),
    NotImplemented,
}

impl std::fmt::Display for DynamicsError {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        match self {
            DynamicsError::EmptyModel => write!(f, "dynamics model cannot be empty"),
            DynamicsError::DimensionMismatch => write!(f, "input dimensions are incompatible"),
            DynamicsError::InvalidInput(message) => write!(f, "invalid input: {message}"),
            DynamicsError::NotImplemented => write!(f, "dynamics routine not yet implemented"),
        }
    }
}

impl std::error::Error for DynamicsError {}

impl IntoNabledError for DynamicsError {
    fn into_nabled_error(self) -> NabledError {
        match self {
            DynamicsError::EmptyModel => NabledError::Shape(ShapeError::EmptyInput),
            DynamicsError::DimensionMismatch => NabledError::Shape(ShapeError::DimensionMismatch),
            DynamicsError::InvalidInput(message) => NabledError::InvalidInput(message),
            DynamicsError::NotImplemented => {
                NabledError::Other("dynamics routine not implemented".to_string())
            }
        }
    }
}

#[cfg(test)]
mod tests {
    use nabled_core::errors::{IntoNabledError, NabledError, ShapeError};

    use super::*;

    #[test]
    fn dynamics_errors_display_and_map_to_shared_taxonomy() {
        assert_eq!(DynamicsError::EmptyModel.to_string(), "dynamics model cannot be empty");
        assert_eq!(
            DynamicsError::DimensionMismatch.to_string(),
            "input dimensions are incompatible"
        );
        assert_eq!(
            DynamicsError::InvalidInput("bad q".to_string()).to_string(),
            "invalid input: bad q"
        );
        assert_eq!(
            DynamicsError::NotImplemented.to_string(),
            "dynamics routine not yet implemented"
        );

        assert!(matches!(
            DynamicsError::EmptyModel.into_nabled_error(),
            NabledError::Shape(ShapeError::EmptyInput)
        ));
        assert!(matches!(
            DynamicsError::DimensionMismatch.into_nabled_error(),
            NabledError::Shape(ShapeError::DimensionMismatch)
        ));
        assert!(matches!(
            DynamicsError::InvalidInput("x".to_string()).into_nabled_error(),
            NabledError::InvalidInput(_)
        ));
        assert!(matches!(DynamicsError::NotImplemented.into_nabled_error(), NabledError::Other(_)));
    }
}