deke_types/
validator_dynamic.rs

1use crate::{DekeError, DekeResult, JointValidator, SRobotQ, 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(&mut 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(&mut 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                fn validate<E: Into<DekeError>, A: TryInto<SRobotQ<$n>, Error = E>>(
126                    &mut self,
127                    q: A,
128                ) -> DekeResult<()> {
129                    match self {
130                        Self::$variant(v) => v.validate(q),
131                        _ => Err(DekeError::ShapeMismatch {
132                            expected: self.dof(),
133                            found: $n,
134                        }),
135                    }
136                }
137
138                fn validate_motion(&mut self, qs: &[SRobotQ<$n>]) -> DekeResult<()> {
139                    match self {
140                        Self::$variant(v) => v.validate_motion(qs),
141                        _ => Err(DekeError::ShapeMismatch {
142                            expected: self.dof(),
143                            found: $n,
144                        }),
145                    }
146                }
147            }
148
149            impl From<JointValidator<$n>> for DynamicJointValidator {
150                fn from(v: JointValidator<$n>) -> Self {
151                    Self::$variant(v)
152                }
153            }
154        )+
155    };
156}
157
158impl_dynamic_joint!(1 J1, 2 J2, 3 J3, 4 J4, 5 J5, 6 J6, 7 J7, 8 J8);
159
160impl DynamicJointValidator {
161    pub fn from_validator(v: impl Into<Self>) -> Self {
162        v.into()
163    }
164}
deke_types/validator_dynamic.rs

deke_types/
validator_dynamic.rs
