repose_motion_planning/
prm.rs

1//! PRM: sampling-based probabilistic roadmap.
2//!
3//! The roadmap starts with the start and goal configurations. Each work unit
4//! adds one uniform sample; free samples connect to their nearest roadmap
5//! neighbours within `connection_radius` through free segments. After every
6//! quantum that changed the roadmap, Dijkstra finds the shortest roadmap
7//! path, which is shortcut by line of sight; the job improves as the roadmap
8//! densifies and converges when it is full. The roadmap is rebuilt for every
9//! job from the job seed, so replay reproduces it.
10
11use crate::CuJointPlanPath;
12use crate::CuJointPlanRequest;
13use crate::CuJointPlanner;
14use crate::CuJointPlannerTask;
15use crate::CuPlanStatus;
16use crate::CuPlannerKind;
17use crate::common::check_endpoints;
18use crate::common::length_quality;
19use crate::common::sample_uniform;
20use crate::common::write_header;
21use crate::common::write_shortcut;
22use crate::validity::CuSceneValidity;
23use crate::validity::joint_distance;
24use bincode::Decode;
25use bincode::Encode;
26use cu_rng::prelude::*;
27use cu29::cutask_anytime::Quality;
28use cu29::prelude::*;
29use repose_collision::CuRobotCollisionModel;
30use repose_kinematics::CuJointMetric;
31use repose_kinematics::CuJointPosition;
32use repose_kinematics::CuJointResolution;
33use repose_kinematics::CuKinematicChain;
34use serde::Deserialize;
35use serde::Serialize;
36
37/// Roadmap node capacity, including start and goal.
38pub const PRM_MAX_NODES: usize = 1024;
39/// Longest roadmap path (in nodes) that can be reconstructed.
40const PRM_MAX_PATH: usize = 256;
41/// Edge capacity per roadmap node.
42pub const PRM_MAX_NEIGHBORS: usize = 10;
43
44/// Parameters of [`CuJointPrm`].
45#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize, Encode, Decode, Reflect)]
46#[serde(default, deny_unknown_fields)]
47pub struct CuJointPrmParams {
48    /// Joint-space radius for roadmap edges (dimensionless, metric-scaled).
49    pub connection_radius: f64,
50    /// Neighbours tried per new node (`1..=PRM_MAX_NEIGHBORS`).
51    pub neighbors: u32,
52    /// Roadmap capacity (`3..=PRM_MAX_NODES`).
53    pub max_nodes: u32,
54    /// Resolution of the segment collision checks.
55    pub collision_check_step: CuJointResolution,
56    /// Scaling of dimensionless joint-space distances and costs.
57    pub metric: CuJointMetric,
58}
59
60impl Default for CuJointPrmParams {
61    fn default() -> Self {
62        Self {
63            connection_radius: 2.0,
64            neighbors: 8,
65            max_nodes: PRM_MAX_NODES as u32,
66            collision_check_step: CuJointResolution::default(),
67            metric: CuJointMetric::default(),
68        }
69    }
70}
71
72impl CuJointPrmParams {
73    /// Checks every parameter, the metric, and the collision-check resolution.
74    pub fn validate(&self) -> CuResult<()> {
75        self.metric.validate()?;
76        self.collision_check_step.metric().validate()?;
77        let valid = {
78            self.connection_radius.is_finite()
79                && self.connection_radius > 0.0
80                && (1..=PRM_MAX_NEIGHBORS as u32).contains(&self.neighbors)
81                && (3..=PRM_MAX_NODES as u32).contains(&self.max_nodes)
82        };
83        if valid {
84            Ok(())
85        } else {
86            Err(CuError::from(
87                "repose-motion-planning: invalid PRM parameters",
88            ))
89        }
90    }
91}
92
93/// Probabilistic roadmap planner (single query per job).
94///
95/// Built with [`CuJointPlanner::new`] from robot models and [`CuJointPrmParams`];
96/// runs in [`CuJointPrmTask`].
97pub struct CuJointPrm<const N: usize, const M: usize = N> {
98    validity: CuSceneValidity<N, M>,
99    params: CuJointPrmParams,
100    nodes: [[CuJointPosition; N]; PRM_MAX_NODES],
101    edges: [[u16; PRM_MAX_NEIGHBORS]; PRM_MAX_NODES],
102    degree: [u8; PRM_MAX_NODES],
103    len: usize,
104    rng: CuRng,
105    straight_cost: f64,
106    samples: u32,
107    status: CuPlanStatus,
108    best: CuJointPlanPath<N>,
109}
110
111const START: usize = 0;
112const GOAL: usize = 1;
113
114/// [`CuJointPlannerTask`] running [`CuJointPrm`].
115pub type CuJointPrmTask<const N: usize, const M: usize = N> =
116    CuJointPlannerTask<CuJointPrm<N, M>, N, M>;
117
118impl<const N: usize, const M: usize> CuJointPrm<N, M> {
119    /// The validated planner parameters.
120    pub const fn params(&self) -> &CuJointPrmParams {
121        &self.params
122    }
123
124    fn build(
125        chain: CuKinematicChain<N>,
126        model: CuRobotCollisionModel<N, M>,
127        params: CuJointPrmParams,
128    ) -> Self {
129        Self {
130            validity: CuSceneValidity::new(chain, model, params.collision_check_step),
131            params,
132            nodes: [[CuJointPosition::default(); N]; PRM_MAX_NODES],
133            edges: [[0; PRM_MAX_NEIGHBORS]; PRM_MAX_NODES],
134            degree: [0; PRM_MAX_NODES],
135            len: 0,
136            rng: CuRng::from_seed(0),
137            straight_cost: 0.0,
138            samples: 0,
139            status: CuPlanStatus::InvalidRequest,
140            best: CuJointPlanPath::default(),
141        }
142    }
143
144    /// Roadmap size of the current job.
145    pub const fn roadmap_size(&self) -> usize {
146        self.len
147    }
148
149    fn link(&mut self, a: usize, b: usize) {
150        let cap = PRM_MAX_NEIGHBORS.min(self.params.neighbors as usize);
151        if (self.degree[a] as usize) < cap && (self.degree[b] as usize) < cap {
152            self.edges[a][self.degree[a] as usize] = b as u16;
153            self.degree[a] += 1;
154            self.edges[b][self.degree[b] as usize] = a as u16;
155            self.degree[b] += 1;
156        }
157    }
158
159    /// Connects node `idx` to its nearest free neighbours; returns whether
160    /// any edge was added.
161    fn connect(&mut self, idx: usize) -> bool {
162        let k = (self.params.neighbors as usize).min(PRM_MAX_NEIGHBORS);
163        let mut nearest = [(usize::MAX, f64::INFINITY); PRM_MAX_NEIGHBORS];
164        for other in 0..self.len {
165            if other == idx {
166                continue;
167            }
168            let d = joint_distance(&self.nodes[idx], &self.nodes[other], self.params.metric);
169            if d > self.params.connection_radius || d >= nearest[k - 1].1 {
170                continue;
171            }
172            let mut slot = k - 1;
173            while slot > 0 && nearest[slot - 1].1 > d {
174                nearest[slot] = nearest[slot - 1];
175                slot -= 1;
176            }
177            nearest[slot] = (other, d);
178        }
179        let mut added = false;
180        for &(other, _) in nearest[..k].iter().take_while(|n| n.0 != usize::MAX) {
181            if self
182                .validity
183                .is_segment_free(&self.nodes[idx], &self.nodes[other])
184            {
185                let before = self.degree[idx];
186                self.link(idx, other);
187                added |= self.degree[idx] != before;
188            }
189        }
190        added
191    }
192
193    /// Dijkstra from start to goal; on a shorter path, shortcuts it into `best`.
194    fn search(&mut self) {
195        let mut dist = [f64::INFINITY; PRM_MAX_NODES];
196        let mut prev = [u16::MAX; PRM_MAX_NODES];
197        let mut done = [false; PRM_MAX_NODES];
198        dist[START] = 0.0;
199        loop {
200            let mut current = usize::MAX;
201            for i in 0..self.len {
202                if !done[i]
203                    && dist[i].is_finite()
204                    && (current == usize::MAX || dist[i] < dist[current])
205                {
206                    current = i;
207                }
208            }
209            if current == usize::MAX || current == GOAL {
210                break;
211            }
212            done[current] = true;
213            for e in 0..self.degree[current] as usize {
214                let next = self.edges[current][e] as usize;
215                let d = dist[current]
216                    + joint_distance(&self.nodes[current], &self.nodes[next], self.params.metric);
217                if d < dist[next] {
218                    dist[next] = d;
219                    prev[next] = current as u16;
220                }
221            }
222        }
223        if !dist[GOAL].is_finite() || dist[GOAL] >= self.best.cost {
224            return;
225        }
226        let mut raw = [[CuJointPosition::default(); N]; PRM_MAX_PATH];
227        let mut len = 0;
228        let mut at = GOAL;
229        loop {
230            if len >= PRM_MAX_PATH {
231                if !self.best.cost.is_finite() {
232                    self.status = CuPlanStatus::CapacityExceeded;
233                }
234                return;
235            }
236            raw[len] = self.nodes[at];
237            len += 1;
238            if at == START {
239                break;
240            }
241            at = prev[at] as usize;
242        }
243        raw[..len].reverse();
244        let mut candidate = CuJointPlanPath::default();
245        if write_shortcut(
246            &self.validity,
247            &raw[..len],
248            self.params.metric,
249            &mut candidate,
250        ) {
251            if candidate.cost < self.best.cost {
252                self.best = candidate;
253            }
254        } else if !self.best.cost.is_finite() {
255            self.status = CuPlanStatus::CapacityExceeded;
256        }
257    }
258
259    fn refresh_status(&mut self) {
260        if self.status.is_final() {
261            return;
262        }
263        let full = self.len >= self.params.max_nodes as usize;
264        let solved = self.best.cost.is_finite();
265        let optimal = solved && self.best.cost <= self.straight_cost + 1e-9;
266        self.status = match (solved, full || optimal) {
267            (true, true) => CuPlanStatus::Converged,
268            (true, false) => CuPlanStatus::Improving,
269            (false, true) => CuPlanStatus::Exhausted,
270            (false, false) => CuPlanStatus::Searching,
271        };
272    }
273}
274
275impl<const N: usize, const M: usize> CuJointPlanner<N, M> for CuJointPrm<N, M> {
276    const KIND: CuPlannerKind = CuPlannerKind::Sampling;
277
278    type Params = CuJointPrmParams;
279
280    fn new(
281        chain: CuKinematicChain<N>,
282        collision_model: CuRobotCollisionModel<N, M>,
283        params: CuJointPrmParams,
284    ) -> CuResult<Self> {
285        params.validate()?;
286        Ok(Self::build(chain, collision_model, params))
287    }
288
289    fn begin(&mut self, request: &CuJointPlanRequest<N>, seed: u64) -> CuResult<CuPlanStatus> {
290        self.rng.reseed(seed);
291        self.samples = 0;
292        self.len = 0;
293        self.degree = [0; PRM_MAX_NODES];
294        self.best = CuJointPlanPath::default();
295        self.straight_cost = joint_distance(&request.start, &request.goal, self.params.metric);
296        self.status = check_endpoints(&mut self.validity, request, self.params.metric);
297        if self.status != CuPlanStatus::Searching {
298            return Ok(self.status);
299        }
300        self.nodes[START] = request.start;
301        self.nodes[GOAL] = request.goal;
302        self.len = 2;
303        if self.validity.is_segment_free(&request.start, &request.goal) {
304            self.link(START, GOAL);
305            self.search();
306        }
307        self.refresh_status();
308        Ok(self.status)
309    }
310
311    fn step(&mut self, budget: u32) -> CuResult<CuPlanStatus> {
312        let mut changed = false;
313        for _ in 0..budget {
314            if self.status.is_final() || self.len >= self.params.max_nodes as usize {
315                break;
316            }
317            self.samples += 1;
318            let q = sample_uniform(self.validity.chain(), &mut self.rng);
319            if !self.validity.is_configuration_free(&q) {
320                continue;
321            }
322            let idx = self.len;
323            self.nodes[idx] = q;
324            self.degree[idx] = 0;
325            self.len += 1;
326            changed |= self.connect(idx);
327        }
328        if changed {
329            self.search();
330        }
331        self.refresh_status();
332        Ok(self.status)
333    }
334
335    fn write_best(&self, out: &mut CuJointPlanPath<N>) -> CuPlanStatus {
336        write_header(out, &self.validity, self.status, self.samples);
337        if self.status.has_path() {
338            out.waypoints = self.best.waypoints.clone();
339            out.len = self.best.len;
340            out.cost = self.best.cost;
341        }
342        self.status
343    }
344
345    fn quality(&self) -> Quality {
346        length_quality(
347            self.straight_cost,
348            if self.status.has_path() {
349                self.best.cost
350            } else {
351                f64::INFINITY
352            },
353        )
354    }
355}
356
357crate::impl_planner_type_path!(CuJointPrm, "repose_motion_planning::CuJointPrm");
repose_motion_planning/prm.rs

repose_motion_planning/
prm.rs
