repose_motion_planning/
optimizer.rs

1//! Joint-path optimizer: non-sampling, deterministic, anytime.
2//!
3//! A CHOMP-style planner. The path is a fixed number of waypoints, seeded on
4//! the straight joint-space line, and every iteration takes one gradient step
5//! on
6//!
7//! ```text
8//! cost = w_smooth · ½ Σ |x[i+1] − x[i]|²  +  w_obstacle · Σ max(0, ε − c(x[i]))² / ε
9//! ```
10//!
11//! in metric-normalized joint coordinates, where `c` is the signed clearance
12//! of the robot and attached objects against the captured scene and `ε` the
13//! target clearance. The obstacle gradient is a forward finite difference of
14//! the clearance, so any [`CuSceneValidity`] scene works without analytic
15//! distance gradients. After each step the whole path is checked segment by
16//! segment; the shortest collision-free path seen so far is the published
17//! result. No randomness is involved: the same request always yields the
18//! same path.
19
20use crate::CuJointPlanPath;
21use crate::CuJointPlanRequest;
22use crate::CuJointPlanner;
23use crate::CuJointPlannerTask;
24use crate::CuPlanStatus;
25use crate::CuPlannerKind;
26use crate::MAX_WAYPOINTS;
27use crate::common::check_endpoints;
28use crate::common::write_header;
29use crate::validity::CuSceneValidity;
30use crate::validity::joint_distance;
31use bincode::Decode;
32use bincode::Encode;
33use cu29::cutask_anytime::Quality;
34use cu29::cutask_anytime::quality_from_f32;
35use cu29::prelude::*;
36use cu29::units::si::f64::Length;
37use cu29::units::si::length::meter;
38use repose_collision::CuRobotCollisionModel;
39use repose_kinematics::CuJointMetric;
40use repose_kinematics::CuJointPosition;
41use repose_kinematics::CuJointPositionsPayload;
42use repose_kinematics::CuJointResolution;
43use repose_kinematics::CuKinematicChain;
44use serde::Deserialize;
45use serde::Serialize;
46
47/// Parameters of [`CuJointPathOptimizer`].
48#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize, Encode, Decode, Reflect)]
49#[serde(default, deny_unknown_fields)]
50pub struct CuJointPathOptimizerParams {
51    /// Path waypoints including start and goal (`3..=MAX_WAYPOINTS`).
52    pub waypoints: u32,
53    /// Gradient step per unit of cost gradient (dimensionless, metric-scaled).
54    pub learning_rate: f64,
55    /// Clearance below which the obstacle term pushes the path away.
56    pub clearance: Length,
57    /// Weight of the obstacle term (dimensionless, `>= 0`).
58    pub obstacle_weight: f64,
59    /// Weight of the smoothness term (dimensionless, `>= 0`).
60    pub smoothness_weight: f64,
61    /// Finite-difference step for the clearance gradient (dimensionless,
62    /// metric-scaled).
63    pub gradient_step: f64,
64    /// Iterations after which the job ends.
65    pub max_iterations: u32,
66    /// Resolution of the segment collision checks.
67    pub collision_check_step: CuJointResolution,
68    /// Scaling of dimensionless joint-space distances and costs.
69    pub metric: CuJointMetric,
70}
71
72impl Default for CuJointPathOptimizerParams {
73    fn default() -> Self {
74        Self {
75            waypoints: 12,
76            learning_rate: 0.05,
77            clearance: Length::new::<meter>(0.05),
78            obstacle_weight: 40.0,
79            smoothness_weight: 1.0,
80            gradient_step: 1e-3,
81            max_iterations: 300,
82            collision_check_step: CuJointResolution::default(),
83            metric: CuJointMetric::default(),
84        }
85    }
86}
87
88impl CuJointPathOptimizerParams {
89    /// Checks every parameter, the metric, and the collision-check resolution.
90    pub fn validate(&self) -> CuResult<()> {
91        self.metric.validate()?;
92        self.collision_check_step.metric().validate()?;
93        let valid = {
94            let positive = |v: f64| v.is_finite() && v > 0.0;
95            (3..=MAX_WAYPOINTS as u32).contains(&self.waypoints)
96                && positive(self.learning_rate)
97                && positive(self.clearance.get::<meter>())
98                && self.obstacle_weight.is_finite()
99                && self.obstacle_weight >= 0.0
100                && self.smoothness_weight.is_finite()
101                && self.smoothness_weight >= 0.0
102                && positive(self.gradient_step)
103                && self.max_iterations > 0
104        };
105        if valid {
106            Ok(())
107        } else {
108            Err(CuError::from(
109                "repose-motion-planning: invalid path optimizer parameters",
110            ))
111        }
112    }
113}
114
115/// Deterministic anytime joint-path optimizer.
116///
117/// Built with [`CuJointPlanner::new`] from robot models and [`CuJointPathOptimizerParams`];
118/// runs in [`CuJointPathOptimizerTask`].
119#[derive(Clone)]
120pub struct CuJointPathOptimizer<const N: usize, const M: usize = N> {
121    validity: CuSceneValidity<N, M>,
122    params: CuJointPathOptimizerParams,
123    path: [[CuJointPosition; N]; MAX_WAYPOINTS],
124    best: [[CuJointPosition; N]; MAX_WAYPOINTS],
125    count: usize,
126    best_cost: f64,
127    straight_cost: f64,
128    iterations: u32,
129    status: CuPlanStatus,
130}
131
132/// [`CuJointPlannerTask`] running [`CuJointPathOptimizer`].
133pub type CuJointPathOptimizerTask<const N: usize, const M: usize = N> =
134    CuJointPlannerTask<CuJointPathOptimizer<N, M>, N, M>;
135
136impl<const N: usize, const M: usize> CuJointPathOptimizer<N, M> {
137    /// The validated planner parameters.
138    pub const fn params(&self) -> &CuJointPathOptimizerParams {
139        &self.params
140    }
141
142    fn build(
143        chain: CuKinematicChain<N>,
144        model: CuRobotCollisionModel<N, M>,
145        params: CuJointPathOptimizerParams,
146    ) -> Self {
147        Self {
148            validity: CuSceneValidity::new(chain, model, params.collision_check_step),
149            params,
150            path: [[CuJointPosition::default(); N]; MAX_WAYPOINTS],
151            best: [[CuJointPosition::default(); N]; MAX_WAYPOINTS],
152            count: 0,
153            best_cost: f64::INFINITY,
154            straight_cost: 0.0,
155            iterations: 0,
156            status: CuPlanStatus::InvalidRequest,
157        }
158    }
159
160    /// Iterations run on the current job.
161    pub const fn iterations(&self) -> u32 {
162        self.iterations
163    }
164
165    fn penalty(&self, q: &[CuJointPosition; N]) -> f64 {
166        self.penalty_of(self.validity.clearance(q))
167    }
168
169    fn penalty_of(&self, clearance: Option<f64>) -> f64 {
170        let eps = self.params.clearance.get::<meter>();
171        match clearance {
172            Some(c) if c >= eps => 0.0,
173            Some(c) => self.params.obstacle_weight * (eps - c) * (eps - c) / eps,
174            // Unevaluable configurations are as bad as deep penetration.
175            None => self.params.obstacle_weight * eps,
176        }
177    }
178
179    fn path_cost(&self, path: &[[CuJointPosition; N]]) -> f64 {
180        path.windows(2)
181            .map(|w| joint_distance(&w[0], &w[1], self.params.metric))
182            .sum()
183    }
184
185    fn path_is_free(&self) -> bool {
186        (1..self.count - 1).all(|i| self.validity.is_configuration_free(&self.path[i]))
187            && (0..self.count - 1).all(|i| {
188                self.validity
189                    .is_segment_free(&self.path[i], &self.path[i + 1])
190            })
191    }
192
193    fn record_if_better(&mut self) {
194        if !self.path_is_free() {
195            return;
196        }
197        let cost = self.path_cost(&self.path[..self.count]);
198        if cost < self.best_cost {
199            self.best_cost = cost;
200            self.best[..self.count].copy_from_slice(&self.path[..self.count]);
201        }
202    }
203
204    /// One gradient step on every interior waypoint; returns the largest
205    /// normalized joint update.
206    fn descend(&mut self) -> f64 {
207        let metric = self.params.metric;
208        let h = self.params.gradient_step;
209        let eps = self.params.clearance.get::<meter>();
210        let mut largest = 0.0_f64;
211        let mut next = self.path;
212        for (i, slot) in next.iter_mut().enumerate().take(self.count - 1).skip(1) {
213            let q = self.path[i];
214            let clearance = self.validity.clearance(&q);
215            let base = self.penalty_of(clearance);
216            // Well clear of every obstacle a finite-difference probe stays in
217            // the flat zero-penalty region, so the clearance queries are skipped.
218            let flat = clearance.is_some_and(|c| c > 2.0 * eps);
219            for j in 0..N {
220                if matches!(q[j], CuJointPosition::Fixed) {
221                    continue;
222                }
223                let scale = metric.scale(q[j]);
224                let x = q[j].si_value() / scale;
225                let prev = self.path[i - 1][j].si_value() / scale;
226                let after = self.path[i + 1][j].si_value() / scale;
227                let mut gradient = self.params.smoothness_weight * (2.0 * x - prev - after);
228                if !flat {
229                    let mut probe = q;
230                    probe[j] = q[j].with_si_value((x + h) * scale);
231                    gradient += (self.penalty(&probe) - base) / h;
232                }
233                let limits = self.validity.chain().joints()[j].limits;
234                let lo = limits.lower().si_value() / scale;
235                let hi = limits.upper().si_value() / scale;
236                let updated = (x - self.params.learning_rate * gradient).clamp(lo, hi);
237                largest = largest.max(libm::fabs(updated - x));
238                slot[j] = q[j].with_si_value(updated * scale);
239            }
240        }
241        self.path = next;
242        largest
243    }
244}
245
246impl<const N: usize, const M: usize> CuJointPlanner<N, M> for CuJointPathOptimizer<N, M> {
247    const KIND: CuPlannerKind = CuPlannerKind::Optimization;
248
249    type Params = CuJointPathOptimizerParams;
250
251    fn new(
252        chain: CuKinematicChain<N>,
253        collision_model: CuRobotCollisionModel<N, M>,
254        params: CuJointPathOptimizerParams,
255    ) -> CuResult<Self> {
256        params.validate()?;
257        Ok(Self::build(chain, collision_model, params))
258    }
259
260    fn begin(&mut self, request: &CuJointPlanRequest<N>, _seed: u64) -> CuResult<CuPlanStatus> {
261        self.iterations = 0;
262        self.best_cost = f64::INFINITY;
263        self.count = self.params.waypoints as usize;
264        self.straight_cost = joint_distance(&request.start, &request.goal, self.params.metric);
265        self.status = check_endpoints(&mut self.validity, request, self.params.metric);
266        if self.status == CuPlanStatus::Searching {
267            let last = (self.count - 1) as f64;
268            for i in 0..self.count {
269                for j in 0..N {
270                    self.path[i][j] = request.start[j]
271                        .interpolate(request.goal[j], i as f64 / last)
272                        .map_err(|_| {
273                            CuError::from("repose-motion-planning: joint kinds disagree")
274                        })?;
275                }
276            }
277            // The straight line is the cheapest possible path; when it is
278            // free there is nothing to optimize.
279            self.record_if_better();
280            if self.best_cost.is_finite() {
281                self.status = CuPlanStatus::Converged;
282            }
283        }
284        Ok(self.status)
285    }
286
287    fn step(&mut self, budget: u32) -> CuResult<CuPlanStatus> {
288        for _ in 0..budget {
289            if self.status.is_final() {
290                break;
291            }
292            let moved = self.descend();
293            self.iterations += 1;
294            self.record_if_better();
295            let done = moved <= 1e-7 || self.iterations >= self.params.max_iterations;
296            self.status = match (self.best_cost.is_finite(), done) {
297                (true, true) => CuPlanStatus::Converged,
298                (true, false) => CuPlanStatus::Improving,
299                (false, true) => CuPlanStatus::Exhausted,
300                (false, false) => CuPlanStatus::Searching,
301            };
302        }
303        Ok(self.status)
304    }
305
306    fn write_best(&self, out: &mut CuJointPlanPath<N>) -> CuPlanStatus {
307        write_header(out, &self.validity, self.status, self.iterations);
308        if self.status.has_path() {
309            for (slot, q) in out.waypoints.iter_mut().zip(&self.best[..self.count]) {
310                *slot = CuJointPositionsPayload { positions: *q };
311            }
312            out.len = self.count as u32;
313            out.cost = self.best_cost;
314        }
315        self.status
316    }
317
318    fn quality(&self) -> Quality {
319        if !self.best_cost.is_finite() {
320            return quality_from_f32(0.0);
321        }
322        if self.best_cost <= 1e-9 {
323            return quality_from_f32(1.0);
324        }
325        quality_from_f32((self.straight_cost / self.best_cost).clamp(0.0, 1.0) as f32)
326    }
327}
328
329crate::impl_planner_type_path!(
330    CuJointPathOptimizer,
331    "repose_motion_planning::CuJointPathOptimizer"
332);
repose_motion_planning/optimizer.rs

repose_motion_planning/
optimizer.rs
