repose_joint_rrt_star/
lib.rs

1//! Deterministic, allocation-free $N$-DOF joint-space RRT* for `copper-rs`.
2//!
3//! [`CuJointRrtStar`] is the sampling-based implementation of
4//! [`repose_motion_planning::CuJointPlanner`]: `begin` validates the request and
5//! seeds the tree (connecting start and goal directly when the line is free),
6//! and each `step` runs a bounded number of RRT* iterations with informed
7//! pruning, neighbourhood rewiring, and line-of-sight shortcutting on output.
8//! All randomness comes from the job seed, so replay reproduces every plan.
9//! [`CuJointRrtStarTask`] is the generic planner task running RRT*; its node
10//! config is [`repose_motion_planning::CuJointPlannerParams`] with a
11//! [`CuJointRrtStarParams`] `params` block:
12//!
13//! ```ron
14//! (
15//!     id: "planner",
16//!     type: "repose_joint_rrt_star::CuJointRrtStarTask<6>",
17//!     config: {
18//!         "chain": "igus_rebel",
19//!         "collision_model": "igus_rebel",
20//!         "params": {"step_size": 0.3, "goal_bias": 0.2, "max_nodes": 200},
21//!         "base_budget": 64,
22//!         "refine_budget": 32,
23//!     },
24//! )
25//! ```
26//!
27//! Request and path payloads ([`repose_motion_planning::CuJointPlanRequest`],
28//! [`repose_motion_planning::CuJointPlanPath`]) come from `repose-motion-planning`.
29
30#![cfg_attr(not(feature = "std"), no_std)]
31
32#[cfg(not(feature = "std"))]
33extern crate alloc;
34
35use bincode::Decode;
36use bincode::Encode;
37use cu_rng::prelude::*;
38use cu29::cutask_anytime::Quality;
39use cu29::cutask_anytime::quality_from_f32;
40use cu29::prelude::*;
41use repose_collision::CuRobotCollisionModel;
42use repose_collision::CuSceneExclusions;
43use repose_collision::CuSceneRevision;
44use repose_kinematics::CuJointMetric;
45use repose_kinematics::CuJointPosition;
46use repose_kinematics::CuJointPositionsPayload;
47use repose_kinematics::CuJointResolution;
48use repose_kinematics::CuKinematicChain;
49use repose_motion_planning::CuJointPlanPath;
50use repose_motion_planning::CuJointPlanRequest;
51use repose_motion_planning::CuJointPlanner;
52use repose_motion_planning::CuJointPlannerTask;
53use repose_motion_planning::CuPlanStatus;
54use repose_motion_planning::CuPlannerKind;
55use repose_motion_planning::CuSceneValidity;
56use repose_motion_planning::MAX_EXCLUSIONS;
57use repose_motion_planning::MAX_WAYPOINTS;
58use repose_motion_planning::joint_distance;
59use serde::Deserialize;
60use serde::Serialize;
61
62/// [`CuJointPlannerTask`] running [`CuJointRrtStar`].
63pub type CuJointRrtStarTask<const N: usize, const M: usize = N> =
64    CuJointPlannerTask<CuJointRrtStar<N, M>, N, M>;
65
66/// Fixed capacity of tree nodes in [`CuJointRrtStar`].
67pub const RRT_STAR_MAX_NODES: usize = 256;
68const CONVERGED_QUALITY: f32 = 0.999;
69
70fn is_converged_quality(q: Quality) -> bool {
71    q.raw() >= CONVERGED_QUALITY
72}
73
74/// Parameters of [`CuJointRrtStar`].
75#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize, Encode, Decode, Reflect)]
76#[serde(default, deny_unknown_fields)]
77pub struct CuJointRrtStarParams {
78    /// Maximum joint-space step per extension (dimensionless, metric-scaled).
79    pub step_size: f64,
80    /// Probability in `[0.0, 1.0]` of sampling the exact goal configuration
81    /// (dimensionless).
82    pub goal_bias: f64,
83    /// Joint-space distance within which a node can connect directly to the
84    /// goal (dimensionless, metric-scaled).
85    pub goal_threshold: f64,
86    /// Neighbourhood rewiring radius (dimensionless, metric-scaled).
87    pub rewire_radius: f64,
88    /// Angular/linear resolution for discrete edge collision sampling.
89    pub collision_check_step: CuJointResolution,
90    /// Maximum active tree nodes (`>= 2`; capped at [`RRT_STAR_MAX_NODES`]).
91    pub max_nodes: u32,
92    /// Scaling of dimensionless joint-space distances and costs.
93    pub metric: CuJointMetric,
94}
95
96impl Default for CuJointRrtStarParams {
97    fn default() -> Self {
98        Self {
99            step_size: 0.35,
100            goal_bias: 0.15,
101            goal_threshold: 0.40,
102            rewire_radius: 0.75,
103            collision_check_step: CuJointResolution::default(),
104            max_nodes: RRT_STAR_MAX_NODES as u32,
105            metric: CuJointMetric::default(),
106        }
107    }
108}
109
110impl CuJointRrtStarParams {
111    /// Checks every parameter, the metric, and the collision-check resolution.
112    pub fn validate(&self) -> CuResult<()> {
113        self.metric.validate()?;
114        self.collision_check_step.metric().validate()?;
115        let positive = |v: f64| v.is_finite() && v > 0.0;
116        if !positive(self.step_size)
117            || !positive(self.rewire_radius)
118            || !positive(self.goal_threshold)
119        {
120            return Err(CuError::from(
121                "repose-joint-rrt-star: 'step_size', 'rewire_radius', and 'goal_threshold' must be positive and finite",
122            ));
123        }
124        if !(self.goal_bias.is_finite() && (0.0..=1.0).contains(&self.goal_bias)) {
125            return Err(CuError::from(
126                "repose-joint-rrt-star: 'goal_bias' must be in [0, 1]",
127            ));
128        }
129        if self.max_nodes < 2 {
130            return Err(CuError::from(
131                "repose-joint-rrt-star: 'max_nodes' must be >= 2",
132            ));
133        }
134        Ok(())
135    }
136}
137
138#[derive(Clone, Copy)]
139struct RrtNode<const N: usize> {
140    q: [CuJointPosition; N],
141    parent: u16,
142    cost: f64,
143}
144
145impl<const N: usize> Default for RrtNode<N> {
146    fn default() -> Self {
147        Self {
148            q: [CuJointPosition::default(); N],
149            parent: u16::MAX,
150            cost: 0.0,
151        }
152    }
153}
154
155/// Allocation-free fixed-capacity joint-space RRT* solver.
156///
157/// Built with [`CuJointPlanner::new`] from robot models and
158/// [`CuJointRrtStarParams`] (invalid parameters fail there); runs in
159/// [`CuJointRrtStarTask`].
160pub struct CuJointRrtStar<const N: usize, const M: usize = N> {
161    validity: CuSceneValidity<N, M>,
162    params: CuJointRrtStarParams,
163    start: [CuJointPosition; N],
164    goal: [CuJointPosition; N],
165    straight_line_cost: f64,
166    nodes: [RrtNode<N>; RRT_STAR_MAX_NODES],
167    node_count: usize,
168    goal_node_idx: Option<usize>,
169    best_cost: f64,
170    iterations: u32,
171    rng: CuRng,
172    status: CuPlanStatus,
173}
174
175impl<const N: usize, const M: usize> CuJointRrtStar<N, M> {
176    /// The validated planner parameters.
177    pub const fn params(&self) -> &CuJointRrtStarParams {
178        &self.params
179    }
180
181    /// Updates the job status after tree changes.
182    fn refresh_status(&mut self) {
183        if self.status.is_final() && !self.status.has_path() {
184            return;
185        }
186        let cap = (self.params.max_nodes as usize).min(RRT_STAR_MAX_NODES);
187        let done = self.node_count >= cap || is_converged_quality(self.quality());
188        self.status = match (self.has_solution(), done) {
189            (true, true) => CuPlanStatus::Converged,
190            (true, false) => CuPlanStatus::Improving,
191            (false, true) => CuPlanStatus::Exhausted,
192            (false, false) => CuPlanStatus::Searching,
193        };
194    }
195
196    /// Runs `budget` bounded RRT* iterations in place.
197    pub fn grow(&mut self, budget: u32) {
198        if self.status.is_final() {
199            return;
200        }
201        let cap = (self.params.max_nodes as usize).min(RRT_STAR_MAX_NODES);
202        let step = self.params.step_size;
203        let goal_threshold = self.params.goal_threshold;
204        let rewire_radius = self.params.rewire_radius;
205
206        let mut it = 0;
207        while it < budget {
208            if self.node_count >= cap {
209                break;
210            }
211            self.iterations += 1;
212            it += 1;
213
214            let q_sample = self.sample_config();
215            let nearest_idx = self.nearest_node(&q_sample);
216            let q_new = steer(
217                &self.nodes[nearest_idx].q,
218                &q_sample,
219                step,
220                self.params.metric,
221            );
222
223            // Informed pruning: skip configurations that cannot possibly improve best_cost
224            let min_possible = joint_distance(&self.start, &q_new, self.params.metric)
225                + joint_distance(&q_new, &self.goal, self.params.metric);
226            if min_possible >= self.best_cost {
227                continue;
228            }
229
230            if !self.validity.is_configuration_free(&q_new) {
231                continue;
232            }
233
234            // Choose best parent inside `rewire_radius` neighborhood
235            let mut best_parent = nearest_idx;
236            let mut best_new_cost = self.nodes[nearest_idx].cost
237                + joint_distance(&self.nodes[nearest_idx].q, &q_new, self.params.metric);
238
239            if !self
240                .validity
241                .is_segment_free(&self.nodes[nearest_idx].q, &q_new)
242            {
243                continue;
244            }
245
246            let mut idx = 0;
247            while idx < self.node_count {
248                if idx != nearest_idx {
249                    let d = joint_distance(&self.nodes[idx].q, &q_new, self.params.metric);
250                    if d <= rewire_radius {
251                        let cand_cost = self.nodes[idx].cost + d;
252                        if cand_cost < best_new_cost
253                            && self.validity.is_segment_free(&self.nodes[idx].q, &q_new)
254                        {
255                            best_parent = idx;
256                            best_new_cost = cand_cost;
257                        }
258                    }
259                }
260                idx += 1;
261            }
262
263            let new_idx = self.node_count;
264            self.nodes[new_idx] = RrtNode {
265                q: q_new,
266                parent: best_parent as u16,
267                cost: best_new_cost,
268            };
269            self.node_count += 1;
270
271            // Rewire neighbors through `new_idx` if cheaper. Skipping ancestors of
272            // `new_idx` keeps the parent links acyclic.
273            let mut rewired = false;
274            let mut nbr = 0;
275            while nbr < new_idx {
276                if nbr != best_parent {
277                    let d = joint_distance(&q_new, &self.nodes[nbr].q, self.params.metric);
278                    if d <= rewire_radius
279                        && best_new_cost + d < self.nodes[nbr].cost
280                        && !self.is_ancestor_of(nbr, new_idx)
281                        && self.validity.is_segment_free(&q_new, &self.nodes[nbr].q)
282                    {
283                        self.nodes[nbr].parent = new_idx as u16;
284                        self.nodes[nbr].cost = best_new_cost + d;
285                        rewired = true;
286                    }
287                }
288                nbr += 1;
289            }
290
291            // Propagate rewired costs to descendants so tree costs stay consistent.
292            if rewired {
293                let mut pass = 0;
294                loop {
295                    let mut changed = false;
296                    let mut i = 0;
297                    while i < self.node_count {
298                        let p = self.nodes[i].parent;
299                        if p != u16::MAX {
300                            let pu = p as usize;
301                            let c = self.nodes[pu].cost
302                                + joint_distance(
303                                    &self.nodes[pu].q,
304                                    &self.nodes[i].q,
305                                    self.params.metric,
306                                );
307                            if c < self.nodes[i].cost - 1e-12 {
308                                self.nodes[i].cost = c;
309                                changed = true;
310                            }
311                        }
312                        i += 1;
313                    }
314                    pass += 1;
315                    if !changed || pass >= self.node_count {
316                        break;
317                    }
318                }
319                if let Some(g) = self.goal_node_idx
320                    && self.nodes[g].cost < self.best_cost
321                {
322                    self.best_cost = self.nodes[g].cost;
323                }
324            }
325
326            // Try connecting `new_idx` to `goal` if within `goal_threshold` and capacity allows
327            let dist_to_goal = joint_distance(&q_new, &self.goal, self.params.metric);
328            if dist_to_goal <= goal_threshold
329                && best_new_cost + dist_to_goal < self.best_cost
330                && self.validity.is_segment_free(&q_new, &self.goal)
331            {
332                if let Some(g_idx) = self.goal_node_idx {
333                    self.nodes[g_idx].parent = new_idx as u16;
334                    self.nodes[g_idx].cost = best_new_cost + dist_to_goal;
335                    self.best_cost = best_new_cost + dist_to_goal;
336                } else if self.node_count < cap {
337                    let g_idx = self.node_count;
338                    self.nodes[g_idx] = RrtNode {
339                        q: self.goal,
340                        parent: new_idx as u16,
341                        cost: best_new_cost + dist_to_goal,
342                    };
343                    self.node_count += 1;
344                    self.goal_node_idx = Some(g_idx);
345                    self.best_cost = best_new_cost + dist_to_goal;
346                }
347            }
348        }
349        self.refresh_status();
350    }
351
352    /// Returns `true` if a collision-free path from `start` to `goal` has been found.
353    pub const fn has_solution(&self) -> bool {
354        self.goal_node_idx.is_some()
355    }
356
357    /// Returns `true` if the node buffer is full or the path is already straight-line optimal.
358    pub fn is_exhausted(&self) -> bool {
359        let cap = (self.params.max_nodes as usize).min(RRT_STAR_MAX_NODES);
360        self.status.is_final() || self.node_count >= cap || is_converged_quality(self.quality())
361    }
362
363    /// Returns the best joint-space path cost found so far.
364    pub fn best_cost(&self) -> f64 {
365        self.best_cost
366    }
367
368    /// Returns the number of RRT* iterations executed for the current job.
369    pub const fn iterations(&self) -> u32 {
370        self.iterations
371    }
372
373    /// Returns the number of nodes currently in the tree.
374    pub const fn tree_size(&self) -> u32 {
375        self.node_count as u32
376    }
377
378    /// Returns the scene revision captured for the current job.
379    pub const fn scene_revision(&self) -> CuSceneRevision {
380        self.validity.scene_revision()
381    }
382
383    /// Exclusions captured for the current job.
384    pub const fn exclusions(&self) -> &CuSceneExclusions<MAX_EXCLUSIONS> {
385        self.validity.exclusions()
386    }
387
388    /// Computes the normalized quality in `[0.0, 1.0]` (`straight_line_cost / best_cost`).
389    pub fn quality(&self) -> Quality {
390        self.quality_of_cost(self.best_cost)
391    }
392
393    /// Computes the normalized quality in `[0.0, 1.0]` of an actually emitted path
394    /// (`straight_line_cost / cost`).
395    pub fn quality_of_cost(&self, cost: f64) -> Quality {
396        if !self.has_solution() || !cost.is_finite() {
397            return quality_from_f32(0.0);
398        }
399        if self.straight_line_cost <= 1e-9 || cost <= 1e-9 {
400            return quality_from_f32(1.0);
401        }
402        let ratio = (self.straight_line_cost / cost).clamp(0.0, 1.0);
403        quality_from_f32(ratio as f32)
404    }
405
406    /// Dimensionless joint-space L2 cost of an emitted waypoint sequence
407    /// (`waypoints[..len]`) using the configured metric.
408    pub fn path_cost(
409        &self,
410        waypoints: &[CuJointPositionsPayload<N>; MAX_WAYPOINTS],
411        len: u32,
412    ) -> f64 {
413        let mut total = 0.0_f64;
414        let mut i = 0usize;
415        while i + 1 < len as usize {
416            total += joint_distance(
417                &waypoints[i].positions,
418                &waypoints[i + 1].positions,
419                self.params.metric,
420            );
421            i += 1;
422        }
423        total
424    }
425
426    /// Extracts and shortcuts the best path in place into `out_waypoints`, returning the
427    /// number of waypoints written (`Some(len)`), or `None` if no solution exists or the
428    /// shortcut path still exceeds `MAX_WAYPOINTS`.
429    pub fn write_path(
430        &self,
431        out_waypoints: &mut [CuJointPositionsPayload<N>; MAX_WAYPOINTS],
432    ) -> Option<u32> {
433        let mut anchor = self.goal_node_idx?;
434
435        // Greedy line-of-sight shortcutting while walking the parent chain from the goal
436        // back to the start, so raw paths longer than MAX_WAYPOINTS are still shortcut
437        // before any capacity rejection. `shortcut[..count]` runs from goal back to start.
438        let mut shortcut = [[CuJointPosition::default(); N]; MAX_WAYPOINTS];
439        let mut count = 0usize;
440        shortcut[count] = self.nodes[anchor].q;
441        count += 1;
442
443        let mut steps = 0;
444        loop {
445            let parent = self.nodes[anchor].parent;
446            if parent == u16::MAX {
447                break; // `anchor` is the start node
448            }
449            steps += 1;
450            if steps > self.node_count {
451                return None; // defensive: parent chain never reached the root
452            }
453            // Farthest ancestor toward the start reachable from `anchor` in a straight line.
454            let mut chosen = parent as usize;
455            let mut curr = parent as usize;
456            let mut hops = 0;
457            while hops <= self.node_count {
458                hops += 1;
459                let up = self.nodes[curr].parent;
460                if up == u16::MAX {
461                    break;
462                }
463                if self
464                    .validity
465                    .is_segment_free(&self.nodes[anchor].q, &self.nodes[up as usize].q)
466                {
467                    chosen = up as usize;
468                }
469                curr = up as usize;
470            }
471            if count >= MAX_WAYPOINTS {
472                return None;
473            }
474            shortcut[count] = self.nodes[chosen].q;
475            count += 1;
476            anchor = chosen;
477        }
478
479        // Reverse goal->start into start->goal.
480        let mut i = 0;
481        while i < count {
482            out_waypoints[i] = CuJointPositionsPayload {
483                positions: shortcut[count - 1 - i],
484            };
485            i += 1;
486        }
487        Some(count as u32)
488    }
489
490    fn sample_config(&mut self) -> [CuJointPosition; N] {
491        let coin: f64 = self.rng.random();
492        if coin < self.params.goal_bias {
493            return self.goal;
494        }
495        let joints = self.validity.chain().joints();
496        let mut q = [CuJointPosition::default(); N];
497        let mut j = 0;
498        while j < N {
499            let lo = joints[j].limits.lower().si_value();
500            let hi = joints[j].limits.upper().si_value();
501            let u: f64 = self.rng.random();
502            q[j] = joints[j].limits.lower().with_si_value(lo + u * (hi - lo));
503            j += 1;
504        }
505        q
506    }
507
508    /// Returns `true` when `maybe_ancestor` lies on the parent chain of `node`.
509    fn is_ancestor_of(&self, maybe_ancestor: usize, node: usize) -> bool {
510        let mut curr = node;
511        let mut guard = 0;
512        while curr != u16::MAX as usize && guard <= self.node_count {
513            if curr == maybe_ancestor {
514                return true;
515            }
516            curr = self.nodes[curr].parent as usize;
517            guard += 1;
518        }
519        false
520    }
521
522    fn nearest_node(&self, q_target: &[CuJointPosition; N]) -> usize {
523        let mut best_idx = 0;
524        let mut best_dist = f64::INFINITY;
525        let mut i = 0;
526        while i < self.node_count {
527            // Do not expand out of the goal node itself
528            if Some(i) != self.goal_node_idx {
529                let d = joint_distance(&self.nodes[i].q, q_target, self.params.metric);
530                if d < best_dist {
531                    best_dist = d;
532                    best_idx = i;
533                }
534            }
535            i += 1;
536        }
537        best_idx
538    }
539}
540
541fn steer<const N: usize>(
542    from: &[CuJointPosition; N],
543    to: &[CuJointPosition; N],
544    max_step: f64,
545    metric: CuJointMetric,
546) -> [CuJointPosition; N] {
547    let dist = joint_distance(from, to, metric);
548    if dist <= max_step || dist <= 1e-12 {
549        return *to;
550    }
551    let scale = max_step / dist;
552    let mut out = [CuJointPosition::default(); N];
553    let mut i = 0;
554    while i < N {
555        let a = from[i].si_value();
556        let b = to[i].si_value();
557        out[i] = from[i].with_si_value(a + scale * (b - a));
558        i += 1;
559    }
560    out
561}
562
563impl<const N: usize, const M: usize> CuJointPlanner<N, M> for CuJointRrtStar<N, M> {
564    const KIND: CuPlannerKind = CuPlannerKind::Sampling;
565
566    type Params = CuJointRrtStarParams;
567
568    fn new(
569        chain: CuKinematicChain<N>,
570        collision_model: CuRobotCollisionModel<N, M>,
571        params: CuJointRrtStarParams,
572    ) -> CuResult<Self> {
573        params.validate()?;
574        Ok(Self {
575            validity: CuSceneValidity::new(chain, collision_model, params.collision_check_step),
576            params,
577            start: [CuJointPosition::default(); N],
578            goal: [CuJointPosition::default(); N],
579            straight_line_cost: 0.0,
580            nodes: [RrtNode::default(); RRT_STAR_MAX_NODES],
581            node_count: 0,
582            goal_node_idx: None,
583            best_cost: f64::INFINITY,
584            iterations: 0,
585            rng: CuRng::from_seed(0),
586            status: CuPlanStatus::InvalidRequest,
587        })
588    }
589
590    fn begin(&mut self, request: &CuJointPlanRequest<N>, seed: u64) -> CuResult<CuPlanStatus> {
591        self.start = request.start;
592        self.goal = request.goal;
593        self.straight_line_cost = joint_distance(&self.start, &self.goal, self.params.metric);
594        self.node_count = 0;
595        self.goal_node_idx = None;
596        self.best_cost = f64::INFINITY;
597        self.iterations = 0;
598        self.rng.reseed(seed);
599        self.status = if !self.validity.capture(&request.scene, &request.exclusions) {
600            CuPlanStatus::InvalidRequest
601        } else if !self.validity.is_configuration_free(&self.start) {
602            CuPlanStatus::InvalidStart
603        } else if !self.validity.is_configuration_free(&self.goal) {
604            CuPlanStatus::InvalidGoal
605        } else {
606            CuPlanStatus::Searching
607        };
608        if self.status != CuPlanStatus::Searching {
609            return Ok(self.status);
610        }
611        self.nodes[0] = RrtNode {
612            q: self.start,
613            parent: u16::MAX,
614            cost: 0.0,
615        };
616        self.node_count = 1;
617        if self.straight_line_cost <= 1e-9 {
618            self.goal_node_idx = Some(0);
619            self.best_cost = 0.0;
620        } else if self.validity.is_segment_free(&self.start, &self.goal) {
621            // Direct line of sight: the straight line is optimal.
622            self.nodes[1] = RrtNode {
623                q: self.goal,
624                parent: 0,
625                cost: self.straight_line_cost,
626            };
627            self.node_count = 2;
628            self.goal_node_idx = Some(1);
629            self.best_cost = self.straight_line_cost;
630        }
631        self.refresh_status();
632        Ok(self.status)
633    }
634
635    fn step(&mut self, budget: u32) -> CuResult<CuPlanStatus> {
636        self.grow(budget);
637        Ok(self.status)
638    }
639
640    fn write_best(&self, out: &mut CuJointPlanPath<N>) -> CuPlanStatus {
641        out.len = 0;
642        out.cost = f64::INFINITY;
643        out.effort = self.iterations;
644        out.status = self.status;
645        out.scene_revision = self.scene_revision();
646        out.exclusions = self.exclusions().clone();
647        if !self.status.has_path() {
648            return self.status;
649        }
650        match self.write_path(&mut out.waypoints) {
651            Some(len) => {
652                out.len = len;
653                out.cost = self.path_cost(&out.waypoints, len);
654            }
655            None => out.status = CuPlanStatus::CapacityExceeded,
656        }
657        out.status
658    }
659
660    fn quality(&self) -> Quality {
661        CuJointRrtStar::quality(self)
662    }
663}
664
665#[cfg(test)]
666mod tests {
667    use super::*;
668    use cu_spatial_payloads::Transform3D;
669    use cu29::cutask_anytime::AnytimeStatus;
670    use cu29::cutask_anytime::CuAnytimeTask;
671    use cu29::units::si::angle::radian;
672    use cu29::units::si::f64::Angle as Angle64;
673    use cu29::units::si::f64::Length;
674    use cu29::units::si::length::meter;
675    use repose_collision::CuCollisionShape;
676    use repose_collision::CuPlanningScene;
677    use repose_collision::CuSphere;
678    use repose_collision::igus_rebel_collision_model;
679    use repose_kinematics::igus_rebel_chain;
680    use repose_motion_planning::MAX_ATTACHED_SHAPES;
681    use repose_motion_planning::MAX_OBSTACLES;
682    use repose_motion_planning::planner_resources;
683
684    #[test]
685    fn planner_initialization_requires_explicit_serial_models() {
686        let resources = || planner_resources::Resources {
687            rng: Owned(CuRng::from_seed(1)),
688        };
689        assert!(CuJointRrtStarTask::<1>::new(None, resources()).is_err());
690        let cfg: ComponentConfig = ron::from_str(r#"#![enable(unwrap_newtypes)]
691{"chain": {"joints": [{"joint_type": "revolute", "origin_translation": [0.0, 0.0, 0.0], "origin_rpy": [0.0, 0.0, 0.0], "axis": "pos_z", "limits": {"revolute": {"lower": -3.0, "upper": 3.0, "max_velocity": 1.0}}}]},"collision_model": {"shapes": [{"shape": {"sphere": {"center": [0.0, 0.0, 0.0], "radius": 0.1}}, "frame": 0}], "safety_margin": 0.01}}"#).unwrap();
692        let mut task = CuJointRrtStarTask::<1>::new(Some(&cfg), resources()).unwrap();
693        let (ctx, _) = CuContext::new_mock_clock();
694        let request = CuMsg::new(Some(CuJointPlanRequest {
695            scene: CuPlanningScene::new(),
696            exclusions: CuSceneExclusions::new(),
697            start: [Angle64::new::<radian>(0.0).into()],
698            goal: [Angle64::new::<radian>(0.5).into()],
699        }));
700        let mut output = CuMsg::new(None);
701        task.base(&ctx, &request, &mut output).unwrap();
702        assert_eq!(output.payload().unwrap().len, 2);
703    }
704
705    fn test_task(seed: u64) -> CuJointRrtStarTask<6> {
706        CuJointRrtStarTask::from_planner(
707            CuJointRrtStar::new(
708                igus_rebel_chain(),
709                igus_rebel_collision_model(),
710                CuJointRrtStarParams::default(),
711            )
712            .unwrap(),
713            64,
714            32,
715            seed,
716        )
717        .unwrap()
718    }
719
720    #[test]
721    fn preset_models_and_nested_params_configure_the_task() {
722        let resources = || planner_resources::Resources {
723            rng: Owned(CuRng::from_seed(1)),
724        };
725        let cfg: ComponentConfig = ron::from_str(
726            r#"#![enable(unwrap_newtypes)] {"chain": "igus_rebel", "collision_model": "igus_rebel",
727                "params": {"step_size": 0.3, "goal_bias": 0.2}, "refine_budget": 8}"#,
728        )
729        .unwrap();
730        let task = CuJointRrtStarTask::<6>::new(Some(&cfg), resources()).unwrap();
731        assert_eq!(task.planner().params().step_size, 0.3);
732        assert_eq!(task.planner().params().goal_bias, 0.2);
733        // Invalid parameters fail at construction, not at the first job.
734        for bad in [
735            r#"{"chain": "igus_rebel", "collision_model": "igus_rebel", "params": {"goal_bias": 1.5}}"#,
736            r#"{"chain": "igus_rebel", "collision_model": "igus_rebel", "params": {"step_size": 0.0}}"#,
737            r#"{"chain": "igus_rebel", "collision_model": "igus_rebel", "params": {"stepsize": 0.3}}"#,
738            r#"{"chain": "igus_rebel", "collision_model": "igus_rebel", "step_size": 0.3}"#,
739            r#"{"chain": "igus_rebel", "collision_model": "igus_rebel", "base_budget": 0}"#,
740            r#"{"chain": "igus_rebel"}"#,
741        ] {
742            let cfg: ComponentConfig =
743                ron::from_str(&format!("#![enable(unwrap_newtypes)] {bad}")).unwrap();
744            assert!(
745                CuJointRrtStarTask::<6>::new(Some(&cfg), resources()).is_err(),
746                "{bad}"
747            );
748        }
749        let invalid = CuJointRrtStarParams {
750            goal_bias: -0.1,
751            ..Default::default()
752        };
753        assert!(invalid.validate().is_err());
754        assert!(
755            CuJointRrtStar::<6>::new(igus_rebel_chain(), igus_rebel_collision_model(), invalid)
756                .is_err()
757        );
758    }
759
760    #[test]
761    fn identical_seeds_reproduce_identical_paths() {
762        let (ctx, _mock) = CuContext::new_mock_clock();
763        let run = |seed| {
764            let mut task = test_task(seed);
765            let req = CuMsg::new(Some(obstructed_request()));
766            let mut out = CuMsg::new(None);
767            task.base(&ctx, &req, &mut out).unwrap();
768            for _ in 0..4 {
769                task.refine(&ctx, &mut out).unwrap();
770            }
771            out.payload().cloned()
772        };
773        assert_eq!(run(11), run(11));
774    }
775
776    #[test]
777    fn test_unobstructed_joint_plan_converges_in_base() {
778        let (ctx, _mock) = CuContext::new_mock_clock();
779        let mut task = test_task(42);
780
781        let start = [Angle64::new::<radian>(0.0).into(); 6];
782        let goal = [
783            Angle64::new::<radian>(0.5).into(),
784            Angle64::new::<radian>(0.3).into(),
785            Angle64::new::<radian>(-0.3).into(),
786            Angle64::new::<radian>(0.2).into(),
787            Angle64::new::<radian>(-0.2).into(),
788            Angle64::new::<radian>(0.4).into(),
789        ];
790
791        let req = CuMsg::new(Some(CuJointPlanRequest {
792            scene: CuPlanningScene::new(),
793            exclusions: CuSceneExclusions::new(),
794            start,
795            goal,
796        }));
797        let mut out = CuMsg::new(None);
798
799        let status = task.base(&ctx, &req, &mut out).unwrap();
800        assert!(matches!(status, AnytimeStatus::Converged(_)));
801        let path = out.payload().unwrap();
802        assert_eq!(path.len, 2);
803        assert_eq!(path.waypoints[0].positions, start);
804        assert_eq!(path.waypoints[1].positions, goal);
805    }
806
807    fn obstructed_request() -> CuJointPlanRequest<6> {
808        let start = [
809            Angle64::new::<radian>(-0.6).into(),
810            Angle64::new::<radian>(0.7).into(),
811            Angle64::new::<radian>(-0.2).into(),
812            Angle64::new::<radian>(0.0).into(),
813            Angle64::new::<radian>(0.0).into(),
814            Angle64::new::<radian>(0.0).into(),
815        ];
816        let goal = [
817            Angle64::new::<radian>(0.6).into(),
818            Angle64::new::<radian>(0.7).into(),
819            Angle64::new::<radian>(-0.2).into(),
820            Angle64::new::<radian>(0.0).into(),
821            Angle64::new::<radian>(0.0).into(),
822            Angle64::new::<radian>(0.0).into(),
823        ];
824
825        // Midpoint q_mid = [0.0, 0.7, -0.2, 0, 0, 0]: compute its end-effector position and place
826        // an obstacle sphere there so the straight-line joint path collides, forcing RRT* to route around!
827        let q_mid = [
828            Angle64::new::<radian>(0.0).into(),
829            Angle64::new::<radian>(0.7).into(),
830            Angle64::new::<radian>(-0.2).into(),
831            Angle64::new::<radian>(0.0).into(),
832            Angle64::new::<radian>(0.0).into(),
833            Angle64::new::<radian>(0.0).into(),
834        ];
835        let mut mid_ee = Transform3D::<f64>::identity();
836        igus_rebel_chain()
837            .forward_kinematics(&q_mid, &mut mid_ee)
838            .unwrap();
839        let m = mid_ee.to_matrix();
840
841        let mut scene = CuPlanningScene::<MAX_OBSTACLES, MAX_ATTACHED_SHAPES>::new();
842        scene
843            .add_obstacle(
844                CuCollisionShape::Sphere(
845                    CuSphere::new(
846                        [m[0][3], m[1][3], m[2][3]].map(Length::new::<meter>),
847                        Length::new::<meter>(0.08),
848                    )
849                    .unwrap(),
850                ),
851                None,
852            )
853            .unwrap();
854
855        CuJointPlanRequest {
856            scene,
857            exclusions: CuSceneExclusions::new(),
858            start,
859            goal,
860        }
861    }
862
863    #[test]
864    fn test_obstructed_joint_plan_refines_around_obstacle() {
865        let (ctx, _mock) = CuContext::new_mock_clock();
866        let mut task = test_task(7);
867        let req = CuMsg::new(Some(obstructed_request()));
868        let mut out = CuMsg::new(None);
869
870        let status = task.base(&ctx, &req, &mut out).unwrap();
871        assert!(!matches!(status, AnytimeStatus::Aborted));
872
873        let mut best_cost = f64::INFINITY;
874        for _ in 0..8 {
875            let st = task.refine(&ctx, &mut out).unwrap();
876            assert!(!matches!(st, AnytimeStatus::Aborted));
877            if let Some(p) = out.payload() {
878                let c = p.cost;
879                assert!(c <= best_cost + 1e-9);
880                best_cost = c;
881            }
882        }
883
884        let path = out
885            .payload()
886            .expect("RRT* should find a collision-free path around the obstacle");
887        assert!(path.len >= 3);
888
889        // Verify every segment of the published path is collision-free
890        let model = igus_rebel_collision_model();
891        let chain = igus_rebel_chain();
892        let verify_env = req.payload().unwrap().scene.collision_env();
893        for w in 0..(path.len as usize - 1) {
894            assert!(
895                model
896                    .is_segment_collision_free(
897                        &chain,
898                        &path.waypoints[w].positions,
899                        &path.waypoints[w + 1].positions,
900                        Angle64::new::<radian>(0.05),
901                        &verify_env,
902                    )
903                    .unwrap()
904            );
905        }
906    }
907}
908
909repose_motion_planning::impl_planner_type_path!(
910    CuJointRrtStar,
911    "repose_joint_rrt_star::CuJointRrtStar"
912);
repose_joint_rrt_star/lib.rs

repose_joint_rrt_star/
lib.rs
