repose_collision/
lib.rs

1//! Deterministic, allocation-free capsule and sphere robot collision checking for `copper-rs`.
2//!
3//! - [`CuRobotCollisionModel`]: robot link shapes ([`CuLinkShape`]) on joint frames, built with
4//!   [`CuRobotCollisionModel::new`] and [`CuCollisionModelParams`], or from RON via
5//!   [`CuCollisionModelSpec`].
6//! - [`CuCollisionEnv`]: borrowed view of obstacles, attached shapes, and exclusions that every
7//!   check takes; [`CuPlanningScene::collision_env`] provides one without copying.
8//! - [`CuCollisionCheckTask`]: the `CuTask` wrapper, configured by [`CuCollisionCheckParams`].
9//!
10//! # Example
11//! ```rust
12//! use repose_collision::{CuCollisionEnv, CuCollisionReport, CuCollisionShape, CuSphere};
13//! use repose_collision::igus_rebel_collision_model;
14//! use repose_kinematics::igus_rebel_chain;
15//! use cu29::units::si::angle::radian;
16//! use cu29::units::si::f64::{Angle, Length};
17//! use cu29::units::si::length::meter;
18//!
19//! let chain = igus_rebel_chain();
20//! let model = igus_rebel_collision_model();
21//! let q_home = [Angle::new::<radian>(0.0).into(); 6];
22//! let mut report = CuCollisionReport::<8>::default();
23//! model.check_configuration(&chain, &q_home, &CuCollisionEnv::EMPTY, &mut report).unwrap();
24//! assert!(report.is_collision_free());
25//!
26//! let m = Length::new::<meter>;
27//! let obstacles = [CuCollisionShape::Sphere(CuSphere::new([m(0.0), m(0.0), m(0.87)], m(0.08)).unwrap())];
28//! model.check_configuration(&chain, &q_home, &CuCollisionEnv::new(&obstacles), &mut report).unwrap();
29//! assert!(!report.is_collision_free());
30//! ```
31
32#![cfg_attr(not(feature = "std"), no_std)]
33
34extern crate alloc;
35
36mod env;
37mod model;
38mod robots;
39mod scene;
40mod scene_exclusions;
41mod serde_arrays;
42mod shapes;
43mod task;
44
45pub use env::CuCollisionEnv;
46pub use model::CuCollisionContact;
47pub use model::CuCollisionModelParams;
48pub use model::CuCollisionReport;
49pub use model::CuContactKind;
50pub use model::CuRobotCollisionModel;
51pub use robots::CuCollisionModelSpec;
52pub use robots::igus_rebel_collision_model;
53pub use scene::CuAttachedShape;
54pub use scene::CuAttachedUpdate;
55pub use scene::CuObjectId;
56pub use scene::CuPlanningScene;
57pub use scene::CuSceneError;
58pub use scene::CuSceneObstacle;
59pub use scene::CuSceneRevision;
60pub use scene::DEFAULT_MAX_ATTACHED;
61pub use scene::DEFAULT_MAX_OBSTACLES;
62pub use scene_exclusions::CuSceneExclusion;
63pub use scene_exclusions::CuSceneExclusionPartner;
64pub use scene_exclusions::CuSceneExclusions;
65pub use scene_exclusions::DEFAULT_MAX_EXCLUSIONS;
66pub use shapes::CuCapsule;
67pub use shapes::CuCollisionShape;
68pub use shapes::CuLinkShape;
69pub use shapes::CuSphere;
70pub use shapes::capsule_capsule_signed_distance;
71pub use shapes::capsule_sphere_signed_distance;
72pub use shapes::shape_signed_distance;
73pub use shapes::shapes_violate_margin;
74pub use shapes::sphere_sphere_signed_distance;
75pub use shapes::transform_shape;
76pub use task::CuCollisionCheckParams;
77pub use task::CuCollisionCheckTask;
78pub use task::DEFAULT_MAX_CONTACTS;
79
80#[cfg(test)]
81mod tests {
82    use crate::CuAttachedShape;
83    use crate::CuAttachedUpdate;
84    use crate::CuCollisionCheckTask;
85    use crate::CuCollisionEnv;
86    use crate::CuCollisionModelParams;
87    use crate::CuCollisionModelSpec;
88    use crate::CuCollisionReport;
89    use crate::CuCollisionShape;
90    use crate::CuContactKind;
91    use crate::CuLinkShape;
92    use crate::CuObjectId;
93    use crate::CuPlanningScene;
94    use crate::CuRobotCollisionModel;
95    use crate::CuSceneExclusion;
96    use crate::CuSceneExclusions;
97    use crate::CuSphere;
98    use crate::capsule_capsule_signed_distance;
99    use crate::capsule_sphere_signed_distance;
100    use crate::igus_rebel_collision_model;
101    use crate::shapes::capsule_m;
102    use crate::shapes::sphere_m;
103    use crate::sphere_sphere_signed_distance;
104    use cu29::prelude::*;
105    use cu29::units::si::angle::radian;
106    use cu29::units::si::f64::Angle as Angle64;
107    use cu29::units::si::f64::Length as Length64;
108    use cu29::units::si::length::meter;
109    use repose_kinematics::CuJointPositionsPayload;
110    use repose_kinematics::CuRobotPreset;
111    use repose_kinematics::igus_rebel_chain;
112
113    fn sphere(center: [f64; 3], radius: f64) -> CuCollisionShape {
114        CuCollisionShape::Sphere(sphere_m(center, radius))
115    }
116
117    fn params(margin_m: f64, skip_adjacent_links: bool) -> CuCollisionModelParams {
118        CuCollisionModelParams {
119            safety_margin: Length64::new::<meter>(margin_m),
120            skip_adjacent_links,
121        }
122    }
123
124    #[test]
125    fn attached_collision_model_snapshot_preserves_geometry_and_pairs() {
126        let shape = sphere([0.0; 3], 0.1);
127        let mut model = CuRobotCollisionModel::<2, 3>::new(
128            &[
129                CuLinkShape::new(shape, None),
130                CuLinkShape::new(shape, Some(0)),
131                CuLinkShape::new(shape, Some(1)),
132            ],
133            params(0.005, false),
134        )
135        .unwrap();
136        model.set_pair_enabled(0, 2, false).unwrap();
137        let mut buffer = [0_u8; 2048];
138        let len =
139            bincode::encode_into_slice(model, &mut buffer, bincode::config::standard()).unwrap();
140        let (restored, consumed): (CuRobotCollisionModel<2, 3>, usize) =
141            bincode::decode_from_slice(&buffer[..len], bincode::config::standard()).unwrap();
142        assert_eq!(consumed, len);
143        assert_eq!(restored, model);
144    }
145
146    #[test]
147    fn model_constructor_validates_and_skips_pairs_uniformly() {
148        let shape = sphere([0.0; 3], 0.1);
149        let link = |frame| CuLinkShape::new(shape, frame);
150        let shapes = [link(Some(0)), link(Some(1)), link(Some(2)), link(Some(2))];
151        let model = CuRobotCollisionModel::<3, 4>::new(&shapes, params(0.0, true)).unwrap();
152        assert!(!model.is_pair_enabled(0, 1)); // adjacent
153        assert!(model.is_pair_enabled(0, 2));
154        assert!(!model.is_pair_enabled(2, 3)); // same rigid body
155        let model = CuRobotCollisionModel::<3, 4>::new(&shapes, params(0.0, false)).unwrap();
156        assert!(model.is_pair_enabled(0, 1));
157        assert!(!model.is_pair_enabled(2, 3));
158        // Fewer shapes than capacity.
159        let model = CuRobotCollisionModel::<3, 4>::new(&shapes[..2], params(0.0, false)).unwrap();
160        assert_eq!(model.shape_count(), 2);
161        // Invalid inputs.
162        assert!(CuRobotCollisionModel::<3, 1>::new(&shapes, params(0.0, true)).is_err());
163        assert!(CuRobotCollisionModel::<2, 4>::new(&shapes, params(0.0, true)).is_err());
164        assert!(CuRobotCollisionModel::<3, 4>::new(&shapes, params(-1.0, true)).is_err());
165        assert!(
166            CuRobotCollisionModel::<3, 4>::new(
167                &[
168                    link(None),
169                    CuLinkShape::new(sphere([f64::NAN; 3], 0.1), None)
170                ],
171                params(0.0, true)
172            )
173            .is_err()
174        );
175    }
176
177    #[test]
178    fn collision_model_spec_resolves_presets_and_inline_models() {
179        let preset: CuCollisionModelSpec<6> = CuRobotPreset::IgusRebel.into();
180        assert_eq!(preset.resolve::<6>().unwrap(), igus_rebel_collision_model());
181        assert!(preset.resolve::<5>().is_err());
182        fn parse<const M: usize>(text: &str) -> CuResult<CuCollisionModelSpec<M>> {
183            // Same path as a node config.
184            let cfg: ComponentConfig = ron::from_str(&alloc::format!(
185                "#![enable(unwrap_newtypes)]\n{{\"collision_model\": {text}}}"
186            ))
187            .map_err(|e| CuError::from(alloc::format!("{e}")))?;
188            let params: crate::CuCollisionCheckParams<1, M> = cfg.deserialize_into()?;
189            params
190                .collision_model
191                .ok_or_else(|| CuError::from("missing"))
192        }
193        let spec: CuCollisionModelSpec<2> = parse(
194            r#"{"shapes": [{"shape": {"sphere": {"center": [0.0, 0.0, 0.0], "radius": 0.1}}, "frame": 0}, {"shape": {"capsule": {"a": [0.0, 0.0, 0.0], "b": [0.0, 0.0, 1.0], "radius": 0.1}}}], "skip_adjacent_links": false}"#,
195        )
196        .unwrap();
197        let model = spec.resolve::<1>().unwrap();
198        assert_eq!(model.link_shapes()[1].frame, None);
199        assert_eq!(model.safety_margin().get::<meter>(), 0.0);
200        assert!(parse::<2>(r#"{"link_shapes": [], "shapes": []}"#).is_err());
201        let spec: CuCollisionModelSpec<6> = parse(r#""igus_rebel""#).unwrap();
202        assert_eq!(spec, preset);
203    }
204
205    #[test]
206    fn test_sphere_and_capsule_signed_distances() {
207        let s1 = sphere_m([0.0, 0.0, 0.0], 0.1);
208        let s2 = sphere_m([0.3, 0.0, 0.0], 0.05);
209        let d_ss = sphere_sphere_signed_distance(&s1, &s2).get::<meter>();
210        assert!((d_ss - 0.15).abs() < 1e-12);
211
212        let c1 = capsule_m([0.0, 0.0, 0.0], [0.0, 0.0, 1.0], 0.1);
213        let s3 = sphere_m([0.25, 0.0, 0.5], 0.05);
214        let d_cs = capsule_sphere_signed_distance(&c1, &s3).get::<meter>();
215        assert!((d_cs - 0.10).abs() < 1e-12);
216
217        // Crossing capsules (perpendicular skew segments at z=0.5, separated by 0.2 in X)
218        let c2 = capsule_m([0.2, -0.5, 0.5], [0.2, 0.5, 0.5], 0.12);
219        let d_cc = capsule_capsule_signed_distance(&c1, &c2).get::<meter>();
220        // Segment distance is 0.2, sum of radii is 0.22 -> penetration of -0.02 m
221        assert!((d_cc - (-0.02)).abs() < 1e-12);
222
223        let m = Length64::new::<meter>;
224        assert!(CuSphere::new([m(0.0); 3], m(-0.1)).is_err());
225        assert!(CuSphere::new([m(f64::INFINITY), m(0.0), m(0.0)], m(0.1)).is_err());
226        assert_eq!(
227            CuSphere::new([m(0.0); 3], m(0.1)).unwrap(),
228            sphere_m([0.0; 3], 0.1)
229        );
230    }
231
232    #[test]
233    fn test_igus_rebel_home_is_collision_free_and_folded_self_collides() {
234        let chain = igus_rebel_chain();
235        let model = igus_rebel_collision_model();
236
237        let q_home = [Angle64::new::<radian>(0.0).into(); 6];
238        let mut report = CuCollisionReport::<16>::default();
239        model
240            .check_configuration(&chain, &q_home, &CuCollisionEnv::EMPTY, &mut report)
241            .unwrap();
242        assert!(report.is_collision_free());
243        assert!(report.min_signed_distance().get::<meter>() > 0.01);
244
245        // Fold upper arm and forearm sharply back into the shoulder/base column
246        let q_folded = [
247            Angle64::new::<radian>(0.0).into(),
248            Angle64::new::<radian>(1.25).into(),
249            Angle64::new::<radian>(2.35).into(),
250            Angle64::new::<radian>(0.0).into(),
251            Angle64::new::<radian>(1.4).into(),
252            Angle64::new::<radian>(0.0).into(),
253        ];
254        model
255            .check_configuration(&chain, &q_folded, &CuCollisionEnv::EMPTY, &mut report)
256            .unwrap();
257        assert!(!report.is_collision_free());
258        assert_eq!(report.as_slice()[0].kind, CuContactKind::SelfCollision);
259    }
260
261    #[test]
262    fn test_environment_collision_and_segment_interpolation() {
263        let chain = igus_rebel_chain();
264        let model = igus_rebel_collision_model();
265
266        let q_start = [Angle64::new::<radian>(0.0).into(); 6];
267        let q_end = [
268            Angle64::new::<radian>(0.8).into(),
269            Angle64::new::<radian>(0.5).into(),
270            Angle64::new::<radian>(-0.4).into(),
271            Angle64::new::<radian>(0.0).into(),
272            Angle64::new::<radian>(0.0).into(),
273            Angle64::new::<radian>(0.0).into(),
274        ];
275
276        assert!(
277            model
278                .is_segment_collision_free(
279                    &chain,
280                    &q_start,
281                    &q_end,
282                    Angle64::new::<radian>(0.05),
283                    &CuCollisionEnv::EMPTY,
284                )
285                .unwrap()
286        );
287
288        // Place an obstacle sphere right on the end-effector at q_home (z ≈ 0.87 m)
289        let obstacles = [sphere([0.0, 0.0, 0.87], 0.08)];
290        let env = CuCollisionEnv::new(&obstacles);
291
292        assert!(
293            !model
294                .is_configuration_collision_free(&chain, &q_start, &env)
295                .unwrap()
296        );
297        assert!(
298            !model
299                .is_segment_collision_free(
300                    &chain,
301                    &q_start,
302                    &q_end,
303                    Angle64::new::<radian>(0.05),
304                    &env,
305                )
306                .unwrap()
307        );
308    }
309
310    #[test]
311    fn attached_shapes_and_exclusions_from_scene() {
312        let chain = igus_rebel_chain();
313        let model = igus_rebel_collision_model();
314        let q = [Angle64::new::<radian>(0.0).into(); 6];
315        let mut scene = CuPlanningScene::<4, 4>::new();
316        // Gripper overlapping the flange shape (robot shape 5) on frame 5.
317        let gripper = scene
318            .add_attached(Some(5), sphere([0.0, 0.0, 0.05], 0.04), None)
319            .unwrap();
320        let mut report = CuCollisionReport::<8>::default();
321        model
322            .check_configuration(&chain, &q, &scene.collision_env(), &mut report)
323            .unwrap();
324        assert!(!report.is_collision_free());
325        assert_eq!(report.as_slice()[0].link_index, 6);
326        assert!(report.as_slice().iter().any(|c| c.partner_index == 5));
327
328        let mut exclusions = CuSceneExclusions::<4>::new();
329        exclusions.add(CuSceneExclusion::robot(gripper, 4)).unwrap();
330        exclusions.add(CuSceneExclusion::robot(gripper, 5)).unwrap();
331        exclusions.validate(&scene, model.shape_count()).unwrap();
332        let env = scene.collision_env().with_exclusions(&exclusions);
333        assert!(
334            model
335                .is_configuration_collision_free(&chain, &q, &env)
336                .unwrap()
337        );
338
339        // Unknown IDs fail and leave the report untouched.
340        let mut bad = CuSceneExclusions::<1>::new();
341        bad.add(CuSceneExclusion::robot(CuObjectId::new(99), 0))
342            .unwrap();
343        let before = report.clone();
344        let env = scene.collision_env().with_exclusions(&bad);
345        assert!(
346            model
347                .check_configuration(&chain, &q, &env, &mut report)
348                .is_err()
349        );
350        assert_eq!(report, before);
351
352        // Same-frame attached shapes are one rigid body; attach/detach keep IDs.
353        let box_id = scene
354            .add_obstacle(sphere([0.5, 0.0, 0.0], 0.05), None)
355            .unwrap();
356        scene
357            .attach(box_id, Some(5), sphere([0.0, 0.0, 0.05], 0.04))
358            .unwrap();
359        assert_eq!(scene.attached_shapes().len(), 2);
360        let env = scene.collision_env().with_exclusions(&exclusions);
361        model
362            .check_configuration(&chain, &q, &env, &mut report)
363            .unwrap();
364        // Only the box (attached index 1) against robot shapes; never box vs. gripper.
365        assert!(!report.is_collision_free());
366        assert!(
367            report
368                .as_slice()
369                .iter()
370                .all(|c| c.link_index == 7 && c.partner_index < 6)
371        );
372        scene
373            .update_attached(box_id, CuAttachedUpdate::new().with_frame(Some(4)))
374            .unwrap();
375        assert_eq!(scene.attached_shape(box_id).unwrap().frame, Some(4));
376        scene.detach(box_id, sphere([0.5, 0.0, 0.0], 0.05)).unwrap();
377        assert_eq!(scene.obstacle_ids(), &[box_id]);
378        let attached = CuAttachedShape::new(gripper, Some(5), sphere([0.0; 3], 0.01)).unwrap();
379        assert_eq!(attached.frame, Some(5));
380    }
381
382    #[test]
383    fn test_collision_check_task() {
384        let (ctx, _mock) = CuContext::new_mock_clock();
385        let mut task = CuCollisionCheckTask::<6, 6, 8, 8, 8>::from_params(
386            igus_rebel_chain(),
387            igus_rebel_collision_model(),
388        )
389        .unwrap();
390
391        let joints_msg = CuMsg::new(Some(CuJointPositionsPayload {
392            positions: [Angle64::new::<radian>(0.0).into(); 6],
393        }));
394        let scene_msg = CuMsg::new(Some(CuPlanningScene::<8, 8>::new()));
395        let exclusions_msg = CuMsg::new(Some(CuSceneExclusions::new()));
396        let mut out_msg = CuMsg::new(None);
397
398        task.process(
399            &ctx,
400            &(&joints_msg, &scene_msg, &exclusions_msg),
401            &mut out_msg,
402        )
403        .unwrap();
404        let report = out_msg.payload().unwrap();
405        assert!(report.is_collision_free());
406    }
407}
repose_collision/lib.rs

repose_collision/
lib.rs
