repose_motion_planning/
rrt_connect.rs

1//! RRT-Connect: sampling-based, bidirectional, feasibility-first.
2//!
3//! Two trees grow from start and goal. Each iteration extends one tree one
4//! step toward a uniform sample, then greedily connects the other tree to the
5//! new node; the trees swap roles every iteration. The first connection is
6//! shortcut by line of sight and the job converges: RRT-Connect finds a path
7//! quickly but does not optimize it (use RRT*, PRM, or an optimizer for that).
8//! All randomness comes from the job seed.
9
10use crate::CuJointPlanPath;
11use crate::CuJointPlanRequest;
12use crate::CuJointPlanner;
13use crate::CuJointPlannerTask;
14use crate::CuPlanStatus;
15use crate::CuPlannerKind;
16use crate::common::check_endpoints;
17use crate::common::length_quality;
18use crate::common::sample_uniform;
19use crate::common::steer;
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/// Node capacity of each of the two trees.
38pub const RRT_CONNECT_MAX_NODES: usize = 256;
39
40/// Parameters of [`CuJointRrtConnect`].
41#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize, Encode, Decode, Reflect)]
42#[serde(default, deny_unknown_fields)]
43pub struct CuJointRrtConnectParams {
44    /// Maximum joint-space step per extension (dimensionless, metric-scaled).
45    pub step_size: f64,
46    /// Node capacity per tree (`2..=RRT_CONNECT_MAX_NODES`).
47    pub max_nodes: u32,
48    /// Resolution of the segment collision checks.
49    pub collision_check_step: CuJointResolution,
50    /// Scaling of dimensionless joint-space distances and costs.
51    pub metric: CuJointMetric,
52}
53
54impl Default for CuJointRrtConnectParams {
55    fn default() -> Self {
56        Self {
57            step_size: 0.3,
58            max_nodes: RRT_CONNECT_MAX_NODES as u32,
59            collision_check_step: CuJointResolution::default(),
60            metric: CuJointMetric::default(),
61        }
62    }
63}
64
65impl CuJointRrtConnectParams {
66    /// Checks every parameter, the metric, and the collision-check resolution.
67    pub fn validate(&self) -> CuResult<()> {
68        self.metric.validate()?;
69        self.collision_check_step.metric().validate()?;
70        let valid = {
71            self.step_size.is_finite()
72                && self.step_size > 0.0
73                && (2..=RRT_CONNECT_MAX_NODES as u32).contains(&self.max_nodes)
74        };
75        if valid {
76            Ok(())
77        } else {
78            Err(CuError::from(
79                "repose-motion-planning: invalid RRT-Connect parameters",
80            ))
81        }
82    }
83}
84
85#[derive(Clone)]
86struct Tree<const N: usize> {
87    nodes: [[CuJointPosition; N]; RRT_CONNECT_MAX_NODES],
88    parents: [u16; RRT_CONNECT_MAX_NODES],
89    len: usize,
90}
91
92enum Extend {
93    Trapped,
94    Advanced(usize),
95    Reached(usize),
96}
97
98impl<const N: usize> Tree<N> {
99    fn new() -> Self {
100        Self {
101            nodes: [[CuJointPosition::default(); N]; RRT_CONNECT_MAX_NODES],
102            parents: [u16::MAX; RRT_CONNECT_MAX_NODES],
103            len: 0,
104        }
105    }
106
107    fn reset(&mut self, root: [CuJointPosition; N]) {
108        self.nodes[0] = root;
109        self.parents[0] = u16::MAX;
110        self.len = 1;
111    }
112
113    fn nearest(&self, q: &[CuJointPosition; N], metric: CuJointMetric) -> usize {
114        let mut best = (0, f64::INFINITY);
115        for i in 0..self.len {
116            let d = joint_distance(&self.nodes[i], q, metric);
117            if d < best.1 {
118                best = (i, d);
119            }
120        }
121        best.0
122    }
123
124    /// One step from the nearest node toward `target`.
125    fn extend<const M: usize>(
126        &mut self,
127        validity: &CuSceneValidity<N, M>,
128        target: &[CuJointPosition; N],
129        params: &CuJointRrtConnectParams,
130    ) -> Extend {
131        if self.len >= params.max_nodes as usize {
132            return Extend::Trapped;
133        }
134        let near = self.nearest(target, params.metric);
135        let q_new = steer(&self.nodes[near], target, params.step_size, params.metric);
136        if !validity.is_segment_free(&self.nodes[near], &q_new) {
137            return Extend::Trapped;
138        }
139        let idx = self.len;
140        self.nodes[idx] = q_new;
141        self.parents[idx] = near as u16;
142        self.len += 1;
143        if joint_distance(&q_new, target, params.metric) <= 1e-9 {
144            Extend::Reached(idx)
145        } else {
146            Extend::Advanced(idx)
147        }
148    }
149
150    /// Repeated extension toward `target` until reached or blocked.
151    fn connect<const M: usize>(
152        &mut self,
153        validity: &CuSceneValidity<N, M>,
154        target: &[CuJointPosition; N],
155        params: &CuJointRrtConnectParams,
156    ) -> Option<usize> {
157        loop {
158            match self.extend(validity, target, params) {
159                Extend::Advanced(_) => {}
160                Extend::Reached(idx) => return Some(idx),
161                Extend::Trapped => return None,
162            }
163        }
164    }
165
166    /// Writes the chain from node `idx` to the root into `out`; returns its length.
167    fn chain_to_root(&self, mut idx: usize, out: &mut [[CuJointPosition; N]]) -> usize {
168        let mut len = 0;
169        loop {
170            out[len] = self.nodes[idx];
171            len += 1;
172            let parent = self.parents[idx];
173            if parent == u16::MAX || len >= out.len() {
174                return len;
175            }
176            idx = parent as usize;
177        }
178    }
179}
180
181/// Bidirectional RRT-Connect planner.
182///
183/// Built with [`CuJointPlanner::new`] from robot models and [`CuJointRrtConnectParams`];
184/// runs in [`CuJointRrtConnectTask`].
185pub struct CuJointRrtConnect<const N: usize, const M: usize = N> {
186    validity: CuSceneValidity<N, M>,
187    params: CuJointRrtConnectParams,
188    /// `trees[0]` grows from the start, `trees[1]` from the goal.
189    trees: [Tree<N>; 2],
190    active: usize,
191    rng: CuRng,
192    straight_cost: f64,
193    iterations: u32,
194    status: CuPlanStatus,
195    best: CuJointPlanPath<N>,
196}
197
198/// [`CuJointPlannerTask`] running [`CuJointRrtConnect`].
199pub type CuJointRrtConnectTask<const N: usize, const M: usize = N> =
200    CuJointPlannerTask<CuJointRrtConnect<N, M>, N, M>;
201
202impl<const N: usize, const M: usize> CuJointRrtConnect<N, M> {
203    /// The validated planner parameters.
204    pub const fn params(&self) -> &CuJointRrtConnectParams {
205        &self.params
206    }
207
208    fn build(
209        chain: CuKinematicChain<N>,
210        model: CuRobotCollisionModel<N, M>,
211        params: CuJointRrtConnectParams,
212    ) -> Self {
213        Self {
214            validity: CuSceneValidity::new(chain, model, params.collision_check_step),
215            params,
216            trees: [Tree::new(), Tree::new()],
217            active: 0,
218            rng: CuRng::from_seed(0),
219            straight_cost: 0.0,
220            iterations: 0,
221            status: CuPlanStatus::InvalidRequest,
222            best: CuJointPlanPath::default(),
223        }
224    }
225
226    /// Joins the trees at `start_idx` (start tree) and `goal_idx` (goal tree).
227    fn finish(&mut self, start_idx: usize, goal_idx: usize) {
228        let mut raw = [[CuJointPosition::default(); N]; 2 * RRT_CONNECT_MAX_NODES];
229        let head = self.trees[0].chain_to_root(start_idx, &mut raw);
230        raw[..head].reverse();
231        let tail = self.trees[1].chain_to_root(goal_idx, &mut raw[head..]);
232        let len = head + tail;
233        self.status = if write_shortcut(
234            &self.validity,
235            &raw[..len],
236            self.params.metric,
237            &mut self.best,
238        ) {
239            CuPlanStatus::Converged
240        } else {
241            CuPlanStatus::CapacityExceeded
242        };
243    }
244}
245
246impl<const N: usize, const M: usize> CuJointPlanner<N, M> for CuJointRrtConnect<N, M> {
247    const KIND: CuPlannerKind = CuPlannerKind::Sampling;
248
249    type Params = CuJointRrtConnectParams;
250
251    fn new(
252        chain: CuKinematicChain<N>,
253        collision_model: CuRobotCollisionModel<N, M>,
254        params: CuJointRrtConnectParams,
255    ) -> CuResult<Self> {
256        params.validate()?;
257        Ok(Self::build(chain, collision_model, params))
258    }
259
260    fn begin(&mut self, request: &CuJointPlanRequest<N>, seed: u64) -> CuResult<CuPlanStatus> {
261        self.rng.reseed(seed);
262        self.iterations = 0;
263        self.active = 0;
264        self.best = CuJointPlanPath::default();
265        self.straight_cost = joint_distance(&request.start, &request.goal, self.params.metric);
266        self.status = check_endpoints(&mut self.validity, request, self.params.metric);
267        if self.status != CuPlanStatus::Searching {
268            return Ok(self.status);
269        }
270        self.trees[0].reset(request.start);
271        self.trees[1].reset(request.goal);
272        if self.validity.is_segment_free(&request.start, &request.goal) {
273            self.trees[1].parents[0] = u16::MAX;
274            self.finish(0, 0);
275        }
276        Ok(self.status)
277    }
278
279    fn step(&mut self, budget: u32) -> CuResult<CuPlanStatus> {
280        for _ in 0..budget {
281            if self.status.is_final() {
282                break;
283            }
284            self.iterations += 1;
285            let (a, b) = (self.active, 1 - self.active);
286            self.active = b;
287            let sample = sample_uniform(self.validity.chain(), &mut self.rng);
288            let [first, second] = &mut self.trees;
289            let (grow, other) = if a == 0 {
290                (first, second)
291            } else {
292                (second, first)
293            };
294            let added = match grow.extend(&self.validity, &sample, &self.params) {
295                Extend::Advanced(idx) | Extend::Reached(idx) => idx,
296                Extend::Trapped => {
297                    let full = |t: &Tree<N>| t.len >= self.params.max_nodes as usize;
298                    if full(grow) && full(other) {
299                        self.status = CuPlanStatus::Exhausted;
300                    }
301                    continue;
302                }
303            };
304            let target = grow.nodes[added];
305            if let Some(joined) = other.connect(&self.validity, &target, &self.params) {
306                if a == 0 {
307                    self.finish(added, joined);
308                } else {
309                    self.finish(joined, added);
310                }
311            }
312        }
313        Ok(self.status)
314    }
315
316    fn write_best(&self, out: &mut CuJointPlanPath<N>) -> CuPlanStatus {
317        write_header(out, &self.validity, self.status, self.iterations);
318        if self.status.has_path() {
319            out.waypoints = self.best.waypoints.clone();
320            out.len = self.best.len;
321            out.cost = self.best.cost;
322        }
323        self.status
324    }
325
326    fn quality(&self) -> Quality {
327        length_quality(
328            self.straight_cost,
329            if self.status.has_path() {
330                self.best.cost
331            } else {
332                f64::INFINITY
333            },
334        )
335    }
336}
337
338crate::impl_planner_type_path!(
339    CuJointRrtConnect,
340    "repose_motion_planning::CuJointRrtConnect"
341);
repose_motion_planning/rrt_connect.rs

repose_motion_planning/
rrt_connect.rs
