deke_types/
validator_dynamic.rs

1use crate::{DekeError, DekeResult, JointValidator, SRobotQ, SRobotQLike, Validator};
2
3macro_rules! dynamic_joint_new {
4    ($lower:ident, $upper:ident, $($variant:ident $n:literal),+) => {
5        match $lower.len() {
6            $($n => {
7                let lo: [f32; $n] = $lower.as_slice().try_into().map_err(|_| DekeError::ShapeMismatch { expected: $n, found: $lower.len() })?;
8                let hi: [f32; $n] = $upper.as_slice().try_into().map_err(|_| DekeError::ShapeMismatch { expected: $n, found: $upper.len() })?;
9                Ok(DynamicJointValidator::$variant(JointValidator::new(SRobotQ(lo), SRobotQ(hi))))
10            }),+,
11            _ => Err(DekeError::ShapeMismatch { expected: 8, found: $lower.len() }),
12        }
13    };
14}
15
16#[derive(Debug, Clone)]
17pub enum DynamicJointValidator {
18    J1(JointValidator<1>),
19    J2(JointValidator<2>),
20    J3(JointValidator<3>),
21    J4(JointValidator<4>),
22    J5(JointValidator<5>),
23    J6(JointValidator<6>),
24    J7(JointValidator<7>),
25    J8(JointValidator<8>),
26}
27
28impl DynamicJointValidator {
29    pub fn try_new(lower: Vec<f32>, upper: Vec<f32>) -> DekeResult<Self> {
30        if lower.len() != upper.len() {
31            return Err(DekeError::ShapeMismatch {
32                expected: lower.len(),
33                found: upper.len(),
34            });
35        }
36        dynamic_joint_new!(lower, upper, J1 1, J2 2, J3 3, J4 4, J5 5, J6 6, J7 7, J8 8)
37    }
38
39    pub fn dof(&self) -> usize {
40        match self {
41            Self::J1(_) => 1,
42            Self::J2(_) => 2,
43            Self::J3(_) => 3,
44            Self::J4(_) => 4,
45            Self::J5(_) => 5,
46            Self::J6(_) => 6,
47            Self::J7(_) => 7,
48            Self::J8(_) => 8,
49        }
50    }
51
52    pub fn validate_dyn(&self, q: &[f32]) -> DekeResult<()> {
53        match self {
54            Self::J1(v) => {
55                let arr: &[f32; 1] = q.try_into().map_err(|_| DekeError::ShapeMismatch {
56                    expected: 1,
57                    found: q.len(),
58                })?;
59                v.validate(SRobotQ(*arr), &())
60            }
61            Self::J2(v) => {
62                let arr: &[f32; 2] = q.try_into().map_err(|_| DekeError::ShapeMismatch {
63                    expected: 2,
64                    found: q.len(),
65                })?;
66                v.validate(SRobotQ(*arr), &())
67            }
68            Self::J3(v) => {
69                let arr: &[f32; 3] = q.try_into().map_err(|_| DekeError::ShapeMismatch {
70                    expected: 3,
71                    found: q.len(),
72                })?;
73                v.validate(SRobotQ(*arr), &())
74            }
75            Self::J4(v) => {
76                let arr: &[f32; 4] = q.try_into().map_err(|_| DekeError::ShapeMismatch {
77                    expected: 4,
78                    found: q.len(),
79                })?;
80                v.validate(SRobotQ(*arr), &())
81            }
82            Self::J5(v) => {
83                let arr: &[f32; 5] = q.try_into().map_err(|_| DekeError::ShapeMismatch {
84                    expected: 5,
85                    found: q.len(),
86                })?;
87                v.validate(SRobotQ(*arr), &())
88            }
89            Self::J6(v) => {
90                let arr: &[f32; 6] = q.try_into().map_err(|_| DekeError::ShapeMismatch {
91                    expected: 6,
92                    found: q.len(),
93                })?;
94                v.validate(SRobotQ(*arr), &())
95            }
96            Self::J7(v) => {
97                let arr: &[f32; 7] = q.try_into().map_err(|_| DekeError::ShapeMismatch {
98                    expected: 7,
99                    found: q.len(),
100                })?;
101                v.validate(SRobotQ(*arr), &())
102            }
103            Self::J8(v) => {
104                let arr: &[f32; 8] = q.try_into().map_err(|_| DekeError::ShapeMismatch {
105                    expected: 8,
106                    found: q.len(),
107                })?;
108                v.validate(SRobotQ(*arr), &())
109            }
110        }
111    }
112
113    pub fn validate_motion_dyn(&self, qs: &[&[f32]]) -> DekeResult<()> {
114        for q in qs {
115            self.validate_dyn(q)?;
116        }
117        Ok(())
118    }
119}
120
121macro_rules! impl_dynamic_joint {
122    ($($n:literal $variant:ident),+) => {
123        $(
124            impl Validator<$n> for DynamicJointValidator {
125                type Context<'ctx> = ();
126
127                fn validate<'ctx, E: Into<DekeError>, A: SRobotQLike<$n, E>>(
128                    &self,
129                    q: A,
130                    ctx: &Self::Context<'ctx>,
131                ) -> DekeResult<()> {
132                    match self {
133                        Self::$variant(v) => v.validate(q, ctx),
134                        _ => Err(DekeError::ShapeMismatch {
135                            expected: self.dof(),
136                            found: $n,
137                        }),
138                    }
139                }
140
141                fn validate_motion<'ctx>(
142                    &self,
143                    qs: &[SRobotQ<$n>],
144                    ctx: &Self::Context<'ctx>,
145                ) -> DekeResult<()> {
146                    match self {
147                        Self::$variant(v) => v.validate_motion(qs, ctx),
148                        _ => Err(DekeError::ShapeMismatch {
149                            expected: self.dof(),
150                            found: $n,
151                        }),
152                    }
153                }
154            }
155
156            impl From<JointValidator<$n>> for DynamicJointValidator {
157                fn from(v: JointValidator<$n>) -> Self {
158                    Self::$variant(v)
159                }
160            }
161        )+
162    };
163}
164
165impl_dynamic_joint!(1 J1, 2 J2, 3 J3, 4 J4, 5 J5, 6 J6, 7 J7, 8 J8);
166
167impl DynamicJointValidator {
168    pub fn from_validator(v: impl Into<Self>) -> Self {
169        v.into()
170    }
171}
deke_types/validator_dynamic.rs

deke_types/
validator_dynamic.rs
