1use glam::Affine3A;
2
3use crate::{DHChain, DHJoint, HPChain, HPJoint, URDFChain, URDFJoint};
4use crate::{FKChain, DekeError, DekeResult, SRobotQ};
5
6macro_rules! dynamic_fk {
7 ($name:ident, $chain:ident, $joint:ident) => {
8 #[derive(Debug, Clone)]
9 pub enum $name {
10 J1($chain<1>),
11 J2($chain<2>),
12 J3($chain<3>),
13 J4($chain<4>),
14 J5($chain<5>),
15 J6($chain<6>),
16 J7($chain<7>),
17 J8($chain<8>),
18 }
19
20 impl $name {
21 pub fn try_new(joints: Vec<$joint>) -> DekeResult<Self> {
22 Ok(match joints.len() {
23 1 => Self::J1($chain::new(joints.try_into().ok().unwrap())),
24 2 => Self::J2($chain::new(joints.try_into().ok().unwrap())),
25 3 => Self::J3($chain::new(joints.try_into().ok().unwrap())),
26 4 => Self::J4($chain::new(joints.try_into().ok().unwrap())),
27 5 => Self::J5($chain::new(joints.try_into().ok().unwrap())),
28 6 => Self::J6($chain::new(joints.try_into().ok().unwrap())),
29 7 => Self::J7($chain::new(joints.try_into().ok().unwrap())),
30 8 => Self::J8($chain::new(joints.try_into().ok().unwrap())),
31 n => return Err(DekeError::ShapeMismatch { expected: 8, found: n }),
32 })
33 }
34
35 pub fn dof(&self) -> usize {
36 match self {
37 Self::J1(_) => 1,
38 Self::J2(_) => 2,
39 Self::J3(_) => 3,
40 Self::J4(_) => 4,
41 Self::J5(_) => 5,
42 Self::J6(_) => 6,
43 Self::J7(_) => 7,
44 Self::J8(_) => 8,
45 }
46 }
47
48 pub fn fk_dyn(&self, q: &[f32]) -> DekeResult<Vec<Affine3A>> {
49 dynamic_fk!(@dispatch_fk self, q,
50 J1 1, J2 2, J3 3, J4 4, J5 5, J6 6, J7 7, J8 8
51 )
52 }
53
54 pub fn fk_end_dyn(&self, q: &[f32]) -> DekeResult<Affine3A> {
55 dynamic_fk!(@dispatch_fk_end self, q,
56 J1 1, J2 2, J3 3, J4 4, J5 5, J6 6, J7 7, J8 8
57 )
58 }
59 }
60
61 dynamic_fk!(@impl_fkchain $name, $chain, 1 J1, 2 J2, 3 J3, 4 J4, 5 J5, 6 J6, 7 J7, 8 J8);
62 };
63
64 (@dispatch_fk $self:ident, $q:ident, $($variant:ident $n:literal),+) => {
65 match $self {
66 $(Self::$variant(chain) => {
67 let arr: &[f32; $n] = $q.try_into().map_err(|_| DekeError::ShapeMismatch {
68 expected: $n,
69 found: $q.len(),
70 })?;
71 Ok(FKChain::<$n>::fk(chain, &SRobotQ(*arr)).map_err(|e| -> DekeError { e.into() })?.to_vec())
72 }),+
73 }
74 };
75
76 (@dispatch_fk_end $self:ident, $q:ident, $($variant:ident $n:literal),+) => {
77 match $self {
78 $(Self::$variant(chain) => {
79 let arr: &[f32; $n] = $q.try_into().map_err(|_| DekeError::ShapeMismatch {
80 expected: $n,
81 found: $q.len(),
82 })?;
83 FKChain::<$n>::fk_end(chain, &SRobotQ(*arr)).map_err(|e| -> DekeError { e.into() })
84 }),+
85 }
86 };
87
88 (@impl_fkchain $name:ident, $chain:ident, $($n:literal $variant:ident),+) => {
89 $(
90 impl FKChain<$n> for $name {
91 type Error = DekeError;
92
93 fn fk(&self, q: &SRobotQ<$n>) -> Result<[Affine3A; $n], Self::Error> {
94 match self {
95 Self::$variant(chain) => FKChain::<$n>::fk(chain, q).map_err(Into::into),
96 _ => Err(DekeError::ShapeMismatch {
97 expected: self.dof(),
98 found: $n,
99 }),
100 }
101 }
102
103 fn fk_end(&self, q: &SRobotQ<$n>) -> Result<Affine3A, Self::Error> {
104 match self {
105 Self::$variant(chain) => FKChain::<$n>::fk_end(chain, q).map_err(Into::into),
106 _ => Err(DekeError::ShapeMismatch {
107 expected: self.dof(),
108 found: $n,
109 }),
110 }
111 }
112 }
113
114 impl From<$chain<$n>> for $name {
115 fn from(chain: $chain<$n>) -> Self {
116 Self::$variant(chain)
117 }
118 }
119 )+
120 };
121}
122
123dynamic_fk!(DynamicDHChain, DHChain, DHJoint);
124dynamic_fk!(DynamicHPChain, HPChain, HPJoint);
125dynamic_fk!(DynamicURDFChain, URDFChain, URDFJoint);
126
127impl DynamicDHChain {
128 pub fn from_chain(chain: impl Into<Self>) -> Self { chain.into() }
129}
130
131impl DynamicHPChain {
132 pub fn from_chain(chain: impl Into<Self>) -> Self { chain.into() }
133}
134
135impl DynamicURDFChain {
136 pub fn from_chain(chain: impl Into<Self>) -> Self { chain.into() }
137}
138
139trait ErasedFK<const N: usize>: Send + Sync {
140 fn fk(&self, q: &SRobotQ<N>) -> Result<[Affine3A; N], DekeError>;
141 fn fk_end(&self, q: &SRobotQ<N>) -> Result<Affine3A, DekeError>;
142 fn clone_box(&self) -> Box<dyn ErasedFK<N>>;
143}
144
145impl<const N: usize, FK: FKChain<N> + 'static> ErasedFK<N> for FK {
146 fn fk(&self, q: &SRobotQ<N>) -> Result<[Affine3A; N], DekeError> {
147 FKChain::fk(self, q).map_err(Into::into)
148 }
149
150 fn fk_end(&self, q: &SRobotQ<N>) -> Result<Affine3A, DekeError> {
151 FKChain::fk_end(self, q).map_err(Into::into)
152 }
153
154 fn clone_box(&self) -> Box<dyn ErasedFK<N>> {
155 Box::new(self.clone())
156 }
157}
158
159pub struct BoxFK<const N: usize>(Box<dyn ErasedFK<N>>);
160
161impl<const N: usize> BoxFK<N> {
162 pub fn new(fk: impl FKChain<N> + 'static) -> Self {
163 Self(Box::new(fk))
164 }
165}
166
167impl<const N: usize> Clone for BoxFK<N> {
168 fn clone(&self) -> Self {
169 Self(self.0.clone_box())
170 }
171}
172
173impl<const N: usize> FKChain<N> for BoxFK<N> {
174 type Error = DekeError;
175
176 fn fk(&self, q: &SRobotQ<N>) -> Result<[Affine3A; N], DekeError> {
177 self.0.fk(q)
178 }
179
180 fn fk_end(&self, q: &SRobotQ<N>) -> Result<Affine3A, DekeError> {
181 self.0.fk_end(q)
182 }
183}