repose_motion_planning/
direct.rs

1//! Direct joint-line planner: non-sampling, deterministic, one shot.
2//!
3//! The straight joint-space line from start to goal is either collision-free
4//! against the captured scene (`Converged`) or it is not (`Exhausted`). It is
5//! the cheapest possible planner and a useful first attempt before a sampling
6//! or optimizing planner.
7
8use crate::CuJointPlanPath;
9use crate::CuJointPlanRequest;
10use crate::CuJointPlanner;
11use crate::CuJointPlannerTask;
12use crate::CuPlanStatus;
13use crate::CuPlannerKind;
14use crate::common::check_endpoints;
15use crate::common::write_header;
16use crate::validity::CuSceneValidity;
17use crate::validity::joint_distance;
18use bincode::Decode;
19use bincode::Encode;
20use cu29::cutask_anytime::Quality;
21use cu29::cutask_anytime::quality_from_f32;
22use cu29::prelude::*;
23use repose_collision::CuRobotCollisionModel;
24use repose_kinematics::CuJointMetric;
25use repose_kinematics::CuJointPosition;
26use repose_kinematics::CuJointPositionsPayload;
27use repose_kinematics::CuJointResolution;
28use repose_kinematics::CuKinematicChain;
29use serde::Deserialize;
30use serde::Serialize;
31
32/// Parameters of [`CuJointDirect`].
33#[derive(
34    Debug, Clone, Copy, PartialEq, Default, Serialize, Deserialize, Encode, Decode, Reflect,
35)]
36#[serde(default, deny_unknown_fields)]
37pub struct CuJointDirectParams {
38    /// Resolution of the segment collision check.
39    pub collision_check_step: CuJointResolution,
40    /// Scaling of dimensionless joint-space distances and costs.
41    pub metric: CuJointMetric,
42}
43
44impl CuJointDirectParams {
45    /// Checks the metric and collision-check resolution.
46    pub fn validate(&self) -> CuResult<()> {
47        self.metric.validate()?;
48        self.collision_check_step.metric().validate()
49    }
50}
51
52/// Straight joint-space line planner.
53///
54/// Built with [`CuJointPlanner::new`] from robot models and
55/// [`CuJointDirectParams`]; runs in [`CuJointDirectTask`].
56#[derive(Clone)]
57pub struct CuJointDirect<const N: usize, const M: usize = N> {
58    validity: CuSceneValidity<N, M>,
59    metric: CuJointMetric,
60    start: [CuJointPosition; N],
61    goal: [CuJointPosition; N],
62    status: CuPlanStatus,
63    effort: u32,
64}
65
66/// [`CuJointPlannerTask`] running [`CuJointDirect`].
67pub type CuJointDirectTask<const N: usize, const M: usize = N> =
68    CuJointPlannerTask<CuJointDirect<N, M>, N, M>;
69
70impl<const N: usize, const M: usize> CuJointPlanner<N, M> for CuJointDirect<N, M> {
71    const KIND: CuPlannerKind = CuPlannerKind::Direct;
72
73    type Params = CuJointDirectParams;
74
75    fn new(
76        chain: CuKinematicChain<N>,
77        collision_model: CuRobotCollisionModel<N, M>,
78        params: CuJointDirectParams,
79    ) -> CuResult<Self> {
80        params.validate()?;
81        Ok(Self {
82            validity: CuSceneValidity::new(chain, collision_model, params.collision_check_step),
83            metric: params.metric,
84            start: [CuJointPosition::default(); N],
85            goal: [CuJointPosition::default(); N],
86            status: CuPlanStatus::InvalidRequest,
87            effort: 0,
88        })
89    }
90
91    fn begin(&mut self, request: &CuJointPlanRequest<N>, _seed: u64) -> CuResult<CuPlanStatus> {
92        self.start = request.start;
93        self.goal = request.goal;
94        self.effort = 0;
95        self.status = check_endpoints(&mut self.validity, request, self.metric);
96        Ok(self.status)
97    }
98
99    fn step(&mut self, budget: u32) -> CuResult<CuPlanStatus> {
100        if self.status == CuPlanStatus::Searching && budget > 0 {
101            self.effort = 1;
102            self.status = if self.validity.is_segment_free(&self.start, &self.goal) {
103                CuPlanStatus::Converged
104            } else {
105                CuPlanStatus::Exhausted
106            };
107        }
108        Ok(self.status)
109    }
110
111    fn write_best(&self, out: &mut CuJointPlanPath<N>) -> CuPlanStatus {
112        write_header(out, &self.validity, self.status, self.effort);
113        if self.status.has_path() {
114            out.waypoints[0] = CuJointPositionsPayload {
115                positions: self.start,
116            };
117            out.waypoints[1] = CuJointPositionsPayload {
118                positions: self.goal,
119            };
120            out.len = 2;
121            out.cost = joint_distance(&self.start, &self.goal, self.metric);
122        }
123        self.status
124    }
125
126    fn quality(&self) -> Quality {
127        quality_from_f32(if self.status.has_path() { 1.0 } else { 0.0 })
128    }
129}
130
131crate::impl_planner_type_path!(CuJointDirect, "repose_motion_planning::CuJointDirect");
repose_motion_planning/direct.rs

repose_motion_planning/
direct.rs
