Motion planning · repose-motion-planning

Write a planner

A planner implements one trait. The task takes care of config, budgets, seeding, publishing and replay, so a planner only plans.

The trait

rust
pub trait CuJointPlanner<const N: usize, const M: usize = N>: Sized + TypePath {
    const KIND: CuPlannerKind;                       // Sampling | Optimization | Search | Direct
    const DEFAULT_BUDGETS: (u32, u32) = (64, 32);    // (base, refine) work units
    type Params: Clone + Default + DeserializeOwned; // the RON `params` block

    fn new(chain: CuKinematicChain<N>, collision_model: CuRobotCollisionModel<N, M>,
           params: Self::Params) -> CuResult<Self>;
    fn begin(&mut self, request: &CuJointPlanRequest<N>, seed: u64) -> CuResult<CuPlanStatus>;
    fn step(&mut self, budget: u32) -> CuResult<CuPlanStatus>;
    fn write_best(&self, out: &mut CuJointPlanPath<N>) -> CuPlanStatus;
    fn quality(&self) -> Quality;
}

The contract

  1. new validates parameters. Invalid parameters fail here, at startup, not on the first job. Preallocate all storage now.
  2. begin discards any previous job, captures the scene (use CuSceneValidity), checks start and goal, and returns InvalidStart/InvalidGoal/InvalidRequest or a running status. Take all randomness from seed.
  3. step does at most budget units of work and returns the current status. Keep each unit bounded so the task's timing stays predictable.
  4. write_best writes the best path so far (waypoints, len, cost, effort, scene revision, exclusions). No waypoints unless the status has a path.
  5. quality returns straight-line cost / path cost, or 0 without a path.

No allocation after new, no panics, no I/O. Implement Freezable state through the task: what the task needs for replay is the seed and job counter it already owns, so a planner that is deterministic in (request, seed) replays for free.

Run it

rust
pub type MyPlannerTask<const N: usize> = CuJointPlannerTask<MyPlanner<N>, N>;
ron
(id: "planner", type: "my_crate::MyPlannerTask<6>",
 resources: {"rng": "planner_rng.rng"},
 config: {"chain": "igus_rebel", "collision_model": "igus_rebel", "params": {/* MyPlannerParams */}}),

Register it by name

To make it selectable as a string (as the template's "planner" key does), add it to the planners! list in the umbrella crate's planners.rs: one line with the variant name and type. CuPlannerChoice and CuAnyPlanner are generated from that list.

Reuse the helpers

Look at direct.rs for the smallest complete planner and rrt_connect.rs for a sampling one. Use CuSceneValidity for all collision questions so your planner agrees with the others on what is free.

API reference