repose_collision/
scene_exclusions.rs

1//! Stable-ID scene exclusions for attached-object collision pairs.
2//!
3//! Mounting contact between a grasped object and the robot must be permitted:
4//! an attached shape overlaps its mount link by construction. Exclusions name
5//! those pairs with stable [`CuObjectId`]s so they survive scene compaction,
6//! alongside robot shape indices for attached-vs-robot pairs.
7//!
8//! [`CuSceneExclusions`] is a fixed-capacity `CuMsgPayload` published alongside
9//! [`CuPlanningScene`]. Pass it to a check with
10//! [`CuCollisionEnv::with_exclusions`](crate::CuCollisionEnv::with_exclusions); every
11//! check validates it against the environment and rejects invalid references
12//! (unassigned IDs, IDs absent from the snapshot, self-references, out-of-range
13//! robot shape indices) instead of silently skipping them.
14
15use crate::env::CuCollisionEnv;
16use crate::scene::CuObjectId;
17use crate::scene::CuPlanningScene;
18use crate::scene::CuSceneError;
19use bincode::Decode;
20use bincode::Encode;
21use cu29::prelude::*;
22use serde::Deserialize;
23use serde::Serialize;
24
25/// Default capacity for stable-ID exclusions in [`CuSceneExclusions`].
26pub const DEFAULT_MAX_EXCLUSIONS: usize = 16;
27
28/// Partner of an attached shape in a [`CuSceneExclusion`].
29#[derive(Debug, Clone, Copy, PartialEq, Eq, Encode, Decode, Serialize, Deserialize, Reflect)]
30#[serde(rename_all = "snake_case")]
31pub enum CuSceneExclusionPartner {
32    /// A robot link shape from `CuRobotCollisionModel::link_shapes`, by shape index.
33    RobotShape {
34        /// Index into the robot collision model's active shapes.
35        shape_index: u16,
36    },
37    /// Another attached shape in the same scene snapshot, by stable ID.
38    Attached {
39        /// Stable identifier of the partner attached shape.
40        attached_id: CuObjectId,
41    },
42    /// An environment obstacle in the same scene snapshot, by stable ID.
43    Obstacle {
44        /// Stable identifier of the partner obstacle.
45        obstacle_id: CuObjectId,
46    },
47}
48
49/// One excluded attached-shape pair, addressed by stable IDs where the scene owns them.
50///
51/// The subject is always an attached shape; the partner selects robot, attached,
52/// or obstacle geometry. Attached-vs-attached pairs apply symmetrically.
53#[derive(
54    Debug, Clone, Copy, Default, PartialEq, Eq, Encode, Decode, Serialize, Deserialize, Reflect,
55)]
56pub struct CuSceneExclusion {
57    /// Stable identifier of the attached shape this exclusion applies to.
58    pub attached_id: CuObjectId,
59    /// Partner geometry the attached shape must not be checked against.
60    pub partner: CuSceneExclusionPartner,
61}
62
63impl Default for CuSceneExclusionPartner {
64    fn default() -> Self {
65        Self::RobotShape { shape_index: 0 }
66    }
67}
68
69impl CuSceneExclusion {
70    /// Sentinel exclusion with unassigned IDs, used for unused storage slots.
71    pub const NIL: Self = Self {
72        attached_id: CuObjectId::NIL,
73        partner: CuSceneExclusionPartner::RobotShape { shape_index: 0 },
74    };
75
76    /// Excludes attached object `attached_id` against robot shape `shape_index`.
77    pub const fn robot(attached_id: CuObjectId, shape_index: u16) -> Self {
78        Self {
79            attached_id,
80            partner: CuSceneExclusionPartner::RobotShape { shape_index },
81        }
82    }
83
84    /// Excludes attached object `attached_id` against attached object `other_id`.
85    pub const fn attached(attached_id: CuObjectId, other_id: CuObjectId) -> Self {
86        Self {
87            attached_id,
88            partner: CuSceneExclusionPartner::Attached {
89                attached_id: other_id,
90            },
91        }
92    }
93
94    /// Excludes attached object `attached_id` against obstacle `obstacle_id`.
95    pub const fn obstacle(attached_id: CuObjectId, obstacle_id: CuObjectId) -> Self {
96        Self {
97            attached_id,
98            partner: CuSceneExclusionPartner::Obstacle { obstacle_id },
99        }
100    }
101}
102
103/// Fixed-capacity stable-ID exclusion set (`CuMsgPayload`-compliant).
104///
105/// Published alongside a [`CuPlanningScene`] snapshot and validated against it on every check,
106/// so exclusions always bind to the current snapshot.
107#[derive(Debug, Clone, PartialEq, Serialize, Deserialize, Reflect)]
108#[reflect(no_field_bounds, from_reflect = false)]
109pub struct CuSceneExclusions<const MAX_EXCLUSIONS: usize = DEFAULT_MAX_EXCLUSIONS> {
110    #[serde(with = "crate::serde_arrays::exclusion_array")]
111    exclusions: [CuSceneExclusion; MAX_EXCLUSIONS],
112    len: u32,
113}
114
115impl<const MAX_EXCLUSIONS: usize> Default for CuSceneExclusions<MAX_EXCLUSIONS> {
116    fn default() -> Self {
117        Self::new()
118    }
119}
120
121impl<const MAX_EXCLUSIONS: usize> Encode for CuSceneExclusions<MAX_EXCLUSIONS> {
122    fn encode<E: bincode::enc::Encoder>(
123        &self,
124        encoder: &mut E,
125    ) -> Result<(), bincode::error::EncodeError> {
126        Encode::encode(&self.len, encoder)?;
127        Encode::encode(&self.exclusions, encoder)
128    }
129}
130
131impl<C, const MAX_EXCLUSIONS: usize> Decode<C> for CuSceneExclusions<MAX_EXCLUSIONS> {
132    fn decode<D: bincode::de::Decoder<Context = C>>(
133        decoder: &mut D,
134    ) -> Result<Self, bincode::error::DecodeError> {
135        let len: u32 = Decode::decode(decoder)?;
136        let exclusions = Decode::decode(decoder)?;
137        if len as usize > MAX_EXCLUSIONS {
138            return Err(bincode::error::DecodeError::Other(
139                "Invalid scene exclusions snapshot: count exceeds capacity",
140            ));
141        }
142        Ok(Self { exclusions, len })
143    }
144}
145
146impl<'de, C, const MAX_EXCLUSIONS: usize> bincode::BorrowDecode<'de, C>
147    for CuSceneExclusions<MAX_EXCLUSIONS>
148{
149    fn borrow_decode<D: bincode::de::BorrowDecoder<'de, Context = C>>(
150        decoder: &mut D,
151    ) -> Result<Self, bincode::error::DecodeError> {
152        Decode::decode(decoder)
153    }
154}
155
156impl<const MAX_EXCLUSIONS: usize> CuSceneExclusions<MAX_EXCLUSIONS> {
157    /// Creates an empty exclusion set.
158    pub const fn new() -> Self {
159        Self {
160            exclusions: [CuSceneExclusion::NIL; MAX_EXCLUSIONS],
161            len: 0,
162        }
163    }
164
165    /// Returns the number of active exclusions.
166    pub const fn len(&self) -> usize {
167        self.len as usize
168    }
169
170    /// Returns `true` if no exclusions are active.
171    pub const fn is_empty(&self) -> bool {
172        self.len == 0
173    }
174
175    /// Returns the maximum number of exclusions.
176    pub const fn capacity(&self) -> usize {
177        MAX_EXCLUSIONS
178    }
179
180    /// Borrows the active exclusions.
181    pub fn as_slice(&self) -> &[CuSceneExclusion] {
182        &self.exclusions[..self.len as usize]
183    }
184
185    /// Appends an exclusion if capacity permits.
186    pub fn add(&mut self, exclusion: CuSceneExclusion) -> CuResult<()> {
187        let idx = self.len as usize;
188        if idx >= MAX_EXCLUSIONS {
189            return Err(CuError::from(
190                "repose_collision: scene exclusion capacity exceeded",
191            ));
192        }
193        self.exclusions[idx] = exclusion;
194        self.len += 1;
195        Ok(())
196    }
197
198    /// Removes all exclusions in place.
199    pub fn clear(&mut self) {
200        self.len = 0;
201    }
202
203    /// Checks every entry against `scene` and a robot model with `robot_shape_count` shapes.
204    ///
205    /// Unassigned or unknown IDs, self-references, and out-of-range robot shape indices fail.
206    /// Checks perform the same validation; this lets a caller reject a snapshot up front.
207    pub fn validate<const MAX_OBS: usize, const MAX_ATT: usize>(
208        &self,
209        scene: &CuPlanningScene<MAX_OBS, MAX_ATT>,
210        robot_shape_count: usize,
211    ) -> CuResult<()> {
212        validate_exclusions(self.as_slice(), &scene.collision_env(), robot_shape_count)
213    }
214}
215
216/// Validates `exclusions` against the IDs present in `env`.
217pub(crate) fn validate_exclusions(
218    exclusions: &[CuSceneExclusion],
219    env: &CuCollisionEnv<'_>,
220    robot_shape_count: usize,
221) -> CuResult<()> {
222    for entry in exclusions {
223        if !entry.attached_id.is_valid() {
224            return Err(CuSceneError::InvalidObjectId(entry.attached_id).into());
225        }
226        if !env.find_attached(entry.attached_id) {
227            return Err(CuSceneError::ObjectNotFound(entry.attached_id).into());
228        }
229        match entry.partner {
230            CuSceneExclusionPartner::RobotShape { shape_index } => {
231                if shape_index as usize >= robot_shape_count {
232                    return Err(CuError::from(
233                        "repose_collision: scene exclusion robot shape index out of range",
234                    ));
235                }
236            }
237            CuSceneExclusionPartner::Attached { attached_id } => {
238                if !attached_id.is_valid() {
239                    return Err(CuSceneError::InvalidObjectId(attached_id).into());
240                }
241                if attached_id == entry.attached_id {
242                    return Err(CuError::from(
243                        "repose_collision: scene exclusion references its own attached shape",
244                    ));
245                }
246                if !env.find_attached(attached_id) {
247                    return Err(CuSceneError::ObjectNotFound(attached_id).into());
248                }
249            }
250            CuSceneExclusionPartner::Obstacle { obstacle_id } => {
251                if !obstacle_id.is_valid() {
252                    return Err(CuSceneError::InvalidObjectId(obstacle_id).into());
253                }
254                if !env.find_obstacle(obstacle_id) {
255                    return Err(CuSceneError::ObjectNotFound(obstacle_id).into());
256                }
257            }
258        }
259    }
260    Ok(())
261}
repose_collision/scene_exclusions.rs

repose_collision/
scene_exclusions.rs
