repose_motion_planning/
astar.rs

1//! A* lattice search: search-based, deterministic, resolution-complete.
2//!
3//! Configurations live on a grid anchored at the start with spacing
4//! `resolution` (normalized joint units). Each expansion moves one joint by
5//! one grid step in either direction through a free segment, at cost
6//! `resolution`; the heuristic is the straight-line distance to the goal,
7//! which is admissible and consistent for these moves. When an expanded node
8//! lies within one grid cell diagonal of the goal and sees it through a free
9//! segment, the lattice path plus that last segment is shortcut by line of
10//! sight and the job converges. Exhausting the open set or the node capacity
11//! ends the job without a path. No randomness is involved.
12
13use crate::CuJointPlanPath;
14use crate::CuJointPlanRequest;
15use crate::CuJointPlanner;
16use crate::CuJointPlannerTask;
17use crate::CuPlanStatus;
18use crate::CuPlannerKind;
19use crate::common::check_endpoints;
20use crate::common::length_quality;
21use crate::common::write_header;
22use crate::common::write_shortcut;
23use crate::validity::CuSceneValidity;
24use crate::validity::joint_distance;
25use bincode::Decode;
26use bincode::Encode;
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/// Lattice node capacity.
38pub const ASTAR_MAX_NODES: usize = 2048;
39/// Longest lattice path (in grid steps) that can be reconstructed.
40const ASTAR_MAX_PATH: usize = 512;
41const HASH_SLOTS: usize = 2 * ASTAR_MAX_NODES;
42const EMPTY: u16 = u16::MAX;
43
44/// Parameters of [`CuJointAStar`].
45#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize, Encode, Decode, Reflect)]
46#[serde(default, deny_unknown_fields)]
47pub struct CuJointAStarParams {
48    /// Grid spacing (dimensionless, metric-scaled joint units).
49    pub resolution: f64,
50    /// Node capacity (`2..=ASTAR_MAX_NODES`).
51    pub max_nodes: u32,
52    /// Resolution of the segment collision checks.
53    pub collision_check_step: CuJointResolution,
54    /// Scaling of dimensionless joint-space distances and costs.
55    pub metric: CuJointMetric,
56}
57
58impl Default for CuJointAStarParams {
59    fn default() -> Self {
60        Self {
61            resolution: 0.25,
62            max_nodes: ASTAR_MAX_NODES as u32,
63            collision_check_step: CuJointResolution::default(),
64            metric: CuJointMetric::default(),
65        }
66    }
67}
68
69impl CuJointAStarParams {
70    /// Checks every parameter, the metric, and the collision-check resolution.
71    pub fn validate(&self) -> CuResult<()> {
72        self.metric.validate()?;
73        self.collision_check_step.metric().validate()?;
74        let valid = {
75            self.resolution.is_finite()
76                && self.resolution > 0.0
77                && (2..=ASTAR_MAX_NODES as u32).contains(&self.max_nodes)
78        };
79        if valid {
80            Ok(())
81        } else {
82            Err(CuError::from(
83                "repose-motion-planning: invalid A* parameters",
84            ))
85        }
86    }
87}
88
89/// A* search over a joint-space lattice.
90///
91/// Built with [`CuJointPlanner::new`] from robot models and [`CuJointAStarParams`];
92/// runs in [`CuJointAStarTask`].
93#[derive(Clone)]
94pub struct CuJointAStar<const N: usize, const M: usize = N> {
95    validity: CuSceneValidity<N, M>,
96    params: CuJointAStarParams,
97    start: [CuJointPosition; N],
98    goal: [CuJointPosition; N],
99    cells: [[i16; N]; ASTAR_MAX_NODES],
100    cost: [f64; ASTAR_MAX_NODES],
101    estimate: [f64; ASTAR_MAX_NODES],
102    parent: [u16; ASTAR_MAX_NODES],
103    closed: [bool; ASTAR_MAX_NODES],
104    table: [u16; HASH_SLOTS],
105    len: usize,
106    open: usize,
107    goal_reach: f64,
108    straight_cost: f64,
109    expansions: u32,
110    status: CuPlanStatus,
111    best: CuJointPlanPath<N>,
112}
113
114/// [`CuJointPlannerTask`] running [`CuJointAStar`].
115pub type CuJointAStarTask<const N: usize, const M: usize = N> =
116    CuJointPlannerTask<CuJointAStar<N, M>, N, M>;
117
118impl<const N: usize, const M: usize> CuJointAStar<N, M> {
119    /// The validated planner parameters.
120    pub const fn params(&self) -> &CuJointAStarParams {
121        &self.params
122    }
123
124    fn build(
125        chain: CuKinematicChain<N>,
126        model: CuRobotCollisionModel<N, M>,
127        params: CuJointAStarParams,
128    ) -> Self {
129        Self {
130            validity: CuSceneValidity::new(chain, model, params.collision_check_step),
131            params,
132            start: [CuJointPosition::default(); N],
133            goal: [CuJointPosition::default(); N],
134            cells: [[0; N]; ASTAR_MAX_NODES],
135            cost: [0.0; ASTAR_MAX_NODES],
136            estimate: [0.0; ASTAR_MAX_NODES],
137            parent: [EMPTY; ASTAR_MAX_NODES],
138            closed: [false; ASTAR_MAX_NODES],
139            table: [EMPTY; HASH_SLOTS],
140            len: 0,
141            open: 0,
142            goal_reach: 0.0,
143            straight_cost: 0.0,
144            expansions: 0,
145            status: CuPlanStatus::InvalidRequest,
146            best: CuJointPlanPath::default(),
147        }
148    }
149
150    /// Nodes created on the current job.
151    pub const fn lattice_size(&self) -> usize {
152        self.len
153    }
154
155    fn configuration(&self, cell: &[i16; N]) -> [CuJointPosition; N] {
156        let metric = self.params.metric;
157        core::array::from_fn(|j| {
158            let q = self.start[j];
159            let offset = cell[j] as f64 * self.params.resolution * metric.scale(q);
160            q.with_si_value(q.si_value() + offset)
161        })
162    }
163
164    fn hash(cell: &[i16; N]) -> usize {
165        let mut h: u64 = 0xcbf2_9ce4_8422_2325;
166        for &c in cell {
167            h ^= c as u16 as u64;
168            h = h.wrapping_mul(0x0100_0000_01b3);
169        }
170        (h as usize) % HASH_SLOTS
171    }
172
173    fn find(&self, cell: &[i16; N]) -> Option<usize> {
174        let mut slot = Self::hash(cell);
175        loop {
176            match self.table[slot] {
177                EMPTY => return None,
178                idx if self.cells[idx as usize] == *cell => return Some(idx as usize),
179                _ => slot = (slot + 1) % HASH_SLOTS,
180            }
181        }
182    }
183
184    fn insert(&mut self, cell: [i16; N], cost: f64, parent: u16) -> usize {
185        let idx = self.len;
186        self.cells[idx] = cell;
187        self.cost[idx] = cost;
188        self.estimate[idx] =
189            cost + joint_distance(&self.configuration(&cell), &self.goal, self.params.metric);
190        self.parent[idx] = parent;
191        self.closed[idx] = false;
192        let mut slot = Self::hash(&cell);
193        while self.table[slot] != EMPTY {
194            slot = (slot + 1) % HASH_SLOTS;
195        }
196        self.table[slot] = idx as u16;
197        self.len += 1;
198        self.open += 1;
199        idx
200    }
201
202    fn finish(&mut self, last: usize) {
203        let mut raw = [[CuJointPosition::default(); N]; ASTAR_MAX_PATH];
204        let mut len = 0;
205        raw[len] = self.goal;
206        len += 1;
207        let mut at = last;
208        loop {
209            if len >= ASTAR_MAX_PATH {
210                self.status = CuPlanStatus::CapacityExceeded;
211                return;
212            }
213            raw[len] = self.configuration(&self.cells[at]);
214            len += 1;
215            if self.parent[at] == EMPTY {
216                break;
217            }
218            at = self.parent[at] as usize;
219        }
220        raw[..len].reverse();
221        self.status = if write_shortcut(
222            &self.validity,
223            &raw[..len],
224            self.params.metric,
225            &mut self.best,
226        ) {
227            CuPlanStatus::Converged
228        } else {
229            CuPlanStatus::CapacityExceeded
230        };
231    }
232
233    fn expand(&mut self) {
234        let mut current = usize::MAX;
235        for i in 0..self.len {
236            if !self.closed[i]
237                && (current == usize::MAX || self.estimate[i] < self.estimate[current])
238            {
239                current = i;
240            }
241        }
242        if current == usize::MAX {
243            self.status = CuPlanStatus::Exhausted;
244            return;
245        }
246        self.closed[current] = true;
247        self.open -= 1;
248        self.expansions += 1;
249        let cell = self.cells[current];
250        let q = self.configuration(&cell);
251        if joint_distance(&q, &self.goal, self.params.metric) <= self.goal_reach
252            && self.validity.is_segment_free(&q, &self.goal)
253        {
254            self.finish(current);
255            return;
256        }
257        for j in 0..N {
258            if matches!(self.start[j], CuJointPosition::Fixed) {
259                continue;
260            }
261            for direction in [-1i16, 1] {
262                let mut next = cell;
263                next[j] = match next[j].checked_add(direction) {
264                    Some(v) => v,
265                    None => continue,
266                };
267                let cost = self.cost[current] + self.params.resolution;
268                match self.find(&next) {
269                    Some(idx) if self.closed[idx] || cost >= self.cost[idx] => {}
270                    Some(idx) => {
271                        let q_next = self.configuration(&next);
272                        if self.validity.is_segment_free(&q, &q_next) {
273                            self.estimate[idx] += cost - self.cost[idx];
274                            self.cost[idx] = cost;
275                            self.parent[idx] = current as u16;
276                        }
277                    }
278                    None => {
279                        if self.len >= self.params.max_nodes as usize {
280                            continue;
281                        }
282                        let q_next = self.configuration(&next);
283                        if self.validity.is_segment_free(&q, &q_next) {
284                            self.insert(next, cost, current as u16);
285                        }
286                    }
287                }
288            }
289        }
290        if self.open == 0 {
291            self.status = CuPlanStatus::Exhausted;
292        }
293    }
294}
295
296impl<const N: usize, const M: usize> CuJointPlanner<N, M> for CuJointAStar<N, M> {
297    const KIND: CuPlannerKind = CuPlannerKind::Search;
298
299    type Params = CuJointAStarParams;
300
301    fn new(
302        chain: CuKinematicChain<N>,
303        collision_model: CuRobotCollisionModel<N, M>,
304        params: CuJointAStarParams,
305    ) -> CuResult<Self> {
306        params.validate()?;
307        Ok(Self::build(chain, collision_model, params))
308    }
309
310    fn begin(&mut self, request: &CuJointPlanRequest<N>, _seed: u64) -> CuResult<CuPlanStatus> {
311        self.start = request.start;
312        self.goal = request.goal;
313        self.expansions = 0;
314        self.len = 0;
315        self.open = 0;
316        self.table = [EMPTY; HASH_SLOTS];
317        self.best = CuJointPlanPath::default();
318        self.straight_cost = joint_distance(&request.start, &request.goal, self.params.metric);
319        let active = self
320            .start
321            .iter()
322            .filter(|q| !matches!(q, CuJointPosition::Fixed))
323            .count()
324            .max(1);
325        // Every point is within half a cell diagonal of the lattice; one full
326        // diagonal leaves room for the free-segment check to succeed.
327        self.goal_reach = self.params.resolution * libm::sqrt(active as f64);
328        self.status = check_endpoints(&mut self.validity, request, self.params.metric);
329        if self.status == CuPlanStatus::Searching {
330            self.insert([0; N], 0.0, EMPTY);
331        }
332        Ok(self.status)
333    }
334
335    fn step(&mut self, budget: u32) -> CuResult<CuPlanStatus> {
336        for _ in 0..budget {
337            if self.status.is_final() {
338                break;
339            }
340            self.expand();
341        }
342        Ok(self.status)
343    }
344
345    fn write_best(&self, out: &mut CuJointPlanPath<N>) -> CuPlanStatus {
346        write_header(out, &self.validity, self.status, self.expansions);
347        if self.status.has_path() {
348            out.waypoints = self.best.waypoints.clone();
349            out.len = self.best.len;
350            out.cost = self.best.cost;
351        }
352        self.status
353    }
354
355    fn quality(&self) -> Quality {
356        length_quality(
357            self.straight_cost,
358            if self.status.has_path() {
359                self.best.cost
360            } else {
361                f64::INFINITY
362            },
363        )
364    }
365}
366
367crate::impl_planner_type_path!(CuJointAStar, "repose_motion_planning::CuJointAStar");
repose_motion_planning/astar.rs

repose_motion_planning/
astar.rs
