oxmpl_js/geometric/
rrt_connect.rs

1// Copyright (c) 2025 Ross Gardiner, Junior Sundar
2//
3// SPDX-License-Identifier: BSD-3-Clause
4
5use crate::base::{
6    goal::JsGoal,
7    path::JsPath,
8    planner::JsPlannerConfig,
9    problem_definition::{JsProblemDefinition, ProblemDefinitionVariant},
10    state_validity_checker::JsStateValidityChecker,
11};
12use oxmpl::base::{
13    planner::Planner,
14    space::{
15        CompoundStateSpace, RealVectorStateSpace, SE2StateSpace, SE3StateSpace, SO2StateSpace,
16        SO3StateSpace,
17    },
18    state::{CompoundState, RealVectorState, SE2State, SE3State, SO2State, SO3State},
19};
20use oxmpl::geometric::RRTConnect;
21use std::sync::Arc;
22use std::time::Duration;
23use wasm_bindgen::prelude::*;
24
25enum RRTConnectVariant {
26    RealVector(RRTConnect<RealVectorState, RealVectorStateSpace, JsGoal>),
27    SO2(RRTConnect<SO2State, SO2StateSpace, JsGoal>),
28    SO3(RRTConnect<SO3State, SO3StateSpace, JsGoal>),
29    Compound(RRTConnect<CompoundState, CompoundStateSpace, JsGoal>),
30    SE2(RRTConnect<SE2State, SE2StateSpace, JsGoal>),
31    SE3(RRTConnect<SE3State, SE3StateSpace, JsGoal>),
32}
33
34#[wasm_bindgen(js_name = RRTConnect)]
35pub struct JsRRTConnect {
36    planner: RRTConnectVariant,
37    pd: ProblemDefinitionVariant,
38}
39
40#[wasm_bindgen(js_class = RRTConnect)]
41impl JsRRTConnect {
42    #[wasm_bindgen(constructor)]
43    pub fn new(
44        max_distance: f64,
45        goal_bias: f64,
46        problem_def: &JsProblemDefinition,
47        config: &JsPlannerConfig,
48    ) -> Self {
49        let planner_config = config.into();
50        match &problem_def.inner {
51            ProblemDefinitionVariant::RealVector(pd) => Self {
52                planner: RRTConnectVariant::RealVector(RRTConnect::new(
53                    max_distance,
54                    goal_bias,
55                    &planner_config,
56                )),
57                pd: ProblemDefinitionVariant::RealVector(pd.clone()),
58            },
59            ProblemDefinitionVariant::SO2(pd) => Self {
60                planner: RRTConnectVariant::SO2(RRTConnect::new(
61                    max_distance,
62                    goal_bias,
63                    &planner_config,
64                )),
65                pd: ProblemDefinitionVariant::SO2(pd.clone()),
66            },
67            ProblemDefinitionVariant::SO3(pd) => Self {
68                planner: RRTConnectVariant::SO3(RRTConnect::new(
69                    max_distance,
70                    goal_bias,
71                    &planner_config,
72                )),
73                pd: ProblemDefinitionVariant::SO3(pd.clone()),
74            },
75            ProblemDefinitionVariant::Compound(pd) => Self {
76                planner: RRTConnectVariant::Compound(RRTConnect::new(
77                    max_distance,
78                    goal_bias,
79                    &planner_config,
80                )),
81                pd: ProblemDefinitionVariant::Compound(pd.clone()),
82            },
83            ProblemDefinitionVariant::SE2(pd) => Self {
84                planner: RRTConnectVariant::SE2(RRTConnect::new(
85                    max_distance,
86                    goal_bias,
87                    &planner_config,
88                )),
89                pd: ProblemDefinitionVariant::SE2(pd.clone()),
90            },
91            ProblemDefinitionVariant::SE3(pd) => Self {
92                planner: RRTConnectVariant::SE3(RRTConnect::new(
93                    max_distance,
94                    goal_bias,
95                    &planner_config,
96                )),
97                pd: ProblemDefinitionVariant::SE3(pd.clone()),
98            },
99        }
100    }
101
102    pub fn setup(&mut self, validity_checker: &JsStateValidityChecker) {
103        let checker = Arc::new(validity_checker.clone());
104        match &mut self.planner {
105            RRTConnectVariant::RealVector(p) => {
106                if let ProblemDefinitionVariant::RealVector(pd) = &self.pd {
107                    p.setup(pd.clone(), checker);
108                }
109            }
110            RRTConnectVariant::SO2(p) => {
111                if let ProblemDefinitionVariant::SO2(pd) = &self.pd {
112                    p.setup(pd.clone(), checker);
113                }
114            }
115            RRTConnectVariant::SO3(p) => {
116                if let ProblemDefinitionVariant::SO3(pd) = &self.pd {
117                    p.setup(pd.clone(), checker);
118                }
119            }
120            RRTConnectVariant::Compound(p) => {
121                if let ProblemDefinitionVariant::Compound(pd) = &self.pd {
122                    p.setup(pd.clone(), checker);
123                }
124            }
125            RRTConnectVariant::SE2(p) => {
126                if let ProblemDefinitionVariant::SE2(pd) = &self.pd {
127                    p.setup(pd.clone(), checker);
128                }
129            }
130            RRTConnectVariant::SE3(p) => {
131                if let ProblemDefinitionVariant::SE3(pd) = &self.pd {
132                    p.setup(pd.clone(), checker);
133                }
134            }
135        }
136    }
137
138    pub fn solve(&mut self, timeout_secs: f32) -> Result<JsPath, String> {
139        let timeout = Duration::from_secs_f32(timeout_secs);
140        match &mut self.planner {
141            RRTConnectVariant::RealVector(p) => p
142                .solve(timeout)
143                .map(JsPath::from)
144                .map_err(|e| e.to_string()),
145            RRTConnectVariant::SO2(p) => p
146                .solve(timeout)
147                .map(JsPath::from)
148                .map_err(|e| e.to_string()),
149            RRTConnectVariant::SO3(p) => p
150                .solve(timeout)
151                .map(JsPath::from)
152                .map_err(|e| e.to_string()),
153            RRTConnectVariant::Compound(p) => p
154                .solve(timeout)
155                .map(JsPath::from)
156                .map_err(|e| e.to_string()),
157            RRTConnectVariant::SE2(p) => p
158                .solve(timeout)
159                .map(JsPath::from)
160                .map_err(|e| e.to_string()),
161            RRTConnectVariant::SE3(p) => p
162                .solve(timeout)
163                .map(JsPath::from)
164                .map_err(|e| e.to_string()),
165        }
166    }
167}
oxmpl_js/geometric/rrt_connect.rs

oxmpl_js/geometric/
rrt_connect.rs
