repose_motion_planning/
trajectory_task.rs

1//! Time planned joint paths into fixed-capacity trajectories.
2
3use crate::CuJointPlanPath;
4use crate::MAX_WAYPOINTS;
5use cu29::prelude::*;
6use repose_kinematics::CuJointPosition;
7use repose_trajectory::CuTimingOverrides;
8use repose_trajectory::CuTimingParams;
9use repose_trajectory::CuTimingStatus;
10use repose_trajectory::CuTrajectoryPayload;
11use repose_trajectory::parameterize;
12
13/// Times each published plan into a [`CuTrajectoryPayload<N, CAP>`].
14///
15/// Input is a planner's [`CuJointPlanPath<N>`]. A path with
16/// [`CuPlanStatus::has_path`](crate::CuPlanStatus::has_path) is the final
17/// result of its planning job and is timed; failure statuses and empty
18/// cycles publish nothing. A path that cannot be timed within the
19/// configured limits or the trajectory capacity `CAP` is an error.
20///
21/// Node config: [`CuTimingParams<N>`] (`limits` required, `profile` default
22/// `"trapezoidal"`, `sample_period` in seconds, default `0.02`):
23///
24/// ```ron
25/// // CuPlanToTrajectoryTask<6, 1024>
26/// config: {
27///     "limits": {
28///         "max_velocities": [{"revolute": 0.8}, /* ... */],
29///         "max_accelerations": [{"revolute": 2.0}, /* ... */],
30///     },
31///     "profile": "s_curve",
32///     "sample_period": 0.01,
33/// }
34/// ```
35///
36/// Timing does not recheck the interpolated trajectory for collisions; the
37/// path's `scene_revision` identifies the scene it was planned against.
38#[derive(Reflect)]
39#[reflect(no_field_bounds, from_reflect = false)]
40pub struct CuPlanToTrajectoryTask<const N: usize, const CAP: usize> {
41    #[reflect(ignore)]
42    params: CuTimingParams<N>,
43    #[reflect(ignore)]
44    waypoints: [[CuJointPosition; N]; MAX_WAYPOINTS],
45}
46
47impl<const N: usize, const CAP: usize> CuPlanToTrajectoryTask<N, CAP> {
48    /// Creates a timing task from validated timing parameters.
49    pub fn from_params(params: CuTimingParams<N>) -> CuResult<Self> {
50        params.validate()?;
51        Ok(Self {
52            params,
53            waypoints: [[CuJointPosition::default(); N]; MAX_WAYPOINTS],
54        })
55    }
56
57    /// The timing parameters.
58    pub const fn params(&self) -> &CuTimingParams<N> {
59        &self.params
60    }
61}
62
63impl<const N: usize, const CAP: usize> Freezable for CuPlanToTrajectoryTask<N, CAP> {}
64
65impl<const N: usize, const CAP: usize> CuTask for CuPlanToTrajectoryTask<N, CAP> {
66    type Resources<'r> = ();
67    type Input<'m> = input_msg!(CuJointPlanPath<N>);
68    type Output<'m> = output_msg!(CuTrajectoryPayload<N, CAP>);
69
70    fn new(cfg: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult<Self> {
71        let cfg = cfg.ok_or_else(|| CuError::from("plan timing: missing 'limits'"))?;
72        Self::from_params(cfg.deserialize_into::<CuTimingParams<N>>()?)
73    }
74
75    fn process<'i, 'o>(
76        &mut self,
77        _ctx: &CuContext,
78        input: &Self::Input<'i>,
79        output: &mut Self::Output<'o>,
80    ) -> CuResult<()> {
81        output.tov = input.tov;
82        output.clear_payload();
83        let Some(path) = input.payload().filter(|path| path.status.has_path()) else {
84            return Ok(());
85        };
86        let len = path.len as usize;
87        if !(1..=MAX_WAYPOINTS).contains(&len) {
88            return Err(CuError::from("plan timing: invalid path length"));
89        }
90        for (dst, src) in self.waypoints.iter_mut().zip(&path.waypoints[..len]) {
91            *dst = src.positions;
92        }
93        let trajectory = output
94            .payload_mut()
95            .get_or_insert_with(CuTrajectoryPayload::new);
96        let status = parameterize(
97            &self.waypoints[..len],
98            &self.params,
99            CuTimingOverrides::default(),
100            trajectory,
101        )?;
102        if status != CuTimingStatus::Success {
103            output.clear_payload();
104            return Err(CuError::from("plan timing: parameterization failed"));
105        }
106        Ok(())
107    }
108}
109
110#[cfg(test)]
111mod tests {
112    use super::*;
113    use crate::CuPlanStatus;
114    use cu29::units::si::angle::radian;
115    use cu29::units::si::angular_acceleration::radian_per_second_squared;
116    use cu29::units::si::angular_velocity::radian_per_second;
117    use cu29::units::si::f64::Angle;
118    use cu29::units::si::f64::AngularAcceleration;
119    use cu29::units::si::f64::AngularVelocity;
120    use cu29::units::si::f64::Time;
121    use cu29::units::si::time::second;
122    use repose_kinematics::CuJointAcceleration;
123    use repose_kinematics::CuJointVelocity;
124    use repose_trajectory::CuJointTrajectoryLimits;
125    use repose_trajectory::CuTrajectoryProfile;
126
127    fn joint(rad: f64) -> CuJointPosition {
128        CuJointPosition::Revolute(Angle::new::<radian>(rad))
129    }
130
131    fn task() -> CuPlanToTrajectoryTask<1, 256> {
132        let limits = CuJointTrajectoryLimits::new(
133            [CuJointVelocity::Revolute(AngularVelocity::new::<
134                radian_per_second,
135            >(1.0))],
136            [CuJointAcceleration::Revolute(AngularAcceleration::new::<
137                radian_per_second_squared,
138            >(2.0))],
139        );
140        CuPlanToTrajectoryTask::from_params(CuTimingParams {
141            limits,
142            profile: CuTrajectoryProfile::Trapezoidal,
143            sample_period: Time::new::<second>(0.02),
144        })
145        .unwrap()
146    }
147
148    fn path(status: CuPlanStatus) -> CuJointPlanPath<1> {
149        let mut path = CuJointPlanPath::<1> {
150            status,
151            ..Default::default()
152        };
153        if status.has_path() {
154            path.waypoints[0].positions = [joint(0.0)];
155            path.waypoints[1].positions = [joint(0.5)];
156            path.len = 2;
157        }
158        path
159    }
160
161    fn run(
162        task: &mut CuPlanToTrajectoryTask<1, 256>,
163        path: CuJointPlanPath<1>,
164    ) -> CuResult<Option<CuTrajectoryPayload<1, 256>>> {
165        let input = CuMsg::new(Some(path));
166        let mut output = CuMsg::<CuTrajectoryPayload<1, 256>>::default();
167        let (ctx, _clock) = CuContext::new_mock_clock();
168        task.process(&ctx, &input, &mut output)?;
169        Ok(output.payload().cloned())
170    }
171
172    #[test]
173    fn test_times_final_paths() {
174        let mut task = task();
175        for status in [CuPlanStatus::Improving, CuPlanStatus::Converged] {
176            let trajectory = run(&mut task, path(status)).unwrap().unwrap();
177            assert!(!trajectory.is_empty());
178        }
179    }
180
181    #[test]
182    fn test_ignores_failed_plans() {
183        let mut task = task();
184        for status in [CuPlanStatus::Searching, CuPlanStatus::InvalidGoal] {
185            assert!(run(&mut task, path(status)).unwrap().is_none());
186        }
187    }
188
189    #[test]
190    fn test_reads_config() {
191        let cfg: ComponentConfig = ron::from_str(
192            r#"#![enable(unwrap_newtypes)] {"limits": {"max_velocities": [{"revolute": 1.0}], "max_accelerations": [{"revolute": 2.0}]},
193                "profile": "cubic", "sample_period": 0.01}"#,
194        )
195        .unwrap();
196        let task = CuPlanToTrajectoryTask::<1, 256>::new(Some(&cfg), ()).unwrap();
197        assert_eq!(task.params().profile, CuTrajectoryProfile::Cubic);
198        assert_eq!(task.params().sample_period, Time::new::<second>(0.01));
199        let legacy: ComponentConfig = ron::from_str(
200            r#"#![enable(unwrap_newtypes)] {"limits": {"max_velocities": [{"revolute": 1.0}], "max_accelerations": [{"revolute": 2.0}]},
201                "sample_period_s": 0.01}"#,
202        )
203        .unwrap();
204        assert!(CuPlanToTrajectoryTask::<1, 256>::new(Some(&legacy), ()).is_err());
205        assert!(CuPlanToTrajectoryTask::<1, 256>::new(None, ()).is_err());
206    }
207}
repose_motion_planning/trajectory_task.rs

repose_motion_planning/
trajectory_task.rs
