repose_collision/
env.rs

1//! Borrowed, zero-copy view of everything a robot is checked against.
2
3use crate::scene::CuAttachedShape;
4use crate::scene::CuObjectId;
5use crate::scene_exclusions::CuSceneExclusion;
6use crate::scene_exclusions::CuSceneExclusionPartner;
7use crate::scene_exclusions::CuSceneExclusions;
8use crate::shapes::CuCollisionShape;
9
10/// Borrowed view of the collision environment: world obstacles, attached shapes, and exclusions.
11///
12/// The view copies nothing. Build it from raw obstacle shapes with [`new`](Self::new), or from a
13/// planning scene with [`CuPlanningScene::collision_env`](crate::CuPlanningScene::collision_env)
14/// (which also carries obstacle IDs, so obstacle exclusions resolve). [`EMPTY`](Self::EMPTY)
15/// checks the robot against itself only.
16///
17/// # Attached shapes
18///
19/// Attached shapes are transformed by their frame (`None` = base frame) and checked against each
20/// other, the robot's own shapes, and the obstacles:
21///
22/// - attached vs. robot and attached vs. attached report [`CuContactKind::SelfCollision`];
23/// - attached vs. obstacle reports [`CuContactKind::EnvironmentCollision`].
24///
25/// Attached shapes on the same frame (including two `None`) are one rigid body and are never
26/// checked against each other. Attached-vs-robot pairs are never skipped automatically; exclude
27/// mounting contact explicitly with [`CuSceneExclusions`].
28///
29/// # Contact index encoding
30///
31/// For attached-shape contacts `link_index` is `robot_shape_count + attached_index`, so it never
32/// collides with robot shape indices `0..robot_shape_count`. `partner_index` is the robot shape
33/// index, `robot_shape_count + attached_index` for attached partners, or the obstacle index.
34///
35/// # Exclusions
36///
37/// Exclusions are validated at the start of every check: unassigned IDs, IDs absent from the view,
38/// self-references, and out-of-range robot shape indices fail the check and leave the report
39/// unmodified.
40///
41/// [`CuContactKind::SelfCollision`]: crate::CuContactKind::SelfCollision
42/// [`CuContactKind::EnvironmentCollision`]: crate::CuContactKind::EnvironmentCollision
43#[derive(Debug, Clone, Copy, Default)]
44pub struct CuCollisionEnv<'a> {
45    obstacles: &'a [CuCollisionShape],
46    /// Either empty (anonymous obstacles) or parallel to `obstacles`.
47    obstacle_ids: &'a [CuObjectId],
48    attached: &'a [CuAttachedShape],
49    exclusions: &'a [CuSceneExclusion],
50}
51
52impl<'a> CuCollisionEnv<'a> {
53    /// No obstacles, attached shapes, or exclusions: self-collision only.
54    pub const EMPTY: CuCollisionEnv<'static> = CuCollisionEnv {
55        obstacles: &[],
56        obstacle_ids: &[],
57        attached: &[],
58        exclusions: &[],
59    };
60
61    /// Environment of anonymous world-frame obstacles.
62    pub const fn new(obstacles: &'a [CuCollisionShape]) -> Self {
63        Self {
64            obstacles,
65            obstacle_ids: &[],
66            attached: &[],
67            exclusions: &[],
68        }
69    }
70
71    /// Obstacles with stable IDs; `ids` must be parallel to `obstacles`.
72    pub(crate) const fn with_obstacle_ids(
73        obstacles: &'a [CuCollisionShape],
74        obstacle_ids: &'a [CuObjectId],
75    ) -> Self {
76        Self {
77            obstacles,
78            obstacle_ids,
79            attached: &[],
80            exclusions: &[],
81        }
82    }
83
84    /// Adds shapes attached to robot frames (or the base frame).
85    pub const fn with_attached(mut self, attached: &'a [CuAttachedShape]) -> Self {
86        self.attached = attached;
87        self
88    }
89
90    /// Adds stable-ID pair exclusions for the attached shapes.
91    pub fn with_exclusions<const MAX_EXCLUSIONS: usize>(
92        mut self,
93        exclusions: &'a CuSceneExclusions<MAX_EXCLUSIONS>,
94    ) -> Self {
95        self.exclusions = exclusions.as_slice();
96        self
97    }
98
99    /// World-frame obstacle shapes.
100    pub const fn obstacles(&self) -> &'a [CuCollisionShape] {
101        self.obstacles
102    }
103
104    /// Attached shapes.
105    pub const fn attached(&self) -> &'a [CuAttachedShape] {
106        self.attached
107    }
108
109    /// Stable-ID exclusions.
110    pub const fn exclusions(&self) -> &'a [CuSceneExclusion] {
111        self.exclusions
112    }
113
114    pub(crate) fn obstacle_id(&self, index: usize) -> Option<CuObjectId> {
115        self.obstacle_ids.get(index).copied()
116    }
117
118    pub(crate) fn find_attached(&self, id: CuObjectId) -> bool {
119        self.attached.iter().any(|a| a.id == id)
120    }
121
122    pub(crate) fn find_obstacle(&self, id: CuObjectId) -> bool {
123        self.obstacle_ids.contains(&id)
124    }
125
126    /// `true` if the pair (attached `attached_id`, `partner`) is excluded, in either order for
127    /// attached-vs-attached pairs.
128    pub(crate) fn is_excluded(
129        &self,
130        attached_id: CuObjectId,
131        partner: CuSceneExclusionPartner,
132    ) -> bool {
133        self.exclusions.iter().any(|e| {
134            (e.attached_id == attached_id && e.partner == partner)
135                || matches!(partner, CuSceneExclusionPartner::Attached { attached_id: other }
136                    if e.attached_id == other
137                        && e.partner == CuSceneExclusionPartner::Attached { attached_id })
138        })
139    }
140}
repose_collision/env.rs

repose_collision/
env.rs
