repose_collision/
task.rs

1//! Copper `CuTask` for real-time self-collision, environment, and attached-shape collision checking.
2
3use crate::env::CuCollisionEnv;
4use crate::model::CuCollisionReport;
5use crate::model::CuRobotCollisionModel;
6use crate::robots::CuCollisionModelSpec;
7use crate::scene::CuPlanningScene;
8use crate::scene::DEFAULT_MAX_ATTACHED;
9use crate::scene::DEFAULT_MAX_OBSTACLES;
10use crate::scene_exclusions::CuSceneExclusions;
11use crate::scene_exclusions::DEFAULT_MAX_EXCLUSIONS;
12use cu29::prelude::*;
13use repose_kinematics::CuChainSpec;
14use repose_kinematics::CuJointPosition;
15use repose_kinematics::CuJointPositionsPayload;
16use repose_kinematics::CuKinematicChain;
17use serde::Deserialize;
18use serde::Serialize;
19
20/// Default contact capacity of [`CuCollisionCheckTask`].
21pub const DEFAULT_MAX_CONTACTS: usize = 16;
22
23/// Node config of [`CuCollisionCheckTask`].
24///
25/// ```ron
26/// config: {"chain": "igus_rebel", "collision_model": "igus_rebel"}
27/// ```
28///
29/// Both keys are required. `chain` is a [`CuChainSpec`] and `collision_model` a
30/// [`CuCollisionModelSpec`] (preset name or inline model).
31#[derive(Debug, Clone, Serialize, Deserialize, Reflect)]
32#[reflect(no_field_bounds, from_reflect = false)]
33// Field-level defaults: serde does not support container `default` on const-generic structs.
34#[serde(deny_unknown_fields)]
35pub struct CuCollisionCheckParams<const N: usize, const M: usize = N> {
36    /// Required robot chain: a preset name (`"igus_rebel"`) or an inline chain.
37    #[serde(default)]
38    #[reflect(ignore)]
39    pub chain: Option<CuChainSpec<N>>,
40    /// Required collision model: a preset name (`"igus_rebel"`) or an inline model.
41    #[serde(default)]
42    #[reflect(ignore)]
43    pub collision_model: Option<CuCollisionModelSpec<M>>,
44}
45
46impl<const N: usize, const M: usize> Default for CuCollisionCheckParams<N, M> {
47    fn default() -> Self {
48        Self {
49            chain: None,
50            collision_model: None,
51        }
52    }
53}
54
55impl<const N: usize, const M: usize> CuCollisionCheckParams<N, M> {
56    /// Resolves and validates the configured chain.
57    pub fn resolve_chain(&self) -> CuResult<CuKinematicChain<N>> {
58        self.chain
59            .as_ref()
60            .ok_or_else(|| CuError::from("Missing required `chain` configuration"))?
61            .resolve()
62    }
63
64    /// Resolves and validates the configured collision model.
65    pub fn resolve_collision_model(&self) -> CuResult<CuRobotCollisionModel<N, M>> {
66        self.collision_model
67            .as_ref()
68            .ok_or_else(|| CuError::from("Missing required `collision_model` configuration"))?
69            .resolve()
70    }
71
72    /// Checks that both the chain and the collision model are present and valid.
73    pub fn validate(&self) -> CuResult<()> {
74        self.resolve_chain()?;
75        self.resolve_collision_model()?;
76        Ok(())
77    }
78}
79
80/// Stateless `CuTask` evaluating self-collision, environment obstacles, and attached shapes in place.
81///
82/// Node config: [`CuCollisionCheckParams`]. Inputs: joint positions, planning scene, exclusions.
83#[derive(Reflect)]
84#[reflect(no_field_bounds, from_reflect = false)]
85pub struct CuCollisionCheckTask<
86    const N: usize,
87    const M: usize = N,
88    const MAX_OBS: usize = DEFAULT_MAX_OBSTACLES,
89    const MAX_ATTACHED: usize = DEFAULT_MAX_ATTACHED,
90    const MAX_CONTACTS: usize = DEFAULT_MAX_CONTACTS,
91    const MAX_EXCL: usize = DEFAULT_MAX_EXCLUSIONS,
92> {
93    #[reflect(ignore)]
94    chain: CuKinematicChain<N>,
95    #[reflect(ignore)]
96    model: CuRobotCollisionModel<N, M>,
97}
98
99impl<
100    const N: usize,
101    const M: usize,
102    const MAX_OBS: usize,
103    const MAX_ATTACHED: usize,
104    const MAX_CONTACTS: usize,
105    const MAX_EXCL: usize,
106> CuCollisionCheckTask<N, M, MAX_OBS, MAX_ATTACHED, MAX_CONTACTS, MAX_EXCL>
107{
108    /// Creates a collision check task from a kinematic chain and robot collision model.
109    pub fn from_params(
110        chain: CuKinematicChain<N>,
111        model: CuRobotCollisionModel<N, M>,
112    ) -> CuResult<Self> {
113        chain.validate()?;
114        Ok(Self { chain, model })
115    }
116
117    /// Borrows the kinematic chain.
118    pub const fn chain(&self) -> &CuKinematicChain<N> {
119        &self.chain
120    }
121
122    /// Borrows the robot collision model.
123    pub const fn model(&self) -> &CuRobotCollisionModel<N, M> {
124        &self.model
125    }
126
127    /// Checks self-collision, environment obstacles, and attached shapes for configuration `q`
128    /// against an optional planning scene snapshot, populating `out_report` in place.
129    ///
130    /// Stable-ID `exclusions` are validated against `scene`; invalid references, or non-empty
131    /// exclusions without a scene, fail the check.
132    pub fn check_configuration(
133        &self,
134        q: &[CuJointPosition; N],
135        scene: Option<&CuPlanningScene<MAX_OBS, MAX_ATTACHED>>,
136        exclusions: Option<&CuSceneExclusions<MAX_EXCL>>,
137        out_report: &mut CuCollisionReport<MAX_CONTACTS>,
138    ) -> CuResult<()> {
139        let env = match (scene, exclusions) {
140            (Some(scene), Some(exclusions)) => scene.collision_env().with_exclusions(exclusions),
141            (Some(scene), None) => scene.collision_env(),
142            (None, Some(exclusions)) if !exclusions.is_empty() => {
143                return Err(CuError::from(
144                    "repose_collision: scene exclusions require a planning scene snapshot",
145                ));
146            }
147            (None, _) => CuCollisionEnv::EMPTY,
148        };
149        self.model
150            .check_configuration(&self.chain, q, &env, out_report)
151    }
152}
153
154impl<
155    const N: usize,
156    const M: usize,
157    const MAX_OBS: usize,
158    const MAX_ATTACHED: usize,
159    const MAX_CONTACTS: usize,
160    const MAX_EXCL: usize,
161> Freezable for CuCollisionCheckTask<N, M, MAX_OBS, MAX_ATTACHED, MAX_CONTACTS, MAX_EXCL>
162{
163}
164
165impl<
166    const N: usize,
167    const M: usize,
168    const MAX_OBS: usize,
169    const MAX_ATTACHED: usize,
170    const MAX_CONTACTS: usize,
171    const MAX_EXCL: usize,
172> CuTask for CuCollisionCheckTask<N, M, MAX_OBS, MAX_ATTACHED, MAX_CONTACTS, MAX_EXCL>
173{
174    type Resources<'r> = ();
175    type Input<'m> = input_msg!(
176        'm,
177        CuJointPositionsPayload<N>,
178        CuPlanningScene<MAX_OBS, MAX_ATTACHED>,
179        CuSceneExclusions<MAX_EXCL>
180    );
181    type Output<'m> = output_msg!(CuCollisionReport<MAX_CONTACTS>);
182
183    fn new(cfg: Option<&ComponentConfig>, _res: Self::Resources<'_>) -> CuResult<Self>
184    where
185        Self: Sized,
186    {
187        let params: CuCollisionCheckParams<N, M> = cfg
188            .map(ComponentConfig::deserialize_into::<CuCollisionCheckParams<N, M>>)
189            .transpose()?
190            .unwrap_or_default();
191        params.validate()?;
192        Self::from_params(params.resolve_chain()?, params.resolve_collision_model()?)
193    }
194
195    fn process<'i, 'o>(
196        &mut self,
197        _ctx: &CuContext,
198        input: &Self::Input<'i>,
199        output: &mut Self::Output<'o>,
200    ) -> CuResult<()> {
201        output.tov = if !matches!(input.0.tov, Tov::None) {
202            input.0.tov
203        } else if !matches!(input.1.tov, Tov::None) {
204            input.1.tov
205        } else {
206            input.2.tov
207        };
208
209        let Some(joints) = input.0.payload() else {
210            output.clear_payload();
211            return Ok(());
212        };
213
214        let mut report = CuCollisionReport::<MAX_CONTACTS>::default();
215        self.check_configuration(
216            &joints.positions,
217            input.1.payload(),
218            input.2.payload(),
219            &mut report,
220        )?;
221        output.set_payload(report);
222        Ok(())
223    }
224}
225
226#[cfg(test)]
227mod tests {
228    use super::*;
229    use cu29::units::si::length::meter;
230
231    const ONE_JOINT_CHAIN: &str = r#"{"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}}}]}"#;
232
233    fn config(body: &str) -> ComponentConfig {
234        ron::from_str(&alloc::format!("#![enable(unwrap_newtypes)]\n{{{body}}}")).unwrap()
235    }
236
237    #[test]
238    fn serial_collision_task_requires_matching_explicit_models() {
239        assert!(CuCollisionCheckTask::<1>::new(None, ()).is_err());
240        let chain_only = config(&alloc::format!(r#""chain": {ONE_JOINT_CHAIN}"#));
241        assert!(CuCollisionCheckTask::<1>::new(Some(&chain_only), ()).is_err());
242        let model = r#"{"shapes": [{"shape": {"sphere": {"center": [0.0, 0.0, 0.0], "radius": 0.1}}, "frame": 0}], "safety_margin": 0.01}"#;
243        let cfg = config(&alloc::format!(
244            r#""chain": {ONE_JOINT_CHAIN}, "collision_model": {model}"#
245        ));
246        let task = CuCollisionCheckTask::<1>::new(Some(&cfg), ()).unwrap();
247        assert_eq!(task.model().link_shapes().len(), 1);
248        assert_eq!(task.model().safety_margin().get::<meter>(), 0.01);
249        assert!(
250            cfg.deserialize_into::<CuCollisionCheckParams<1, 2>>()
251                .is_err()
252        );
253        // The old second spelling of the margin is rejected instead of silently ignored.
254        let typo = config(&alloc::format!(
255            r#""chain": {ONE_JOINT_CHAIN}, "collision_model": {model}, "safety_margin_m": 0.02"#
256        ));
257        assert!(CuCollisionCheckTask::<1>::new(Some(&typo), ()).is_err());
258    }
259
260    #[test]
261    fn collision_task_accepts_presets() {
262        let cfg = config(r#""chain": "igus_rebel", "collision_model": "igus_rebel""#);
263        let task = CuCollisionCheckTask::<6>::new(Some(&cfg), ()).unwrap();
264        assert_eq!(task.model(), &crate::igus_rebel_collision_model());
265        assert!(CuCollisionCheckTask::<6, 4>::new(Some(&cfg), ()).is_err());
266    }
267}
repose_collision/task.rs

repose_collision/
task.rs
