repose_motion_planning/
validity.rs

1//! Scene-aware validity checking shared by every planner.
2//!
3//! [`CuSceneValidity`] captures one [`CuPlanningScene`] snapshot per planning
4//! job and answers the three questions any planner asks, sampling-based or
5//! not: is a configuration free, is a joint-space segment free, and how much
6//! clearance does a configuration have.
7
8use crate::MAX_ATTACHED_SHAPES;
9use crate::MAX_EXCLUSIONS;
10use crate::MAX_OBSTACLES;
11use cu29::units::si::length::meter;
12use repose_collision::CuCollisionEnv;
13use repose_collision::CuCollisionReport;
14use repose_collision::CuPlanningScene;
15use repose_collision::CuRobotCollisionModel;
16use repose_collision::CuSceneExclusions;
17use repose_collision::CuSceneRevision;
18use repose_kinematics::CuJointMetric;
19use repose_kinematics::CuJointPosition;
20use repose_kinematics::CuJointResolution;
21use repose_kinematics::CuKinematicChain;
22
23/// Collision queries against one captured planning-scene snapshot.
24///
25/// Every query checks the robot against itself, the scene obstacles, and the
26/// scene's attached shapes in one pass, honouring the captured stable-ID
27/// exclusions.
28#[derive(Clone)]
29pub struct CuSceneValidity<const N: usize, const M: usize = N> {
30    chain: CuKinematicChain<N>,
31    model: CuRobotCollisionModel<N, M>,
32    resolution: CuJointResolution,
33    scene: CuPlanningScene<MAX_OBSTACLES, MAX_ATTACHED_SHAPES>,
34    exclusions: CuSceneExclusions<MAX_EXCLUSIONS>,
35    /// `false` until a valid scene has been captured.
36    captured: bool,
37}
38
39impl<const N: usize, const M: usize> CuSceneValidity<N, M> {
40    /// Creates a checker with an empty scene; `resolution` sets the segment
41    /// sampling step.
42    pub fn new(
43        chain: CuKinematicChain<N>,
44        model: CuRobotCollisionModel<N, M>,
45        resolution: CuJointResolution,
46    ) -> Self {
47        Self {
48            chain,
49            model,
50            resolution,
51            scene: CuPlanningScene::new(),
52            exclusions: CuSceneExclusions::new(),
53            captured: true,
54        }
55    }
56
57    /// The kinematic chain the checker evaluates.
58    pub const fn chain(&self) -> &CuKinematicChain<N> {
59        &self.chain
60    }
61
62    /// Segment sampling resolution.
63    pub const fn resolution(&self) -> CuJointResolution {
64        self.resolution
65    }
66
67    /// Revision of the captured scene.
68    pub const fn scene_revision(&self) -> CuSceneRevision {
69        self.scene.revision()
70    }
71
72    /// Exclusions captured with the scene.
73    pub const fn exclusions(&self) -> &CuSceneExclusions<MAX_EXCLUSIONS> {
74        &self.exclusions
75    }
76
77    /// Captures a scene snapshot for the next job. Returns `false` (and
78    /// treats every configuration as blocked) when the resolution, the
79    /// scene's attached shapes, or the stable-ID exclusions are invalid for
80    /// this robot.
81    pub fn capture(
82        &mut self,
83        scene: &CuPlanningScene<MAX_OBSTACLES, MAX_ATTACHED_SHAPES>,
84        exclusions: &CuSceneExclusions<MAX_EXCLUSIONS>,
85    ) -> bool {
86        self.scene = scene.clone();
87        self.exclusions = exclusions.clone();
88        self.captured = self.resolution.metric().validate().is_ok()
89            && self
90                .scene
91                .attached_shapes()
92                .iter()
93                .all(|a| a.frame.is_none_or(|frame| frame < N))
94            && exclusions
95                .validate(&self.scene, self.model.shape_count())
96                .is_ok();
97        self.captured
98    }
99
100    /// Obstacles, attached shapes, and exclusions of the captured scene.
101    fn env(&self) -> CuCollisionEnv<'_> {
102        self.scene.collision_env().with_exclusions(&self.exclusions)
103    }
104
105    /// `true` when `q` is within joint limits and free of self, environment,
106    /// and attached-object collisions.
107    pub fn is_configuration_free(&self, q: &[CuJointPosition; N]) -> bool {
108        self.captured
109            && self
110                .model
111                .is_configuration_collision_free(&self.chain, q, &self.env())
112                .unwrap_or(false)
113    }
114
115    /// `true` when the joint-space line from `a` to `b` is free at the
116    /// configured resolution (discrete sampling, not swept volumes).
117    pub fn is_segment_free(&self, a: &[CuJointPosition; N], b: &[CuJointPosition; N]) -> bool {
118        let metric = self.resolution.metric();
119        let mut max_delta = 0.0_f64;
120        for j in 0..N {
121            let joint = &self.chain.joints()[j];
122            if joint.validate_coordinate(a[j]).is_err() || joint.validate_coordinate(b[j]).is_err()
123            {
124                return false;
125            }
126            max_delta = max_delta.max(libm::fabs(
127                metric.normalized(b[j]) - metric.normalized(a[j]),
128            ));
129        }
130        if !max_delta.is_finite() || max_delta > 1_000_000.0 {
131            return false;
132        }
133        let steps = (libm::ceil(max_delta) as usize).max(1);
134        for s in 0..=steps {
135            let alpha = s as f64 / steps as f64;
136            let mut q = [CuJointPosition::default(); N];
137            for j in 0..N {
138                match a[j].interpolate(b[j], alpha) {
139                    Ok(value) => q[j] = value,
140                    Err(_) => return false,
141                }
142            }
143            if !self.is_configuration_free(&q) {
144                return false;
145            }
146        }
147        true
148    }
149
150    /// Smallest signed surface distance in meters between the robot (and
151    /// attached objects) and itself or the scene; negative means penetration.
152    /// `None` when `q` cannot be evaluated.
153    pub fn clearance(&self, q: &[CuJointPosition; N]) -> Option<f64> {
154        if !self.captured {
155            return None;
156        }
157        let mut report = CuCollisionReport::<0>::default();
158        self.model
159            .check_configuration(&self.chain, q, &self.env(), &mut report)
160            .ok()?;
161        Some(report.min_signed_distance().get::<meter>())
162    }
163}
164
165/// Joint-space L2 distance under `metric` (dimensionless, metric-scaled).
166pub fn joint_distance<const N: usize>(
167    a: &[CuJointPosition; N],
168    b: &[CuJointPosition; N],
169    metric: CuJointMetric,
170) -> f64 {
171    let mut sum = 0.0_f64;
172    for j in 0..N {
173        let d = metric.normalized(b[j]) - metric.normalized(a[j]);
174        sum += d * d;
175    }
176    libm::sqrt(sum)
177}
repose_motion_planning/validity.rs

repose_motion_planning/
validity.rs
