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 joint_axes_positions(
109        &self,
110        q: &SRobotQ<N, f32>,
111    ) -> Result<([AVec3<f32>; N], [AVec3<f32>; N], AVec3<f32>), Self::Error> {
112        self.f32_chain.joint_axes_positions(q)
113    }
114
115    fn jacobian(&self, q: &SRobotQ<N, f32>) -> Result<[[f32; N]; 6], Self::Error> {
116        self.f32_chain.jacobian(q)
117    }
118
119    fn jacobian_dot(
120        &self,
121        q: &SRobotQ<N, f32>,
122        qdot: &SRobotQ<N, f32>,
123    ) -> Result<[[f32; N]; 6], Self::Error> {
124        self.f32_chain.jacobian_dot(q, qdot)
125    }
126
127    fn jacobian_ddot(
128        &self,
129        q: &SRobotQ<N, f32>,
130        qdot: &SRobotQ<N, f32>,
131        qddot: &SRobotQ<N, f32>,
132    ) -> Result<[[f32; N]; 6], Self::Error> {
133        self.f32_chain.jacobian_ddot(q, qdot, qddot)
134    }
135}
136
137impl<const N: usize, FK32, FK64> FKChain<N, f64> for FPDispatch<N, FK32, FK64>
138where
139    FK32: FKChain<N, f32>,
140    FK64: FKChain<N, f64>,
141{
142    type Error = FK64::Error;
143
144    fn base_tf(&self) -> AAffine3<f64> {
145        self.f64_chain.base_tf()
146    }
147
148    fn max_reach(&self) -> Result<f64, Self::Error> {
149        self.f64_chain.max_reach()
150    }
151
152    fn fk(&self, q: &SRobotQ<N, f64>) -> Result<[AAffine3<f64>; N], Self::Error> {
153        self.f64_chain.fk(q)
154    }
155
156    fn fk_end(&self, q: &SRobotQ<N, f64>) -> Result<AAffine3<f64>, Self::Error> {
157        self.f64_chain.fk_end(q)
158    }
159
160    fn joint_axes_positions(
161        &self,
162        q: &SRobotQ<N, f64>,
163    ) -> Result<([AVec3<f64>; N], [AVec3<f64>; N], AVec3<f64>), Self::Error> {
164        self.f64_chain.joint_axes_positions(q)
165    }
166
167    fn jacobian(&self, q: &SRobotQ<N, f64>) -> Result<[[f64; N]; 6], Self::Error> {
168        self.f64_chain.jacobian(q)
169    }
170
171    fn jacobian_dot(
172        &self,
173        q: &SRobotQ<N, f64>,
174        qdot: &SRobotQ<N, f64>,
175    ) -> Result<[[f64; N]; 6], Self::Error> {
176        self.f64_chain.jacobian_dot(q, qdot)
177    }
178
179    fn jacobian_ddot(
180        &self,
181        q: &SRobotQ<N, f64>,
182        qdot: &SRobotQ<N, f64>,
183        qddot: &SRobotQ<N, f64>,
184    ) -> Result<[[f64; N]; 6], Self::Error> {
185        self.f64_chain.jacobian_ddot(q, qdot, qddot)
186    }
187}
188
189#[cfg(test)]
190mod tests {
191    use super::*;
192    use crate::{DHChain, DHJoint};
193    use glam_traits_ext::{TAffine3, TVec3};
194
195    #[test]
196    fn dispatch_routes_to_correct_precision() {
197        const F32_CHAIN: DHChain<2, f32> = DHChain::<2, f32>::new([
198            DHJoint { a: 1.0, alpha: 0.0, d: 0.0, theta_offset: 0.0 },
199            DHJoint { a: 1.0, alpha: 0.0, d: 0.0, theta_offset: 0.0 },
200        ]);
201        const F64_CHAIN: DHChain<2, f64> = DHChain::<2, f64>::new_f64([
202            DHJoint { a: 1.0, alpha: 0.0, d: 0.0, theta_offset: 0.0 },
203            DHJoint { a: 1.0, alpha: 0.0, d: 0.0, theta_offset: 0.0 },
204        ]);
205
206        let dispatch = FPDispatch::new(F32_CHAIN, F64_CHAIN);
207
208        let q32 = SRobotQ::<2, f32>::from_array([0.5, -0.3]);
209        let q64 = SRobotQ::<2, f64>::from_array([0.5, -0.3]);
210
211        let end32 = <FPDispatch<_, _, _> as FKChain<2, f32>>::fk_end(&dispatch, &q32)
212            .unwrap()
213            .translation();
214        let end64 = <FPDispatch<_, _, _> as FKChain<2, f64>>::fk_end(&dispatch, &q64)
215            .unwrap()
216            .translation();
217
218        assert!((end32.x() as f64 - end64.x()).abs() < 1e-5);
219        assert!((end32.y() as f64 - end64.y()).abs() < 1e-5);
220    }
221
222    #[test]
223    fn from_f64_derives_f32_chain() {
224        const F64_CHAIN: DHChain<2, f64> = DHChain::<2, f64>::new_f64([
225            DHJoint { a: 1.0, alpha: 0.0, d: 0.0, theta_offset: 0.0 },
226            DHJoint { a: 1.0, alpha: 0.0, d: 0.0, theta_offset: 0.0 },
227        ]);
228
229        let dispatch: FPDispatch<2, DHChain<2, f32>, DHChain<2, f64>> =
230            FPDispatch::from_f64(F64_CHAIN);
231
232        let q32 = SRobotQ::<2, f32>::from_array([0.5, -0.3]);
233        let q64 = SRobotQ::<2, f64>::from_array([0.5, -0.3]);
234        let end32 = <FPDispatch<_, _, _> as FKChain<2, f32>>::fk_end(&dispatch, &q32)
235            .unwrap()
236            .translation();
237        let end64 = <FPDispatch<_, _, _> as FKChain<2, f64>>::fk_end(&dispatch, &q64)
238            .unwrap()
239            .translation();
240        assert!((end32.x() as f64 - end64.x()).abs() < 1e-4);
241        assert!((end32.y() as f64 - end64.y()).abs() < 1e-4);
242    }
243}
deke_types/fk/fp_dispatch.rs

deke_types/fk/
fp_dispatch.rs
