repose/
planners.rs

1//! The one registry of Repose joint planners: [`CuPlannerChoice`] names every
2//! planner (its RON spelling) and [`CuAnyPlanner`] is a [`CuJointPlanner`]
3//! that runs whichever one is chosen, directly or inside
4//! `CuJointPlannerTask<CuAnyPlanner<N>, N>` with `"params": "rrt_star"`. Adding a planner means implementing the
5//! trait and adding one line to the `planners!` list below; applications
6//! select it by name.
7
8#[cfg(not(feature = "std"))]
9extern crate alloc;
10#[cfg(not(feature = "std"))]
11use alloc::boxed::Box;
12
13use cu29::cutask_anytime::Quality;
14use cu29::prelude::*;
15use repose_collision::CuRobotCollisionModel;
16use repose_joint_rrt_star::CuJointRrtStar;
17use repose_kinematics::CuKinematicChain;
18use repose_motion_planning::CuJointAStar;
19use repose_motion_planning::CuJointDirect;
20use repose_motion_planning::CuJointPathOptimizer;
21use repose_motion_planning::CuJointPlanPath;
22use repose_motion_planning::CuJointPlanRequest;
23use repose_motion_planning::CuJointPlanner;
24use repose_motion_planning::CuJointPrm;
25use repose_motion_planning::CuJointRrtConnect;
26use repose_motion_planning::CuJointStomp;
27use repose_motion_planning::CuPlanStatus;
28use repose_motion_planning::CuPlannerKind;
29use serde::Deserialize;
30use serde::Serialize;
31
32/// Generates [`CuPlannerChoice`], [`CuAnyPlanner`] and all their dispatch from
33/// one list, so a planner is added in exactly one line.
34macro_rules! planners {
35    ($($(#[$doc:meta])* $variant:ident => $ty:ident),+ $(,)?) => {
36        /// Every Repose joint planner, by RON name (snake_case of the variant).
37        #[derive(Debug, Clone, Copy, Default, PartialEq, Eq, Serialize, Deserialize, Reflect)]
38        #[serde(rename_all = "snake_case")]
39        pub enum CuPlannerChoice {
40            #[default]
41            $($(#[$doc])* $variant,)+
42        }
43
44        impl CuPlannerChoice {
45            /// Every variant, in declaration order.
46            pub const ALL: &'static [Self] = &[$(Self::$variant),+];
47        }
48
49        /// Any Repose joint planner with default parameters, chosen at runtime.
50        pub enum CuAnyPlanner<const N: usize, const M: usize = N> {
51            $($(#[$doc])* $variant(Box<$ty<N, M>>),)+
52        }
53
54        impl<const N: usize, const M: usize> CuAnyPlanner<N, M> {
55            /// Which planner this is.
56            pub fn choice(&self) -> CuPlannerChoice {
57                match self {
58                    $(Self::$variant(_) => CuPlannerChoice::$variant,)+
59                }
60            }
61        }
62
63        impl<const N: usize, const M: usize> CuJointPlanner<N, M> for CuAnyPlanner<N, M> {
64            /// Varies with the choice; reported as sampling.
65            const KIND: CuPlannerKind = CuPlannerKind::Sampling;
66
67            /// The planner to run, by name.
68            type Params = CuPlannerChoice;
69
70            fn new(
71                chain: CuKinematicChain<N>,
72                collision_model: CuRobotCollisionModel<N, M>,
73                params: CuPlannerChoice,
74            ) -> CuResult<Self> {
75                Ok(match params {
76                    $(CuPlannerChoice::$variant => Self::$variant(Box::new(
77                        CuJointPlanner::new(chain, collision_model, Default::default())?,
78                    )),)+
79                })
80            }
81
82            fn begin(&mut self, request: &CuJointPlanRequest<N>, seed: u64) -> CuResult<CuPlanStatus> {
83                match self {
84                    $(Self::$variant(p) => p.begin(request, seed),)+
85                }
86            }
87
88            fn step(&mut self, budget: u32) -> CuResult<CuPlanStatus> {
89                match self {
90                    $(Self::$variant(p) => p.step(budget),)+
91                }
92            }
93
94            fn write_best(&self, out: &mut CuJointPlanPath<N>) -> CuPlanStatus {
95                match self {
96                    $(Self::$variant(p) => p.write_best(out),)+
97                }
98            }
99
100            fn quality(&self) -> Quality {
101                match self {
102                    $(Self::$variant(p) => p.quality(),)+
103                }
104            }
105        }
106    };
107}
108
109repose_motion_planning::impl_planner_type_path!(CuAnyPlanner, "repose::planners::CuAnyPlanner");
110
111// The single list of planners. The first entry is the default.
112planners! {
113    /// Bidirectional RRT, feasibility-first (`rrt_connect`).
114    RrtConnect => CuJointRrtConnect,
115    /// Asymptotically optimal RRT* with rewiring and shortcutting (`rrt_star`).
116    RrtStar => CuJointRrtStar,
117    /// Probabilistic roadmap (`prm`).
118    Prm => CuJointPrm,
119    /// A* on a joint-space lattice (`a_star`).
120    AStar => CuJointAStar,
121    /// Stochastic trajectory optimization (`stomp`).
122    Stomp => CuJointStomp,
123    /// Gradient path optimizer, CHOMP-style (`path_optimizer`).
124    PathOptimizer => CuJointPathOptimizer,
125    /// Straight joint line, collision-checked (`direct`).
126    Direct => CuJointDirect,
127}
128
129#[cfg(test)]
130mod tests {
131    use super::*;
132    use cu29::units::si::angle::degree;
133    use cu29::units::si::f64::Angle;
134    use repose_collision::igus_rebel_collision_model;
135    use repose_kinematics::CuJointPosition;
136    use repose_kinematics::igus_rebel_chain;
137
138    fn deg(values: [f64; 6]) -> [CuJointPosition; 6] {
139        values.map(|d| CuJointPosition::Revolute(Angle::new::<degree>(d)))
140    }
141
142    #[test]
143    fn test_every_planner_builds_runs_and_round_trips() {
144        let request = CuJointPlanRequest {
145            start: deg([-1.1, 45.5, 60.0, -3.3, -80.9, 28.7]),
146            goal: deg([0.0, 30.0, 60.0, 0.0, 60.0, 0.0]),
147            ..Default::default()
148        };
149        for &kind in CuPlannerChoice::ALL {
150            let name = ron::to_string(&kind).unwrap();
151            assert_eq!(ron::from_str::<CuPlannerChoice>(&name).unwrap(), kind);
152            let mut planner =
153                CuAnyPlanner::<6>::new(igus_rebel_chain(), igus_rebel_collision_model(), kind)
154                    .unwrap();
155            assert_eq!(planner.choice(), kind);
156            assert!(planner.quality().value >= 0.0);
157            let mut status = planner.begin(&request, 1).unwrap();
158            for _ in 0..400 {
159                if status.is_final() {
160                    break;
161                }
162                status = planner.step(64).unwrap();
163            }
164            let mut path = CuJointPlanPath::<6>::default();
165            let best = planner.write_best(&mut path);
166            assert!(best.has_path() || best.is_final(), "{kind:?}: {best:?}");
167        }
168        for (name, kind) in [
169            ("rrt_connect", CuPlannerChoice::RrtConnect),
170            ("a_star", CuPlannerChoice::AStar),
171        ] {
172            assert_eq!(ron::from_str::<CuPlannerChoice>(name).unwrap(), kind);
173        }
174    }
175
176    #[test]
177    fn test_planner_task_selects_by_name_in_ron() {
178        use repose_motion_planning::CuJointPlannerParams;
179        use repose_motion_planning::CuJointPlannerTask;
180        // The node config exactly as an application's copperconfig.ron writes it.
181        let config: ComponentConfig = ron::from_str(
182            r#"#![enable(unwrap_newtypes)] {"chain": "igus_rebel", "collision_model": "igus_rebel", "params": "rrt_star"}"#,
183        )
184        .unwrap();
185        let params: CuJointPlannerParams<CuPlannerChoice, 6, 6> =
186            config.deserialize_into().unwrap();
187        assert_eq!(params.params, CuPlannerChoice::RrtStar);
188        assert!(CuJointPlannerTask::<CuAnyPlanner<6>, 6>::from_params(params, 7).is_ok());
189    }
190}
repose/planners.rs

repose/
planners.rs
