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

deke_types/
validator_dynamic.rs
