repose_kinematics/
task.rs

1//! Copper `CuTask` wrappers for real-time forward kinematics, inverse kinematics, and twist servoing.
2
3use crate::chain::CuKinematicChain;
4use crate::ik::CuDiffIkParams;
5use crate::ik::CuDiffIkStatus;
6use crate::ik::CuIkParams;
7use crate::ik::CuIkStatus;
8use crate::ik::CuTwist6D;
9use crate::joint::CuJointPosition;
10use crate::joint::CuJointVelocity;
11use crate::robots::CuChainSpec;
12use bincode::Decode;
13use bincode::Encode;
14use cu_spatial_payloads::Transform3D;
15use cu29::prelude::*;
16use serde::Deserialize;
17use serde::Serialize;
18
19/// Fixed-DOF joint position payload (`CuMsgPayload`-compliant).
20#[derive(Debug, Clone, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect)]
21#[reflect(no_field_bounds, from_reflect = false)]
22pub struct CuJointPositionsPayload<const N: usize> {
23    #[serde(with = "crate::serde_arrays::position_array")]
24    pub positions: [CuJointPosition; N],
25}
26
27impl<const N: usize> Default for CuJointPositionsPayload<N> {
28    fn default() -> Self {
29        Self {
30            positions: [CuJointPosition::default(); N],
31        }
32    }
33}
34
35/// Health reported by the robot or simulator bridge.
36#[derive(
37    Default, Debug, Clone, Copy, PartialEq, Eq, Encode, Decode, Serialize, Deserialize, Reflect,
38)]
39#[serde(rename_all = "snake_case")]
40pub enum CuRobotStatus {
41    /// The bridge has not confirmed readiness.
42    #[default]
43    NotReady,
44    /// The robot is ready to accept commands.
45    Ready,
46    /// The robot reports a device-specific fault code.
47    Fault(u32),
48    /// An emergency stop is active.
49    EmergencyStop,
50}
51
52/// Measured joint state: the only state payload a bridge has to produce.
53#[derive(Debug, Clone, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect)]
54#[reflect(no_field_bounds, from_reflect = false)]
55pub struct CuJointStatePayload<const N: usize> {
56    /// Current health reported by the bridge.
57    pub status: CuRobotStatus,
58    #[serde(with = "crate::serde_arrays::position_array")]
59    pub positions: [CuJointPosition; N],
60    #[serde(with = "crate::serde_arrays::velocity_array")]
61    pub velocities: [CuJointVelocity; N],
62}
63
64impl<const N: usize> Default for CuJointStatePayload<N> {
65    fn default() -> Self {
66        Self {
67            positions: [CuJointPosition::default(); N],
68            velocities: [CuJointVelocity::default(); N],
69            status: CuRobotStatus::NotReady,
70        }
71    }
72}
73
74/// Fixed-DOF joint velocity command/state payload (`CuMsgPayload`-compliant).
75#[derive(Debug, Clone, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect)]
76#[reflect(no_field_bounds, from_reflect = false)]
77pub struct CuJointVelocitiesPayload<const N: usize> {
78    #[serde(with = "crate::serde_arrays::velocity_array")]
79    pub velocities: [CuJointVelocity; N],
80    pub status: CuDiffIkStatus,
81}
82
83impl<const N: usize> Default for CuJointVelocitiesPayload<N> {
84    fn default() -> Self {
85        Self {
86            velocities: [CuJointVelocity::default(); N],
87            status: CuDiffIkStatus::default(),
88        }
89    }
90}
91
92/// Inverse kinematics solution payload carrying solved joint angles and convergence status.
93#[derive(Debug, Clone, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect)]
94#[reflect(no_field_bounds, from_reflect = false)]
95pub struct CuIkSolutionPayload<const N: usize> {
96    #[serde(with = "crate::serde_arrays::position_array")]
97    pub positions: [CuJointPosition; N],
98    pub status: CuIkStatus,
99}
100
101impl<const N: usize> Default for CuIkSolutionPayload<N> {
102    fn default() -> Self {
103        Self {
104            positions: [CuJointPosition::default(); N],
105            status: CuIkStatus::default(),
106        }
107    }
108}
109
110fn resolve_chain<const N: usize>(
111    spec: Option<&CuChainSpec<N>>,
112    tool_offset: Option<Transform3D<f64>>,
113) -> CuResult<CuKinematicChain<N>> {
114    let spec = spec.ok_or_else(|| CuError::from("Missing required `chain` configuration"))?;
115    let mut chain = spec.resolve()?;
116    if let Some(tool_offset) = tool_offset {
117        chain.set_tool_offset(tool_offset);
118    }
119    chain.validate()?;
120    Ok(chain)
121}
122
123fn read_params<T: Default + serde::de::DeserializeOwned>(
124    cfg: Option<&ComponentConfig>,
125) -> CuResult<T> {
126    Ok(cfg
127        .map(ComponentConfig::deserialize_into::<T>)
128        .transpose()?
129        .unwrap_or_default())
130}
131
132/// Node config of [`CuForwardKinematicsTask`].
133///
134/// ```ron
135/// config: {"chain": "igus_rebel", "tool_offset": /* optional Transform3D */}
136/// ```
137#[derive(Debug, Clone, Serialize, Deserialize, Reflect)]
138#[reflect(no_field_bounds, from_reflect = false)]
139// Field-level defaults: serde does not support container `default` on const-generic structs.
140#[serde(deny_unknown_fields)]
141pub struct CuForwardKinematicsParams<const N: usize> {
142    /// Required robot chain: a preset name (`"igus_rebel"`) or an inline chain.
143    #[serde(default)]
144    #[reflect(ignore)]
145    pub chain: Option<CuChainSpec<N>>,
146    /// Optional flange-to-tool transform replacing the chain's own tool offset.
147    #[serde(default)]
148    #[reflect(ignore)]
149    pub tool_offset: Option<Transform3D<f64>>,
150}
151
152impl<const N: usize> Default for CuForwardKinematicsParams<N> {
153    fn default() -> Self {
154        Self {
155            chain: None,
156            tool_offset: None,
157        }
158    }
159}
160
161impl<const N: usize> CuForwardKinematicsParams<N> {
162    /// Resolves the configured chain (with `tool_offset` applied) and validates it.
163    pub fn resolve_chain(&self) -> CuResult<CuKinematicChain<N>> {
164        resolve_chain(self.chain.as_ref(), self.tool_offset)
165    }
166
167    /// Checks that the chain is present and valid along with every other parameter.
168    pub fn validate(&self) -> CuResult<()> {
169        self.resolve_chain()?;
170        Ok(())
171    }
172}
173
174/// Node config of [`CuInverseKinematicsTask`].
175///
176/// ```ron
177/// config: {"chain": "igus_rebel", "ik": {"max_iterations": 128, "damping": 0.05}}
178/// ```
179#[derive(Debug, Clone, Serialize, Deserialize, Reflect)]
180#[reflect(no_field_bounds, from_reflect = false)]
181// Field-level defaults: serde does not support container `default` on const-generic structs.
182#[serde(deny_unknown_fields)]
183pub struct CuInverseKinematicsParams<const N: usize> {
184    /// Required robot chain: a preset name (`"igus_rebel"`) or an inline chain.
185    #[serde(default)]
186    #[reflect(ignore)]
187    pub chain: Option<CuChainSpec<N>>,
188    /// Optional flange-to-tool transform replacing the chain's own tool offset.
189    #[serde(default)]
190    #[reflect(ignore)]
191    pub tool_offset: Option<Transform3D<f64>>,
192    /// DLS solver parameters.
193    #[serde(default)]
194    pub ik: CuIkParams,
195}
196
197impl<const N: usize> Default for CuInverseKinematicsParams<N> {
198    fn default() -> Self {
199        Self {
200            chain: None,
201            tool_offset: None,
202            ik: CuIkParams::default(),
203        }
204    }
205}
206
207impl<const N: usize> CuInverseKinematicsParams<N> {
208    /// Resolves the configured chain (with `tool_offset` applied) and validates it.
209    pub fn resolve_chain(&self) -> CuResult<CuKinematicChain<N>> {
210        resolve_chain(self.chain.as_ref(), self.tool_offset)
211    }
212
213    /// Checks that the chain is present and valid along with every other parameter.
214    pub fn validate(&self) -> CuResult<()> {
215        self.resolve_chain()?;
216        self.ik.validate()?;
217        Ok(())
218    }
219}
220
221/// Node config of [`CuServoTask`].
222///
223/// ```ron
224/// config: {"chain": "igus_rebel", "diff_ik": {"base_damping": 0.02, "limit_margin": 0.15}}
225/// ```
226#[derive(Debug, Clone, Serialize, Deserialize, Reflect)]
227#[reflect(no_field_bounds, from_reflect = false)]
228// Field-level defaults: serde does not support container `default` on const-generic structs.
229#[serde(deny_unknown_fields)]
230pub struct CuServoParams<const N: usize> {
231    /// Required robot chain: a preset name (`"igus_rebel"`) or an inline chain.
232    #[serde(default)]
233    #[reflect(ignore)]
234    pub chain: Option<CuChainSpec<N>>,
235    /// Optional flange-to-tool transform replacing the chain's own tool offset.
236    #[serde(default)]
237    #[reflect(ignore)]
238    pub tool_offset: Option<Transform3D<f64>>,
239    /// Differential IK parameters.
240    #[serde(default)]
241    pub diff_ik: CuDiffIkParams,
242}
243
244impl<const N: usize> Default for CuServoParams<N> {
245    fn default() -> Self {
246        Self {
247            chain: None,
248            tool_offset: None,
249            diff_ik: CuDiffIkParams::default(),
250        }
251    }
252}
253
254impl<const N: usize> CuServoParams<N> {
255    /// Resolves the configured chain (with `tool_offset` applied) and validates it.
256    pub fn resolve_chain(&self) -> CuResult<CuKinematicChain<N>> {
257        resolve_chain(self.chain.as_ref(), self.tool_offset)
258    }
259
260    /// Checks that the chain is present and valid along with every other parameter.
261    pub fn validate(&self) -> CuResult<()> {
262        self.resolve_chain()?;
263        self.diff_ik.validate()?;
264        Ok(())
265    }
266}
267
268/// Stateless `CuTask` computing end-effector `Transform3D<f64>` from joint angles.
269///
270/// Node config: [`CuForwardKinematicsParams`].
271#[derive(Reflect)]
272#[reflect(no_field_bounds, from_reflect = false)]
273pub struct CuForwardKinematicsTask<const N: usize> {
274    #[reflect(ignore)]
275    chain: CuKinematicChain<N>,
276}
277
278impl<const N: usize> CuForwardKinematicsTask<N> {
279    /// Creates a forward kinematics task from a chain, validating it.
280    pub fn from_params(chain: CuKinematicChain<N>) -> CuResult<Self> {
281        chain.validate()?;
282        Ok(Self { chain })
283    }
284
285    /// Borrows the underlying kinematic chain.
286    pub const fn chain(&self) -> &CuKinematicChain<N> {
287        &self.chain
288    }
289}
290
291impl<const N: usize> Freezable for CuForwardKinematicsTask<N> {}
292
293impl<const N: usize> CuTask for CuForwardKinematicsTask<N> {
294    type Resources<'r> = ();
295    type Input<'m> = input_msg!(CuJointPositionsPayload<N>);
296    type Output<'m> = output_msg!(Transform3D<f64>);
297
298    fn new(cfg: Option<&ComponentConfig>, _res: Self::Resources<'_>) -> CuResult<Self>
299    where
300        Self: Sized,
301    {
302        let params: CuForwardKinematicsParams<N> = read_params(cfg)?;
303        params.validate()?;
304        Self::from_params(params.resolve_chain()?)
305    }
306
307    fn process<'i, 'o>(
308        &mut self,
309        _ctx: &CuContext,
310        input: &Self::Input<'i>,
311        output: &mut Self::Output<'o>,
312    ) -> CuResult<()> {
313        output.tov = input.tov;
314        let Some(joint_msg) = input.payload() else {
315            output.clear_payload();
316            return Ok(());
317        };
318
319        let mut pose = Transform3D::<f64>::identity();
320        self.chain
321            .forward_kinematics(&joint_msg.positions, &mut pose)?;
322        output.set_payload(pose);
323        Ok(())
324    }
325}
326
327/// Stateless `CuTask` solving Damped Least-Squares inverse kinematics from a target pose and seed joints.
328///
329/// Node config: [`CuInverseKinematicsParams`].
330#[derive(Reflect)]
331#[reflect(no_field_bounds, from_reflect = false)]
332pub struct CuInverseKinematicsTask<const N: usize> {
333    #[reflect(ignore)]
334    chain: CuKinematicChain<N>,
335    params: CuIkParams,
336}
337
338impl<const N: usize> CuInverseKinematicsTask<N> {
339    /// Creates an inverse kinematics task from a chain and solver parameters, validating both.
340    pub fn from_params(chain: CuKinematicChain<N>, params: CuIkParams) -> CuResult<Self> {
341        chain.validate()?;
342        params.validate()?;
343        Ok(Self { chain, params })
344    }
345
346    /// Borrows the underlying kinematic chain.
347    pub const fn chain(&self) -> &CuKinematicChain<N> {
348        &self.chain
349    }
350
351    /// Borrows the IK solver parameters.
352    pub const fn params(&self) -> &CuIkParams {
353        &self.params
354    }
355}
356
357impl<const N: usize> Freezable for CuInverseKinematicsTask<N> {}
358
359impl<const N: usize> CuTask for CuInverseKinematicsTask<N> {
360    type Resources<'r> = ();
361    type Input<'m> = input_msg!('m, Transform3D<f64>, CuJointPositionsPayload<N>);
362    type Output<'m> = output_msg!(CuIkSolutionPayload<N>);
363
364    fn new(cfg: Option<&ComponentConfig>, _res: Self::Resources<'_>) -> CuResult<Self>
365    where
366        Self: Sized,
367    {
368        let params: CuInverseKinematicsParams<N> = read_params(cfg)?;
369        params.validate()?;
370        Self::from_params(params.resolve_chain()?, params.ik)
371    }
372
373    fn process<'i, 'o>(
374        &mut self,
375        _ctx: &CuContext,
376        input: &Self::Input<'i>,
377        output: &mut Self::Output<'o>,
378    ) -> CuResult<()> {
379        output.tov = if !matches!(input.0.tov, Tov::None) {
380            input.0.tov
381        } else {
382            input.1.tov
383        };
384
385        let (Some(target_pose), Some(seed_joints)) = (input.0.payload(), input.1.payload()) else {
386            output.clear_payload();
387            return Ok(());
388        };
389
390        let mut positions = [CuJointPosition::default(); N];
391        let status = self.chain.inverse_kinematics_dls(
392            target_pose,
393            &seed_joints.positions,
394            &self.params,
395            &mut positions,
396        )?;
397
398        output.set_payload(CuIkSolutionPayload { positions, status });
399        Ok(())
400    }
401}
402
403/// Real-time Jacobian velocity/twist servoing `CuTask` with singularity and joint-limit damping.
404///
405/// Node config: [`CuServoParams`].
406#[derive(Reflect)]
407#[reflect(no_field_bounds, from_reflect = false)]
408pub struct CuServoTask<const N: usize> {
409    #[reflect(ignore)]
410    chain: CuKinematicChain<N>,
411    params: CuDiffIkParams,
412}
413
414impl<const N: usize> CuServoTask<N> {
415    /// Creates a twist servo task from a chain and differential IK parameters, validating both.
416    pub fn from_params(chain: CuKinematicChain<N>, params: CuDiffIkParams) -> CuResult<Self> {
417        chain.validate()?;
418        params.validate()?;
419        Ok(Self { chain, params })
420    }
421
422    /// Borrows the underlying kinematic chain.
423    pub const fn chain(&self) -> &CuKinematicChain<N> {
424        &self.chain
425    }
426
427    /// Borrows the differential IK parameters.
428    pub const fn params(&self) -> &CuDiffIkParams {
429        &self.params
430    }
431}
432
433impl<const N: usize> Freezable for CuServoTask<N> {}
434
435impl<const N: usize> CuTask for CuServoTask<N> {
436    type Resources<'r> = ();
437    type Input<'m> = input_msg!('m, CuTwist6D, CuJointPositionsPayload<N>);
438    type Output<'m> = output_msg!(CuJointVelocitiesPayload<N>);
439
440    fn new(cfg: Option<&ComponentConfig>, _res: Self::Resources<'_>) -> CuResult<Self>
441    where
442        Self: Sized,
443    {
444        let params: CuServoParams<N> = read_params(cfg)?;
445        params.validate()?;
446        Self::from_params(params.resolve_chain()?, params.diff_ik)
447    }
448
449    fn process<'i, 'o>(
450        &mut self,
451        _ctx: &CuContext,
452        input: &Self::Input<'i>,
453        output: &mut Self::Output<'o>,
454    ) -> CuResult<()> {
455        output.tov = if !matches!(input.0.tov, Tov::None) {
456            input.0.tov
457        } else {
458            input.1.tov
459        };
460
461        let (Some(twist), Some(joints)) = (input.0.payload(), input.1.payload()) else {
462            output.clear_payload();
463            return Ok(());
464        };
465
466        let mut velocities = [CuJointVelocity::default(); N];
467        let status =
468            self.chain
469                .differential_ik(&joints.positions, twist, &self.params, &mut velocities)?;
470
471        output.set_payload(CuJointVelocitiesPayload { velocities, status });
472        Ok(())
473    }
474}
475
476#[cfg(test)]
477mod tests {
478    use super::*;
479
480    #[test]
481    fn serial_task_configuration_is_required_and_dof_checked() {
482        assert!(CuForwardKinematicsTask::<1>::new(None, ()).is_err());
483        assert!(CuInverseKinematicsTask::<1>::new(None, ()).is_err());
484        assert!(CuServoTask::<1>::new(None, ()).is_err());
485        let cfg: ComponentConfig = ron::from_str(r#"#![enable(unwrap_newtypes)]
486{"chain": {"joints": [{"joint_type": "revolute", "origin_translation": [0.0, 0.0, 0.0], "origin_rpy": [0.0, 0.0, 0.0], "axis": "pos_z", "limits": {"revolute": {"lower": -3.0, "upper": 3.0, "max_velocity": 1.0}}}]}}"#).unwrap();
487        let mut task = CuForwardKinematicsTask::<1>::new(Some(&cfg), ()).unwrap();
488        assert!(CuForwardKinematicsTask::<2>::new(Some(&cfg), ()).is_err());
489        assert!(CuInverseKinematicsTask::<1>::new(Some(&cfg), ()).is_ok());
490        assert!(CuServoTask::<1>::new(Some(&cfg), ()).is_ok());
491        let typo: ComponentConfig =
492            ron::from_str(r#"#![enable(unwrap_newtypes)] {"chain": "igus_rebel", "ikk": {}}"#)
493                .unwrap();
494        let err = CuInverseKinematicsTask::<6>::new(Some(&typo), ())
495            .err()
496            .unwrap();
497        assert!(alloc::format!("{err}").contains("ikk"), "{err}");
498        let (ctx, _) = CuContext::new_mock_clock();
499        let input = CuMsg::new(Some(CuJointPositionsPayload::<1>::default()));
500        let mut output = CuMsg::new(None);
501        task.process(&ctx, &input, &mut output).unwrap();
502        assert_eq!(
503            output.payload().unwrap().to_matrix(),
504            Transform3D::<f64>::identity().to_matrix()
505        );
506    }
507}
508
509#[cfg(test)]
510mod preset_tests {
511    use super::*;
512
513    #[test]
514    fn chain_preset_name_resolves_and_checks_dof() {
515        let cfg: ComponentConfig = ron::from_str(
516            r#"#![enable(unwrap_newtypes)] {"chain": "igus_rebel", "ik": {"max_iterations": 10, "position_tolerance": 1e-3}}"#,
517        )
518        .unwrap();
519        let task = CuInverseKinematicsTask::<6>::new(Some(&cfg), ()).unwrap();
520        assert_eq!(task.chain(), &crate::igus_rebel_chain());
521        assert_eq!(task.params().max_iterations, 10);
522        let err = CuInverseKinematicsTask::<5>::new(Some(&cfg), ())
523            .err()
524            .unwrap();
525        assert!(alloc::format!("{err}").contains("N = 5"), "{err}");
526        let unknown: ComponentConfig =
527            ron::from_str(r#"#![enable(unwrap_newtypes)] {"chain": "ur5"}"#).unwrap();
528        let err = CuForwardKinematicsTask::<6>::new(Some(&unknown), ())
529            .err()
530            .unwrap();
531        assert!(alloc::format!("{err}").contains("igus_rebel"), "{err}");
532        let servo: ComponentConfig =
533            ron::from_str(r#"#![enable(unwrap_newtypes)] {"chain": "igus_rebel", "diff_ik": {"base_damping": -1.0}}"#).unwrap();
534        assert!(CuServoTask::<6>::new(Some(&servo), ()).is_err());
535    }
536}
repose_kinematics/task.rs

repose_kinematics/
task.rs
