deke_types/
validator_dynamic.rs

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

deke_types/
validator_dynamic.rs
