deke_types/
validator.rs

1use std::{fmt::Debug, sync::Arc};
2
3use crate::{DekeError, DekeResult, SRobotQ};
4
5pub trait Validator<const N: usize>: Sized + Clone + Debug + Send + Sync + 'static {
6    fn validate<E: Into<DekeError>, A: TryInto<SRobotQ<N>, Error = E>>(
7        &mut self,
8        q: A,
9    ) -> DekeResult<()>;
10    fn validate_motion(&mut self, qs: &[SRobotQ<N>]) -> DekeResult<()>;
11}
12
13#[derive(Debug, Clone)]
14pub struct ValidatorAnd<A, B>(pub A, pub B);
15
16#[derive(Debug, Clone)]
17pub struct ValidatorOr<A, B>(pub A, pub B);
18
19#[derive(Debug, Clone)]
20pub struct ValidatorNot<A>(pub A);
21
22impl<const N: usize, A, B> Validator<N> for ValidatorAnd<A, B>
23where
24    A: Validator<N>,
25    B: Validator<N>,
26{
27    #[inline]
28    fn validate<E: Into<DekeError>, Q: TryInto<SRobotQ<N>, Error = E>>(
29        &mut self,
30        q: Q,
31    ) -> DekeResult<()> {
32        let q = q.try_into().map_err(|e| e.into())?;
33        self.0.validate(q)?;
34        self.1.validate(q)
35    }
36
37    #[inline]
38    fn validate_motion(&mut self, qs: &[SRobotQ<N>]) -> DekeResult<()> {
39        self.0.validate_motion(qs)?;
40        self.1.validate_motion(qs)
41    }
42}
43
44impl<const N: usize, A, B> Validator<N> for ValidatorOr<A, B>
45where
46    A: Validator<N>,
47    B: Validator<N>,
48{
49    #[inline]
50    fn validate<E: Into<DekeError>, Q: TryInto<SRobotQ<N>, Error = E>>(
51        &mut self,
52        q: Q,
53    ) -> DekeResult<()> {
54        let q = q.try_into().map_err(|e| e.into())?;
55        match self.0.validate(q) {
56            Ok(()) => Ok(()),
57            Err(_) => self.1.validate(q),
58        }
59    }
60
61    #[inline]
62    fn validate_motion(&mut self, qs: &[SRobotQ<N>]) -> DekeResult<()> {
63        match self.0.validate_motion(qs) {
64            Ok(()) => Ok(()),
65            Err(_) => self.1.validate_motion(qs),
66        }
67    }
68}
69
70impl<const N: usize, A> Validator<N> for ValidatorNot<A>
71where
72    A: Validator<N>,
73{
74    #[inline]
75    fn validate<E: Into<DekeError>, Q: TryInto<SRobotQ<N>, Error = E>>(
76        &mut self,
77        q: Q,
78    ) -> DekeResult<()> {
79        let q = q.try_into().map_err(|e| e.into())?;
80        match self.0.validate(q) {
81            Ok(()) => Err(DekeError::SuperError),
82            Err(_) => Ok(()),
83        }
84    }
85
86    #[inline]
87    fn validate_motion(&mut self, qs: &[SRobotQ<N>]) -> DekeResult<()> {
88        match self.0.validate_motion(qs) {
89            Ok(()) => Err(DekeError::SuperError),
90            Err(_) => Ok(()),
91        }
92    }
93}
94
95#[derive(Clone)]
96pub struct JointValidator<const N: usize> {
97    lower: SRobotQ<N>,
98    upper: SRobotQ<N>,
99    extras: Option<Arc<[Box<dyn Fn(&SRobotQ<N>) -> bool + Send + Sync>]>>,
100}
101
102impl<const N: usize> Debug for JointValidator<N> {
103    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
104        f.debug_struct("JointValidator")
105            .field("lower", &self.lower)
106            .field("upper", &self.upper)
107            .field(
108                "extras",
109                &format!(
110                    "[{} extra checks]",
111                    self.extras.as_ref().map(|e| e.len()).unwrap_or(0)
112                ),
113            )
114            .finish()
115    }
116}
117
118impl<const N: usize> JointValidator<N> {
119    pub fn new(lower: SRobotQ<N>, upper: SRobotQ<N>) -> Self {
120        Self {
121            lower,
122            upper,
123            extras: None,
124        }
125    }
126
127    pub fn new_with_extras(
128        lower: SRobotQ<N>,
129        upper: SRobotQ<N>,
130        extras: Vec<Box<dyn Fn(&SRobotQ<N>) -> bool + Send + Sync>>,
131    ) -> Self {
132        Self {
133            lower,
134            upper,
135            extras: Some(extras.into()),
136        }
137    }
138}
139
140impl<const N: usize> Validator<N> for JointValidator<N> {
141    #[inline]
142    fn validate<E: Into<DekeError>, Q: TryInto<SRobotQ<N>, Error = E>>(
143        &mut self,
144        q: Q,
145    ) -> DekeResult<()> {
146        let q = q.try_into().map_err(|e| e.into())?;
147        if q.any_lt(&self.lower) || q.any_gt(&self.upper) {
148            return Err(DekeError::ExceedJointLimits);
149        }
150        if let Some(extras) = &self.extras {
151            for check in extras.iter() {
152                if !check(&q) {
153                    return Err(DekeError::ExceedJointLimits);
154                }
155            }
156        }
157        Ok(())
158    }
159
160    #[inline]
161    fn validate_motion(&mut self, qs: &[SRobotQ<N>]) -> DekeResult<()> {
162        for q in qs {
163            self.validate(*q)?;
164        }
165        Ok(())
166    }
167}
deke_types/validator.rs

deke_types/
validator.rs
