repose_collision/
model.rs

1//! Robot link collision models, their parameters, and in-place collision reports.
2
3use crate::env::CuCollisionEnv;
4use crate::scene_exclusions::CuSceneExclusionPartner as Partner;
5use crate::scene_exclusions::validate_exclusions;
6use crate::shapes::CuCollisionShape;
7use crate::shapes::CuLinkShape;
8use crate::shapes::shape_signed_distance;
9use crate::shapes::transform_shape;
10use CuContactKind::EnvironmentCollision;
11use CuContactKind::SelfCollision;
12use bincode::Decode;
13use bincode::Encode;
14use cu_spatial_payloads::Transform3D;
15use cu29::prelude::*;
16use cu29::units::si::f64::Length as Length64;
17use cu29::units::si::length::meter;
18use repose_kinematics::CuJointPosition;
19use repose_kinematics::CuJointResolution;
20use repose_kinematics::CuKinematicChain;
21use serde::Deserialize;
22use serde::Serialize;
23
24/// Category of a detected collision or safety-margin violation.
25#[derive(
26    Debug, Clone, Copy, Default, PartialEq, Eq, Encode, Decode, Serialize, Deserialize, Reflect,
27)]
28#[serde(rename_all = "snake_case")]
29pub enum CuContactKind {
30    #[default]
31    SelfCollision,
32    EnvironmentCollision,
33}
34
35/// Single contact or clearance violation between a robot link and another link or obstacle.
36#[derive(
37    Default, Debug, Clone, Copy, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect,
38)]
39pub struct CuCollisionContact {
40    pub kind: CuContactKind,
41    /// Robot collision-shape index.
42    pub link_index: u16,
43    /// Partner shape index for self-collision, or obstacle index for environment collision.
44    pub partner_index: u16,
45    /// Surface-to-surface signed distance (negative indicates penetration).
46    pub signed_distance: Length64,
47}
48
49/// Fixed-capacity collision report populated in place (`CuMsgPayload`-compliant).
50#[derive(Debug, Clone, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect)]
51#[reflect(no_field_bounds, from_reflect = false)]
52pub struct CuCollisionReport<const MAX_CONTACTS: usize> {
53    #[serde(with = "crate::serde_arrays::contact_array")]
54    contacts: [CuCollisionContact; MAX_CONTACTS],
55    /// Number of valid entries in `contacts`.
56    len: u32,
57    /// Total detected contacts, including those exceeding storage capacity.
58    total_contacts: u32,
59    /// Minimum surface-to-surface signed distance observed across all checked pairs.
60    min_signed_distance: Length64,
61}
62
63impl<const MAX_CONTACTS: usize> Default for CuCollisionReport<MAX_CONTACTS> {
64    fn default() -> Self {
65        Self {
66            contacts: [CuCollisionContact::default(); MAX_CONTACTS],
67            len: 0,
68            total_contacts: 0,
69            min_signed_distance: Length64::new::<meter>(f64::INFINITY),
70        }
71    }
72}
73
74impl<const MAX_CONTACTS: usize> CuCollisionReport<MAX_CONTACTS> {
75    /// Resets the collision report in place for reuse.
76    pub fn clear(&mut self) {
77        self.len = 0;
78        self.total_contacts = 0;
79        self.min_signed_distance = Length64::new::<meter>(f64::INFINITY);
80    }
81
82    /// Returns `true` if no self-collisions or environment collisions were recorded.
83    pub const fn is_collision_free(&self) -> bool {
84        self.total_contacts == 0
85    }
86
87    /// Returns the stored contacts (at most `MAX_CONTACTS`).
88    pub fn as_slice(&self) -> &[CuCollisionContact] {
89        let count = (self.len as usize).min(MAX_CONTACTS);
90        &self.contacts[..count]
91    }
92
93    /// Total detected contacts, including those that did not fit into storage.
94    pub const fn total_contacts(&self) -> usize {
95        self.total_contacts as usize
96    }
97
98    /// Minimum surface-to-surface signed distance over all checked pairs
99    /// (`+inf` when no pair was checked; negative means penetration).
100    pub const fn min_signed_distance(&self) -> Length64 {
101        self.min_signed_distance
102    }
103
104    fn push_contact(&mut self, contact: CuCollisionContact) {
105        self.total_contacts = self.total_contacts.saturating_add(1);
106        let idx = self.len as usize;
107        if idx < MAX_CONTACTS {
108            self.contacts[idx] = contact;
109            self.len += 1;
110        }
111    }
112}
113
114/// Parameters of a [`CuRobotCollisionModel`].
115///
116/// ```ron
117/// {"safety_margin": 0.005, "skip_adjacent_links": true}
118/// ```
119#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize, Reflect)]
120#[serde(default, deny_unknown_fields)]
121pub struct CuCollisionModelParams {
122    /// Minimum surface distance for a pair to count as collision-free (finite, `>= 0`).
123    pub safety_margin: Length64,
124    /// Disable self-collision pairs between shapes on adjacent joint frames
125    /// (`Some(i)` and `Some(i ± 1)`). Shapes on the same frame are never checked against each
126    /// other regardless of this flag.
127    pub skip_adjacent_links: bool,
128}
129
130impl Default for CuCollisionModelParams {
131    fn default() -> Self {
132        Self {
133            safety_margin: Length64::new::<meter>(0.0),
134            skip_adjacent_links: true,
135        }
136    }
137}
138
139impl CuCollisionModelParams {
140    /// Checks that the safety margin is finite and non-negative.
141    pub fn validate(&self) -> CuResult<()> {
142        validate_margin(self.safety_margin)
143    }
144}
145
146fn validate_margin(margin: Length64) -> CuResult<()> {
147    let m = margin.get::<meter>();
148    if m.is_finite() && m >= 0.0 {
149        Ok(())
150    } else {
151        Err(CuError::from("repose_collision: invalid safety margin"))
152    }
153}
154
155/// Fixed-capacity model for `N` movable frames and up to `M` collision shapes.
156///
157/// Each shape is a [`CuLinkShape`] on a joint frame or the base. Shape indices identify contacts
158/// and collision pairs; several shapes may share a frame.
159#[derive(Debug, Clone, Copy, PartialEq, Reflect)]
160#[reflect(no_field_bounds, from_reflect = false)]
161pub struct CuRobotCollisionModel<const N: usize, const M: usize = N> {
162    shapes: [CuLinkShape; M],
163    safety_margin: Length64,
164    check_pair: [[bool; M]; M],
165    len: usize,
166}
167
168// The bincode derive currently emits invalid impl generics for dependent const defaults.
169impl<const N: usize, const M: usize> Encode for CuRobotCollisionModel<N, M> {
170    fn encode<E: bincode::enc::Encoder>(
171        &self,
172        encoder: &mut E,
173    ) -> Result<(), bincode::error::EncodeError> {
174        Encode::encode(&self.shapes, encoder)?;
175        Encode::encode(&self.safety_margin, encoder)?;
176        Encode::encode(&self.check_pair, encoder)?;
177        Encode::encode(&self.len, encoder)
178    }
179}
180impl<C, const N: usize, const M: usize> Decode<C> for CuRobotCollisionModel<N, M> {
181    fn decode<D: bincode::de::Decoder<Context = C>>(
182        decoder: &mut D,
183    ) -> Result<Self, bincode::error::DecodeError> {
184        let model = Self {
185            shapes: Decode::decode(decoder)?,
186            safety_margin: Decode::decode(decoder)?,
187            check_pair: Decode::decode(decoder)?,
188            len: Decode::decode(decoder)?,
189        };
190        if model.len > M || model.link_shapes().iter().any(|s| s.frame >= Some(N)) {
191            return Err(bincode::error::DecodeError::Other(
192                "Invalid collision model snapshot",
193            ));
194        }
195        Ok(model)
196    }
197}
198
199impl<'de, C, const N: usize, const M: usize> bincode::BorrowDecode<'de, C>
200    for CuRobotCollisionModel<N, M>
201{
202    fn borrow_decode<D: bincode::de::BorrowDecoder<'de, Context = C>>(
203        decoder: &mut D,
204    ) -> Result<Self, bincode::error::DecodeError> {
205        Decode::decode(decoder)
206    }
207}
208
209impl<const N: usize, const M: usize> CuRobotCollisionModel<N, M> {
210    /// Creates a model from up to `M` link shapes.
211    ///
212    /// Self-collision is checked between every pair of shapes on different frames, except
213    /// adjacent joint frames when `params.skip_adjacent_links` is set. Use
214    /// [`set_pair_enabled`](Self::set_pair_enabled) for further adjustments.
215    ///
216    /// Fails if more than `M` shapes are given, a frame index is `>= N`, a shape has invalid
217    /// geometry, or the params are invalid.
218    pub fn new(shapes: &[CuLinkShape], params: CuCollisionModelParams) -> CuResult<Self> {
219        params.validate()?;
220        if shapes.len() > M || M > u16::MAX as usize {
221            return Err(CuError::from(
222                "repose_collision: collision model shape count exceeds capacity",
223            ));
224        }
225        if shapes.iter().any(|s| s.frame >= Some(N)) {
226            return Err(CuError::from(
227                "repose_collision: collision model frame index out of range",
228            ));
229        }
230        for s in shapes {
231            s.shape.validate()?;
232        }
233        Ok(Self::assemble(shapes, params))
234    }
235
236    /// Builds the model from already validated inputs.
237    pub(crate) fn assemble(shapes: &[CuLinkShape], params: CuCollisionModelParams) -> Self {
238        let mut model = Self {
239            shapes: [CuLinkShape::default(); M],
240            safety_margin: params.safety_margin,
241            check_pair: [[false; M]; M],
242            len: shapes.len(),
243        };
244        model.shapes[..shapes.len()].copy_from_slice(shapes);
245        for i in 0..shapes.len() {
246            for j in i + 1..shapes.len() {
247                let (a, b) = (shapes[i].frame, shapes[j].frame);
248                // Shapes on the same rigid body cannot move relative to one another.
249                let adjacent = matches!((a, b), (Some(a), Some(b)) if a.abs_diff(b) == 1);
250                let enabled = a != b && !(params.skip_adjacent_links && adjacent);
251                model.check_pair[i][j] = enabled;
252                model.check_pair[j][i] = enabled;
253            }
254        }
255        model
256    }
257
258    /// Number of active collision shapes.
259    pub const fn shape_count(&self) -> usize {
260        self.len
261    }
262
263    /// Borrows the active link shapes.
264    pub fn link_shapes(&self) -> &[CuLinkShape] {
265        &self.shapes[..self.len]
266    }
267
268    /// Returns the configured safety margin.
269    pub const fn safety_margin(&self) -> Length64 {
270        self.safety_margin
271    }
272
273    /// Sets the safety margin required for a pair to be considered collision-free.
274    pub fn set_safety_margin(&mut self, safety_margin: Length64) -> CuResult<()> {
275        validate_margin(safety_margin)?;
276        self.safety_margin = safety_margin;
277        Ok(())
278    }
279
280    /// Enables or disables self-collision checking between two shape indices.
281    pub fn set_pair_enabled(
282        &mut self,
283        link_a: usize,
284        link_b: usize,
285        enabled: bool,
286    ) -> CuResult<()> {
287        if link_a >= self.len || link_b >= self.len {
288            return Err(CuError::from("repose_collision: link index out of bounds"));
289        }
290        self.check_pair[link_a][link_b] = enabled;
291        self.check_pair[link_b][link_a] = enabled;
292        Ok(())
293    }
294
295    /// Returns `true` if self-collision checking is enabled between `link_a` and `link_b`.
296    pub const fn is_pair_enabled(&self, link_a: usize, link_b: usize) -> bool {
297        if link_a >= self.len || link_b >= self.len {
298            return false;
299        }
300        self.check_pair[link_a][link_b]
301    }
302
303    /// Transforms the active link shapes into world coordinates in place using `link_frames`.
304    /// Only the first [`shape_count`](Self::shape_count) entries of `out_shapes` are written.
305    pub fn world_link_shapes(
306        &self,
307        link_frames: &[Transform3D<f64>; N],
308        out_shapes: &mut [CuCollisionShape; M],
309    ) {
310        for (link, out) in self.link_shapes().iter().zip(out_shapes.iter_mut()) {
311            *out = world_shape(link_frames, link.frame, &link.shape);
312        }
313    }
314
315    /// Evaluates self-collision and `env` (obstacles, attached shapes, exclusions) for
316    /// precomputed `link_frames`, writing contacts and the minimum signed distance into
317    /// `out_report` (cleared first).
318    ///
319    /// Fails, leaving `out_report` unmodified, if `env` is invalid for this model (see
320    /// [`CuCollisionEnv`]).
321    pub fn check_frames<const MAX_CONTACTS: usize>(
322        &self,
323        link_frames: &[Transform3D<f64>; N],
324        env: &CuCollisionEnv<'_>,
325        out_report: &mut CuCollisionReport<MAX_CONTACTS>,
326    ) -> CuResult<()> {
327        self.validate_env(env)?;
328        out_report.clear();
329        let margin_m = self.safety_margin.get::<meter>();
330        let mut min_dist_m = f64::INFINITY;
331        self.visit_pairs(link_frames, env, |contact| {
332            let d_m = contact.signed_distance.get::<meter>();
333            min_dist_m = min_dist_m.min(d_m);
334            if d_m < margin_m {
335                out_report.push_contact(contact);
336            }
337            true
338        });
339        out_report.min_signed_distance = Length64::new::<meter>(min_dist_m);
340        Ok(())
341    }
342
343    /// Runs forward kinematics for `q` and calls [`check_frames`](Self::check_frames).
344    pub fn check_configuration<const MAX_CONTACTS: usize>(
345        &self,
346        chain: &CuKinematicChain<N>,
347        q: &[CuJointPosition; N],
348        env: &CuCollisionEnv<'_>,
349        out_report: &mut CuCollisionReport<MAX_CONTACTS>,
350    ) -> CuResult<()> {
351        let mut frames = [Transform3D::<f64>::identity(); N];
352        let mut ee = Transform3D::<f64>::identity();
353        chain.forward_frames(q, &mut frames, &mut ee)?;
354        self.check_frames(&frames, env, out_report)
355    }
356
357    /// Early-exit predicate: `true` if `q` is within joint limits and no checked pair (self,
358    /// environment, attached) violates the safety margin.
359    pub fn is_configuration_collision_free(
360        &self,
361        chain: &CuKinematicChain<N>,
362        q: &[CuJointPosition; N],
363        env: &CuCollisionEnv<'_>,
364    ) -> CuResult<bool> {
365        self.validate_env(env)?;
366        if !chain.is_within_limits(q) {
367            return Ok(false);
368        }
369        let mut frames = [Transform3D::<f64>::identity(); N];
370        let mut ee = Transform3D::<f64>::identity();
371        chain.forward_frames(q, &mut frames, &mut ee)?;
372        let margin_m = self.safety_margin.get::<meter>();
373        let mut free = true;
374        self.visit_pairs(&frames, env, |contact| {
375            free = contact.signed_distance.get::<meter>() >= margin_m;
376            free
377        });
378        Ok(free)
379    }
380
381    /// Checks whether the linearly interpolated joint-space segment from `q_start` to `q_end`
382    /// is collision-free at the given angular and linear sampling resolution.
383    /// This is discrete sampling, not swept-volume continuous collision detection.
384    pub fn is_segment_collision_free(
385        &self,
386        chain: &CuKinematicChain<N>,
387        q_start: &[CuJointPosition; N],
388        q_end: &[CuJointPosition; N],
389        max_step: impl Into<CuJointResolution>,
390        env: &CuCollisionEnv<'_>,
391    ) -> CuResult<bool> {
392        let metric = max_step.into().metric();
393        metric.validate()?;
394        let mut max_delta = 0.0_f64;
395        for j in 0..N {
396            chain.joints()[j].validate_coordinate(q_start[j])?;
397            chain.joints()[j].validate_coordinate(q_end[j])?;
398            max_delta =
399                max_delta.max((metric.normalized(q_end[j]) - metric.normalized(q_start[j])).abs());
400        }
401        // Bound per-call work, including adversarial/non-finite inputs.
402        if max_delta.is_nan() || max_delta > 1_000_000.0 {
403            return Err(CuError::from("Collision segment requires too many samples"));
404        }
405        let steps = (libm::ceil(max_delta) as usize).max(1);
406        for s in 0..=steps {
407            let alpha = (s as f64) / (steps as f64);
408            let mut q_interp = [CuJointPosition::default(); N];
409            for k in 0..N {
410                q_interp[k] = q_start[k].interpolate(q_end[k], alpha)?;
411            }
412            if !self.is_configuration_collision_free(chain, &q_interp, env)? {
413                return Ok(false);
414            }
415        }
416        Ok(true)
417    }
418
419    /// Checks that `env` can be evaluated against this model.
420    fn validate_env(&self, env: &CuCollisionEnv<'_>) -> CuResult<()> {
421        if env.obstacles().len() > u16::MAX as usize
422            || self.len + env.attached().len() > u16::MAX as usize
423        {
424            return Err(CuError::from(
425                "repose_collision: scene query shape count out of range",
426            ));
427        }
428        if env.attached().iter().any(|a| a.frame >= Some(N)) {
429            return Err(CuError::from(
430                "repose_collision: attached shape frame out of range",
431            ));
432        }
433        validate_exclusions(env.exclusions(), env, self.len)
434    }
435
436    /// Visits every checked pair in a fixed order (robot self, robot vs. obstacles, attached vs.
437    /// attached, attached vs. robot, attached vs. obstacles); `visit` returns `false` to stop.
438    fn visit_pairs(
439        &self,
440        link_frames: &[Transform3D<f64>; N],
441        env: &CuCollisionEnv<'_>,
442        mut visit: impl FnMut(CuCollisionContact) -> bool,
443    ) {
444        let mut robot = [CuCollisionShape::default(); M];
445        self.world_link_shapes(link_frames, &mut robot);
446        let robot = &robot[..self.len];
447        let obstacles = env.obstacles();
448        let attached = env.attached();
449        let robot_count = robot.len();
450        let mut emit =
451            |kind, link: usize, partner: usize, a: &CuCollisionShape, b: &CuCollisionShape| {
452                visit(CuCollisionContact {
453                    kind,
454                    link_index: link as u16,
455                    partner_index: partner as u16,
456                    signed_distance: shape_signed_distance(a, b),
457                })
458            };
459
460        for i in 0..robot_count {
461            for j in i + 1..robot_count {
462                if self.check_pair[i][j] && !emit(SelfCollision, i, j, &robot[i], &robot[j]) {
463                    return;
464                }
465            }
466        }
467        for (i, r) in robot.iter().enumerate() {
468            for (o, obstacle) in obstacles.iter().enumerate() {
469                if !emit(EnvironmentCollision, i, o, r, obstacle) {
470                    return;
471                }
472            }
473        }
474        for (i, att) in attached.iter().enumerate() {
475            let w = world_shape(link_frames, att.frame, &att.shape);
476            let link = robot_count + i;
477            for (j, other) in attached.iter().enumerate().skip(i + 1) {
478                // Equal frames (including `None == None`) are one rigid body.
479                if att.frame == other.frame
480                    || env.is_excluded(
481                        att.id,
482                        Partner::Attached {
483                            attached_id: other.id,
484                        },
485                    )
486                {
487                    continue;
488                }
489                let wo = world_shape(link_frames, other.frame, &other.shape);
490                if !emit(SelfCollision, link, robot_count + j, &w, &wo) {
491                    return;
492                }
493            }
494        }
495        for (i, att) in attached.iter().enumerate() {
496            let w = world_shape(link_frames, att.frame, &att.shape);
497            let link = robot_count + i;
498            for (r, shape) in robot.iter().enumerate() {
499                let partner = Partner::RobotShape {
500                    shape_index: r as u16,
501                };
502                if !env.is_excluded(att.id, partner) && !emit(SelfCollision, link, r, &w, shape) {
503                    return;
504                }
505            }
506        }
507        for (i, att) in attached.iter().enumerate() {
508            let w = world_shape(link_frames, att.frame, &att.shape);
509            let link = robot_count + i;
510            for (o, obstacle) in obstacles.iter().enumerate() {
511                let excluded = env.obstacle_id(o).is_some_and(|obstacle_id| {
512                    env.is_excluded(att.id, Partner::Obstacle { obstacle_id })
513                });
514                if !excluded && !emit(EnvironmentCollision, link, o, &w, obstacle) {
515                    return;
516                }
517            }
518        }
519    }
520}
521
522/// World-space copy of `shape` expressed in `frame` (`None` = base, unchanged).
523fn world_shape<const N: usize>(
524    link_frames: &[Transform3D<f64>; N],
525    frame: Option<usize>,
526    shape: &CuCollisionShape,
527) -> CuCollisionShape {
528    match frame {
529        Some(f) => {
530            let mut out = *shape;
531            transform_shape(&link_frames[f], shape, &mut out);
532            out
533        }
534        None => *shape,
535    }
536}
repose_collision/model.rs

repose_collision/
model.rs
