deke_types/fk/
fp_dispatch.rs

1use crate::SRobotQ;
2
3use super::{AAffine3, AVec3, FKChain};
4
5/// FK chain wrapper that holds both an `f32` and an `f64` representation of
6/// the same kinematic chain and dispatches to the correct precision when an
7/// `FKChain<N, F>` method is invoked.
8///
9/// The two inner chains are expected to describe the same robot — typically
10/// the `f64` is canonical and the `f32` is derived from it (see
11/// [`FPDispatch::from_f64`]). Callers that already have both built can use
12/// [`FPDispatch::new`].
13///
14/// Use this when the same robot needs to be consumed by stages with different
15/// precision requirements: e.g. an RRT planner runs on `f32` for SIMD speed,
16/// while a TOPP retimer runs on `f64` for solver stability. A single
17/// `FPDispatch` can be passed to both — each picks up the precision-correct
18/// `FKChain` impl via type inference.
19///
20/// ```
21/// use deke_types::{DHChain, DHJoint, FKChain, FPDispatch, SRobotQ};
22///
23/// // Author the robot once in f64 (e.g. from a const URDF table).
24/// const ARM: DHChain<2, f64> = DHChain::<2, f64>::new_f64([
25///     DHJoint { a: 1.0, alpha: 0.0, d: 0.0, theta_offset: 0.0 },
26///     DHJoint { a: 1.0, alpha: 0.0, d: 0.0, theta_offset: 0.0 },
27/// ]);
28///
29/// // Derive the f32 chain via the cheap From cast and bundle them.
30/// let dispatch: FPDispatch<2, DHChain<2, f32>, DHChain<2, f64>> =
31///     FPDispatch::from_f64(ARM);
32///
33/// // f32 consumers get the SIMD path:
34/// let q32 = SRobotQ::<2, f32>::from_array([0.5, -0.3]);
35/// let _ = <FPDispatch<_, _, _> as FKChain<2, f32>>::fk_end(&dispatch, &q32).unwrap();
36///
37/// // f64 consumers get the precise path:
38/// let q64 = SRobotQ::<2, f64>::from_array([0.5, -0.3]);
39/// let _ = <FPDispatch<_, _, _> as FKChain<2, f64>>::fk_end(&dispatch, &q64).unwrap();
40/// ```
41#[derive(Debug, Clone)]
42pub struct FPDispatch<const N: usize, FK32, FK64>
43where
44    FK32: FKChain<N, f32>,
45    FK64: FKChain<N, f64>,
46{
47    f32_chain: FK32,
48    f64_chain: FK64,
49}
50
51impl<const N: usize, FK32, FK64> FPDispatch<N, FK32, FK64>
52where
53    FK32: FKChain<N, f32>,
54    FK64: FKChain<N, f64>,
55{
56    /// Build from explicit f32 and f64 chains. The two are expected to encode
57    /// the same robot; nothing checks that here.
58    pub fn new(f32_chain: FK32, f64_chain: FK64) -> Self {
59        Self { f32_chain, f64_chain }
60    }
61
62    pub fn f32_chain(&self) -> &FK32 {
63        &self.f32_chain
64    }
65
66    pub fn f64_chain(&self) -> &FK64 {
67        &self.f64_chain
68    }
69}
70
71impl<const N: usize, FK32, FK64> FPDispatch<N, FK32, FK64>
72where
73    FK32: FKChain<N, f32> + From<FK64>,
74    FK64: FKChain<N, f64> + Clone,
75{
76    /// Build from only the f64 chain by deriving the f32 chain via `From`.
77    /// Available when the f32 chain implements `From<FK64>` (which it does
78    /// for the leaf chain types `DHChain`, `HPChain`, and `URDFChain`).
79    pub fn from_f64(f64_chain: FK64) -> Self {
80        let f32_chain = FK32::from(f64_chain.clone());
81        Self { f32_chain, f64_chain }
82    }
83}
84
85impl<const N: usize, FK32, FK64> FKChain<N, f32> for FPDispatch<N, FK32, FK64>
86where
87    FK32: FKChain<N, f32>,
88    FK64: FKChain<N, f64>,
89{
90    type Error = FK32::Error;
91
92    fn base_tf(&self) -> AAffine3<f32> {
93        self.f32_chain.base_tf()
94    }
95
96    fn max_reach(&self) -> Result<f32, Self::Error> {
97        self.f32_chain.max_reach()
98    }
99
100    fn fk(&self, q: &SRobotQ<N, f32>) -> Result<[AAffine3<f32>; N], Self::Error> {
101        self.f32_chain.fk(q)
102    }
103
104    fn fk_end(&self, q: &SRobotQ<N, f32>) -> Result<AAffine3<f32>, Self::Error> {
105        self.f32_chain.fk_end(q)
106    }
107
108    fn all_fk(
109        &self,
110        q: &SRobotQ<N, f32>,
111    ) -> Result<(AAffine3<f32>, [AAffine3<f32>; N], AAffine3<f32>), Self::Error> {
112        self.f32_chain.all_fk(q)
113    }
114
115    fn joint_axes_positions(
116        &self,
117        q: &SRobotQ<N, f32>,
118    ) -> Result<([AVec3<f32>; N], [AVec3<f32>; N], AVec3<f32>), Self::Error> {
119        self.f32_chain.joint_axes_positions(q)
120    }
121
122    fn jacobian(&self, q: &SRobotQ<N, f32>) -> Result<[[f32; N]; 6], Self::Error> {
123        self.f32_chain.jacobian(q)
124    }
125
126    fn jacobian_dot(
127        &self,
128        q: &SRobotQ<N, f32>,
129        qdot: &SRobotQ<N, f32>,
130    ) -> Result<[[f32; N]; 6], Self::Error> {
131        self.f32_chain.jacobian_dot(q, qdot)
132    }
133
134    fn jacobian_ddot(
135        &self,
136        q: &SRobotQ<N, f32>,
137        qdot: &SRobotQ<N, f32>,
138        qddot: &SRobotQ<N, f32>,
139    ) -> Result<[[f32; N]; 6], Self::Error> {
140        self.f32_chain.jacobian_ddot(q, qdot, qddot)
141    }
142}
143
144impl<const N: usize, FK32, FK64> FKChain<N, f64> for FPDispatch<N, FK32, FK64>
145where
146    FK32: FKChain<N, f32>,
147    FK64: FKChain<N, f64>,
148{
149    type Error = FK64::Error;
150
151    fn base_tf(&self) -> AAffine3<f64> {
152        self.f64_chain.base_tf()
153    }
154
155    fn max_reach(&self) -> Result<f64, Self::Error> {
156        self.f64_chain.max_reach()
157    }
158
159    fn fk(&self, q: &SRobotQ<N, f64>) -> Result<[AAffine3<f64>; N], Self::Error> {
160        self.f64_chain.fk(q)
161    }
162
163    fn fk_end(&self, q: &SRobotQ<N, f64>) -> Result<AAffine3<f64>, Self::Error> {
164        self.f64_chain.fk_end(q)
165    }
166
167    fn all_fk(
168        &self,
169        q: &SRobotQ<N, f64>,
170    ) -> Result<(AAffine3<f64>, [AAffine3<f64>; N], AAffine3<f64>), Self::Error> {
171        self.f64_chain.all_fk(q)
172    }
173
174    fn joint_axes_positions(
175        &self,
176        q: &SRobotQ<N, f64>,
177    ) -> Result<([AVec3<f64>; N], [AVec3<f64>; N], AVec3<f64>), Self::Error> {
178        self.f64_chain.joint_axes_positions(q)
179    }
180
181    fn jacobian(&self, q: &SRobotQ<N, f64>) -> Result<[[f64; N]; 6], Self::Error> {
182        self.f64_chain.jacobian(q)
183    }
184
185    fn jacobian_dot(
186        &self,
187        q: &SRobotQ<N, f64>,
188        qdot: &SRobotQ<N, f64>,
189    ) -> Result<[[f64; N]; 6], Self::Error> {
190        self.f64_chain.jacobian_dot(q, qdot)
191    }
192
193    fn jacobian_ddot(
194        &self,
195        q: &SRobotQ<N, f64>,
196        qdot: &SRobotQ<N, f64>,
197        qddot: &SRobotQ<N, f64>,
198    ) -> Result<[[f64; N]; 6], Self::Error> {
199        self.f64_chain.jacobian_ddot(q, qdot, qddot)
200    }
201}
202
203#[cfg(test)]
204mod tests {
205    use super::*;
206    use crate::{DHChain, DHJoint};
207    use glam_traits_ext::{TAffine3, TVec3};
208
209    #[test]
210    fn dispatch_routes_to_correct_precision() {
211        const F32_CHAIN: DHChain<2, f32> = DHChain::<2, f32>::new([
212            DHJoint { a: 1.0, alpha: 0.0, d: 0.0, theta_offset: 0.0 },
213            DHJoint { a: 1.0, alpha: 0.0, d: 0.0, theta_offset: 0.0 },
214        ]);
215        const F64_CHAIN: DHChain<2, f64> = DHChain::<2, f64>::new_f64([
216            DHJoint { a: 1.0, alpha: 0.0, d: 0.0, theta_offset: 0.0 },
217            DHJoint { a: 1.0, alpha: 0.0, d: 0.0, theta_offset: 0.0 },
218        ]);
219
220        let dispatch = FPDispatch::new(F32_CHAIN, F64_CHAIN);
221
222        let q32 = SRobotQ::<2, f32>::from_array([0.5, -0.3]);
223        let q64 = SRobotQ::<2, f64>::from_array([0.5, -0.3]);
224
225        let end32 = <FPDispatch<_, _, _> as FKChain<2, f32>>::fk_end(&dispatch, &q32)
226            .unwrap()
227            .translation();
228        let end64 = <FPDispatch<_, _, _> as FKChain<2, f64>>::fk_end(&dispatch, &q64)
229            .unwrap()
230            .translation();
231
232        assert!((end32.x() as f64 - end64.x()).abs() < 1e-5);
233        assert!((end32.y() as f64 - end64.y()).abs() < 1e-5);
234    }
235
236    #[test]
237    fn from_f64_derives_f32_chain() {
238        const F64_CHAIN: DHChain<2, f64> = DHChain::<2, f64>::new_f64([
239            DHJoint { a: 1.0, alpha: 0.0, d: 0.0, theta_offset: 0.0 },
240            DHJoint { a: 1.0, alpha: 0.0, d: 0.0, theta_offset: 0.0 },
241        ]);
242
243        let dispatch: FPDispatch<2, DHChain<2, f32>, DHChain<2, f64>> =
244            FPDispatch::from_f64(F64_CHAIN);
245
246        let q32 = SRobotQ::<2, f32>::from_array([0.5, -0.3]);
247        let q64 = SRobotQ::<2, f64>::from_array([0.5, -0.3]);
248        let end32 = <FPDispatch<_, _, _> as FKChain<2, f32>>::fk_end(&dispatch, &q32)
249            .unwrap()
250            .translation();
251        let end64 = <FPDispatch<_, _, _> as FKChain<2, f64>>::fk_end(&dispatch, &q64)
252            .unwrap()
253            .translation();
254        assert!((end32.x() as f64 - end64.x()).abs() < 1e-4);
255        assert!((end32.y() as f64 - end64.y()).abs() < 1e-4);
256    }
257}
deke_types/fk/fp_dispatch.rs

deke_types/fk/
fp_dispatch.rs
