repose_motion_planning/
task.rs

1//! The one Copper task that runs any [`CuJointPlanner`].
2
3use crate::CuJointPlanPath;
4use crate::CuJointPlanRequest;
5use crate::CuJointPlanner;
6use crate::CuPlanStatus;
7use bincode::Decode;
8use bincode::Encode;
9use bincode::de::Decoder;
10use bincode::enc::Encoder;
11use bincode::error::DecodeError;
12use bincode::error::EncodeError;
13use cu_rng::prelude::*;
14use cu29::cutask_anytime::AnytimeStatus;
15use cu29::cutask_anytime::CuAnytimeTask;
16use cu29::cutask_anytime::Quality;
17use cu29::cutask_anytime::quality_from_f32;
18use cu29::prelude::*;
19use cu29::resources;
20use repose_collision::CuCollisionModelSpec;
21use repose_kinematics::CuChainSpec;
22use serde::Deserialize;
23use serde::Serialize;
24
25/// Resources of [`CuJointPlannerTask`]: an owned `rng` that seeds jobs.
26pub mod planner_resources {
27    use super::*;
28    resources!({ rng => Owned<CuRng> });
29}
30
31/// Compact progress snapshot for remote inspection.
32#[derive(Default, Debug, Reflect)]
33pub struct CuJointPlannerDebugState {
34    pub status: CuPlanStatus,
35    pub effort: u32,
36    pub published_cost: f64,
37    pub published_quality: Quality,
38}
39
40/// Node config of [`CuJointPlannerTask`] (and every `CuJoint<Planner>Task`
41/// alias): robot models, the planner's own parameters, and work budgets.
42///
43/// `P` is the planner's [`CuJointPlanner::Params`].
44///
45/// ```ron
46/// // CuJointRrtStarTask<6> (repose-joint-rrt-star)
47/// config: {
48///     "chain": "igus_rebel",            // or {"joints": [...]}
49///     "collision_model": "igus_rebel",  // or {"shapes": [...], "safety_margin": 0.005}
50///     "params": {"step_size": 0.3, "max_nodes": 200}, // planner params; omit = defaults
51///     "base_budget": 64,                // optional; default CuJointPlanner::DEFAULT_BUDGETS
52///     "refine_budget": 32,              // optional
53/// }
54/// ```
55#[derive(Debug, Clone, Serialize, Deserialize)]
56// Field-level defaults: serde does not support container `default` on const-generic structs.
57#[serde(
58    deny_unknown_fields,
59    bound(
60        serialize = "P: Serialize",
61        deserialize = "P: Deserialize<'de> + Default"
62    )
63)]
64pub struct CuJointPlannerParams<P, const N: usize, const M: usize = N> {
65    /// Required robot chain: a preset name (`"igus_rebel"`) or an inline chain.
66    #[serde(default)]
67    pub chain: Option<CuChainSpec<N>>,
68    /// Required collision model: a preset name (`"igus_rebel"`) or an inline model.
69    #[serde(default)]
70    pub collision_model: Option<CuCollisionModelSpec<M>>,
71    /// The planner's parameters (validated by [`CuJointPlanner::new`]).
72    #[serde(default)]
73    pub params: P,
74    /// Work units of `base()`; `None` uses the planner's default.
75    #[serde(default)]
76    pub base_budget: Option<u32>,
77    /// Work units of each `refine()`; `None` uses the planner's default.
78    #[serde(default)]
79    pub refine_budget: Option<u32>,
80}
81
82impl<P: Default, const N: usize, const M: usize> Default for CuJointPlannerParams<P, N, M> {
83    fn default() -> Self {
84        Self {
85            chain: None,
86            collision_model: None,
87            params: P::default(),
88            base_budget: None,
89            refine_budget: None,
90        }
91    }
92}
93
94impl<P, const N: usize, const M: usize> CuJointPlannerParams<P, N, M> {
95    /// Checks that the chain and collision model are present and valid and
96    /// that explicit budgets are positive. Planner parameters are validated
97    /// by [`CuJointPlanner::new`].
98    pub fn validate(&self) -> CuResult<()> {
99        self.resolve_models()?;
100        if self.base_budget == Some(0) || self.refine_budget == Some(0) {
101            return Err(CuError::from(
102                "repose-motion-planning: 'base_budget' and 'refine_budget' must be positive",
103            ));
104        }
105        Ok(())
106    }
107
108    fn resolve_models(
109        &self,
110    ) -> CuResult<(
111        repose_kinematics::CuKinematicChain<N>,
112        repose_collision::CuRobotCollisionModel<N, M>,
113    )> {
114        let chain = self
115            .chain
116            .as_ref()
117            .ok_or_else(|| CuError::from("repose-motion-planning: missing required 'chain'"))?
118            .resolve()?;
119        let model = self
120            .collision_model
121            .as_ref()
122            .ok_or_else(|| {
123                CuError::from("repose-motion-planning: missing required 'collision_model'")
124            })?
125            .resolve::<N>()?;
126        Ok((chain, model))
127    }
128}
129
130/// Anytime planning task (`CuAnytimeTask`) over any [`CuJointPlanner`].
131///
132/// `base()` starts a job from the request and runs `base_budget` work units;
133/// each `refine()` runs `refine_budget` more. The output only changes when
134/// the path cost strictly improves, so the published quality is monotone.
135/// Failures (invalid start or goal, invalid request, exhausted search,
136/// capacity) are published as a [`CuJointPlanPath`] without waypoints whose
137/// `status` names the reason, and the job aborts.
138///
139/// Node config: [`CuJointPlannerParams`]. The job seed comes from the task's
140/// `rng` resource; deterministic planners ignore it. Each planner has a task
141/// alias (`CuJointRrtConnectTask<N, M = N>`, ...), so RON names it directly.
142#[derive(Reflect)]
143#[reflect(no_field_bounds, from_reflect = false)]
144pub struct CuJointPlannerTask<P, const N: usize, const M: usize = N> {
145    #[reflect(ignore)]
146    planner: P,
147    base_budget: u32,
148    refine_budget: u32,
149    base_seed: u64,
150    job_counter: u64,
151    #[reflect(ignore)]
152    status: CuPlanStatus,
153    #[reflect(ignore)]
154    effort: u32,
155    published_cost: f64,
156    published_quality: Quality,
157}
158
159impl<P: CuJointPlanner<N, M>, const N: usize, const M: usize> CuJointPlannerTask<P, N, M> {
160    /// Wraps `planner` with explicit positive work budgets and a base seed.
161    pub fn from_planner(
162        planner: P,
163        base_budget: u32,
164        refine_budget: u32,
165        base_seed: u64,
166    ) -> CuResult<Self> {
167        if base_budget == 0 || refine_budget == 0 {
168            return Err(CuError::from(
169                "repose-motion-planning: 'base_budget' and 'refine_budget' must be positive",
170            ));
171        }
172        Ok(Self {
173            planner,
174            base_budget,
175            refine_budget,
176            base_seed,
177            job_counter: 0,
178            status: CuPlanStatus::default(),
179            effort: 0,
180            published_cost: f64::INFINITY,
181            published_quality: quality_from_f32(0.0),
182        })
183    }
184
185    /// Builds the planner from `params` (robot models, planner parameters,
186    /// budgets with per-planner defaults) and wraps it with `base_seed`.
187    pub fn from_params(
188        params: CuJointPlannerParams<P::Params, N, M>,
189        base_seed: u64,
190    ) -> CuResult<Self> {
191        params.validate()?;
192        let (chain, model) = params.resolve_models()?;
193        let planner = P::new(chain, model, params.params)?;
194        let (base, refine) = P::DEFAULT_BUDGETS;
195        Self::from_planner(
196            planner,
197            params.base_budget.unwrap_or(base),
198            params.refine_budget.unwrap_or(refine),
199            base_seed,
200        )
201    }
202
203    /// The wrapped planner.
204    pub const fn planner(&self) -> &P {
205        &self.planner
206    }
207
208    /// Status of the current job.
209    pub const fn status(&self) -> CuPlanStatus {
210        self.status
211    }
212
213    /// Compact snapshot of the current job's progress.
214    pub fn debug_state(&self) -> CuJointPlannerDebugState {
215        CuJointPlannerDebugState {
216            status: self.status,
217            effort: self.effort,
218            published_cost: self.published_cost,
219            published_quality: self.published_quality,
220        }
221    }
222
223    fn reset_published(&mut self, output: &mut CuMsg<CuJointPlanPath<N>>) {
224        output.clear_payload();
225        self.published_cost = f64::INFINITY;
226        self.published_quality = quality_from_f32(0.0);
227    }
228
229    /// Publishes an improved path or a final failure; returns the anytime status.
230    fn publish(&mut self, output: &mut CuMsg<CuJointPlanPath<N>>) -> AnytimeStatus<Quality> {
231        let mut path = CuJointPlanPath::<N>::default();
232        let status = self.planner.write_best(&mut path);
233        self.status = status;
234        self.effort = path.effort;
235        if status.has_path() {
236            if path.cost < self.published_cost {
237                self.published_cost = path.cost;
238                self.published_quality = self.planner.quality();
239                output.set_payload(path);
240            } else if let Some(published) = output.payload_mut() {
241                // Same path, but the job may have just converged.
242                published.status = status;
243                published.effort = path.effort;
244            }
245            return if status == CuPlanStatus::Converged {
246                AnytimeStatus::Converged(self.published_quality)
247            } else {
248                AnytimeStatus::Improved(self.published_quality)
249            };
250        }
251        if status.is_final() {
252            self.published_cost = f64::INFINITY;
253            self.published_quality = quality_from_f32(0.0);
254            output.set_payload(path);
255            return AnytimeStatus::Aborted;
256        }
257        AnytimeStatus::Improved(self.published_quality)
258    }
259}
260
261impl<P, const N: usize, const M: usize> Freezable for CuJointPlannerTask<P, N, M> {
262    fn freeze<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
263        Encode::encode(&self.base_seed, encoder)?;
264        Encode::encode(&self.job_counter, encoder)
265    }
266
267    fn thaw<D: Decoder>(&mut self, decoder: &mut D) -> Result<(), DecodeError> {
268        self.base_seed = Decode::decode(decoder)?;
269        self.job_counter = Decode::decode(decoder)?;
270        Ok(())
271    }
272}
273
274impl<P, const N: usize, const M: usize> CuAnytimeTask for CuJointPlannerTask<P, N, M>
275where
276    P: CuJointPlanner<N, M> + Send + Sync + 'static,
277{
278    type Input<'m> = input_msg!(CuJointPlanRequest<N>);
279    type Output<'m> = output_msg!(CuJointPlanPath<N>);
280    type Resources<'r> = planner_resources::Resources;
281    type Quality = Quality;
282
283    fn register_debug_state_types(registry: &mut TypeRegistry) {
284        registry.register::<CuJointPlannerDebugState>();
285    }
286
287    fn debug_state_type_path() -> &'static str {
288        CuJointPlannerDebugState::type_path()
289    }
290
291    fn with_debug_state<R>(&self, f: impl FnOnce(&dyn bevy_reflect::Reflect) -> R) -> R {
292        f(&self.debug_state())
293    }
294
295    fn new(config: Option<&ComponentConfig>, resources: Self::Resources<'_>) -> CuResult<Self> {
296        let params = config
297            .map(ComponentConfig::deserialize_into::<CuJointPlannerParams<P::Params, N, M>>)
298            .transpose()?
299            .unwrap_or_default();
300        let Owned(mut rng) = resources.rng;
301        Self::from_params(params, rng.random::<u64>())
302    }
303
304    fn base<'i, 'o>(
305        &mut self,
306        _ctx: &CuContext,
307        input: &Self::Input<'i>,
308        output: &mut Self::Output<'o>,
309    ) -> CuResult<AnytimeStatus<Quality>> {
310        output.tov = input.tov;
311        self.reset_published(output);
312        let Some(request) = input.payload() else {
313            self.status = CuPlanStatus::default();
314            return Ok(AnytimeStatus::Aborted);
315        };
316        self.job_counter = self.job_counter.wrapping_add(1);
317        let seed = self.base_seed.wrapping_add(self.job_counter);
318        let status = self.planner.begin(request, seed)?;
319        if !status.is_final() {
320            self.planner.step(self.base_budget)?;
321        }
322        Ok(self.publish(output))
323    }
324
325    fn refine<'o>(
326        &mut self,
327        _ctx: &CuContext,
328        output: &mut Self::Output<'o>,
329    ) -> CuResult<AnytimeStatus<Quality>> {
330        if self.status.is_final() {
331            return Ok(if self.status.has_path() {
332                AnytimeStatus::Converged(self.published_quality)
333            } else {
334                AnytimeStatus::Aborted
335            });
336        }
337        self.planner.step(self.refine_budget)?;
338        Ok(self.publish(output))
339    }
340}
repose_motion_planning/task.rs

repose_motion_planning/
task.rs
