repose_trajectory/
task.rs

1//! `CuTask` following a stored [`CuTrajectoryPayload`] with preemption.
2//!
3//! [`CuFollowTrajectoryTask`] adopts the latest non-empty trajectory from its
4//! input, samples it at the elapsed time since adoption, and emits joint
5//! positions, velocities, and a [`CuFollowStatus`]. A differing trajectory
6//! preempts the running one; an empty trajectory cancels it.
7
8use crate::CuTrajectoryInterpolation;
9use crate::CuTrajectoryPayload;
10use crate::MAX_TRAJECTORY_POINTS;
11use bincode::Decode;
12use bincode::Encode;
13use bincode::de::Decoder;
14use bincode::enc::Encoder;
15use bincode::error::DecodeError;
16use bincode::error::EncodeError;
17use cu29::prelude::*;
18use cu29::units::si::f64::Time;
19use cu29::units::si::time::second;
20use repose_kinematics::CuJointPosition;
21use repose_kinematics::CuJointVelocity;
22use serde::Deserialize;
23use serde::Serialize;
24
25/// Follower outcome mirroring the [`CuIkStatus`](repose_kinematics::CuIkStatus) derive style.
26#[derive(
27    Default, Debug, Clone, Copy, PartialEq, Serialize, Deserialize, Encode, Decode, Reflect,
28)]
29#[serde(rename_all = "snake_case")]
30pub enum CuFollowStatus {
31    /// No trajectory has been adopted yet.
32    #[default]
33    Idle,
34    /// Sampling a stored trajectory before its end time.
35    Running,
36    /// Elapsed time reached the trajectory duration; the final point is held.
37    Finished,
38    /// The input carried an empty trajectory; any stored trajectory is cancelled.
39    ErrorEmptyTrajectory,
40}
41
42/// Follower setpoint carrying sampled joints plus the follow status.
43///
44/// Single-output analogue of `CuIkSolutionPayload`/`CuJointVelocitiesPayload`:
45/// `CuTask` supports exactly one `Output` type, so status rides with the data.
46#[derive(Debug, Clone, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect)]
47#[reflect(no_field_bounds, from_reflect = false)]
48pub struct CuTrajectoryFollowOutput<const N: usize> {
49    #[serde(with = "repose_kinematics::serde_arrays::position_array")]
50    pub positions: [CuJointPosition; N],
51    #[serde(with = "repose_kinematics::serde_arrays::velocity_array")]
52    pub velocities: [CuJointVelocity; N],
53    pub status: CuFollowStatus,
54}
55
56impl<const N: usize> Default for CuTrajectoryFollowOutput<N> {
57    fn default() -> Self {
58        Self {
59            positions: [CuJointPosition::default(); N],
60            velocities: [CuJointVelocity::default(); N],
61            status: CuFollowStatus::default(),
62        }
63    }
64}
65
66/// Parameters (node `config:`) of [`CuFollowTrajectoryTask`].
67///
68/// ```ron
69/// config: {}                          // use each trajectory's own interpolation
70/// config: {"interpolation": "cubic"}  // override it
71/// ```
72#[derive(Default, Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize, Reflect)]
73#[serde(default, deny_unknown_fields)]
74pub struct CuFollowTrajectoryParams {
75    /// Interpolation override; `None` samples each trajectory with its own
76    /// [`CuTrajectoryPayload::interpolation`].
77    pub interpolation: Option<CuTrajectoryInterpolation>,
78}
79
80impl CuFollowTrajectoryParams {
81    /// Always valid; present for the uniform params contract.
82    pub fn validate(&self) -> CuResult<()> {
83        Ok(())
84    }
85}
86
87/// `CuTask` sampling a stored joint trajectory against the message `Tov` clock.
88///
89/// Each trajectory is sampled with its own
90/// [`interpolation`](CuTrajectoryPayload::interpolation) unless
91/// [`CuFollowTrajectoryParams::interpolation`] overrides it.
92///
93/// The trajectory clock is the input message's `Tov`: adoption records the
94/// `Tov::Time` of the adopting call and each call samples at `now - start`,
95/// where `now` is the current input `Tov::Time` (`ctx.now()` fallback when the
96/// input carries no stamp). This mirrors the sibling tasks' `output.tov =
97/// input.tov` propagation while keeping elapsed tracking on the
98/// measurement-time axis rather than processing time.
99#[derive(Reflect)]
100#[reflect(no_field_bounds, from_reflect = false)]
101pub struct CuFollowTrajectoryTask<const N: usize, const MAX_POINTS: usize = MAX_TRAJECTORY_POINTS> {
102    #[reflect(ignore)]
103    trajectory: Option<CuTrajectoryPayload<N, MAX_POINTS>>,
104    #[reflect(ignore)]
105    start: CuTime,
106    params: CuFollowTrajectoryParams,
107}
108
109impl<const N: usize, const MAX_POINTS: usize> CuFollowTrajectoryTask<N, MAX_POINTS> {
110    /// Creates an idle follower holding no trajectory.
111    pub fn from_params(params: CuFollowTrajectoryParams) -> CuResult<Self> {
112        params.validate()?;
113        Ok(Self {
114            trajectory: None,
115            start: CuTime::MIN,
116            params,
117        })
118    }
119
120    /// The follower parameters.
121    pub const fn params(&self) -> &CuFollowTrajectoryParams {
122        &self.params
123    }
124}
125
126impl<const N: usize, const MAX_POINTS: usize> Default for CuFollowTrajectoryTask<N, MAX_POINTS> {
127    /// Idle follower using each trajectory's own interpolation.
128    fn default() -> Self {
129        Self {
130            trajectory: None,
131            start: CuTime::MIN,
132            params: CuFollowTrajectoryParams::default(),
133        }
134    }
135}
136
137impl<const N: usize, const MAX_POINTS: usize> Freezable for CuFollowTrajectoryTask<N, MAX_POINTS> {
138    fn freeze<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
139        Encode::encode(&self.trajectory, encoder)?;
140        Encode::encode(&self.start, encoder)?;
141        Encode::encode(&self.params.interpolation, encoder)
142    }
143
144    fn thaw<D: Decoder>(&mut self, decoder: &mut D) -> Result<(), DecodeError> {
145        self.trajectory = Decode::decode(decoder)?;
146        self.start = Decode::decode(decoder)?;
147        self.params.interpolation = Decode::decode(decoder)?;
148        Ok(())
149    }
150}
151
152impl<const N: usize, const MAX_POINTS: usize> CuTask for CuFollowTrajectoryTask<N, MAX_POINTS> {
153    type Resources<'r> = ();
154    type Input<'m> = input_msg!(CuTrajectoryPayload<N, MAX_POINTS>);
155    type Output<'m> = output_msg!(CuTrajectoryFollowOutput<N>);
156
157    fn new(cfg: Option<&ComponentConfig>, _res: Self::Resources<'_>) -> CuResult<Self>
158    where
159        Self: Sized,
160    {
161        let params = cfg
162            .map(ComponentConfig::deserialize_into::<CuFollowTrajectoryParams>)
163            .transpose()?
164            .unwrap_or_default();
165        Self::from_params(params)
166    }
167
168    fn process<'i, 'o>(
169        &mut self,
170        ctx: &CuContext,
171        input: &Self::Input<'i>,
172        output: &mut Self::Output<'o>,
173    ) -> CuResult<()> {
174        output.tov = input.tov;
175        let now = match input.tov {
176            Tov::Time(t) => t,
177            _ => ctx.now(),
178        };
179
180        if let Some(traj) = input.payload() {
181            if traj.is_empty() {
182                self.trajectory = None;
183                output.set_payload(CuTrajectoryFollowOutput {
184                    status: CuFollowStatus::ErrorEmptyTrajectory,
185                    ..Default::default()
186                });
187                return Ok(());
188            }
189            if self.trajectory.as_ref().is_none_or(|stored| stored != traj) {
190                self.trajectory = Some(traj.clone());
191                self.start = now;
192            }
193        }
194
195        let Some(stored) = self.trajectory.as_ref() else {
196            output.set_payload(CuTrajectoryFollowOutput::default());
197            return Ok(());
198        };
199
200        let elapsed = now - self.start;
201        let elapsed_s = elapsed.as_nanos() as f64 / 1_000_000_000.0;
202        let mut positions = [CuJointPosition::default(); N];
203        let mut velocities = [CuJointVelocity::default(); N];
204        let interpolation = self.params.interpolation.unwrap_or(stored.interpolation);
205        stored.sample_with(
206            interpolation,
207            Time::new::<second>(elapsed_s),
208            &mut positions,
209            &mut velocities,
210        )?;
211        let status = if elapsed_s >= stored.duration().get::<second>() {
212            CuFollowStatus::Finished
213        } else {
214            CuFollowStatus::Running
215        };
216        output.set_payload(CuTrajectoryFollowOutput {
217            positions,
218            velocities,
219            status,
220        });
221        Ok(())
222    }
223}
224
225#[cfg(test)]
226mod tests {
227    use super::*;
228    use crate::CuTrajectoryPoint;
229    use cu29::units::si::angle::radian;
230    use cu29::units::si::angular_velocity::radian_per_second;
231    use cu29::units::si::f64::Angle;
232    use cu29::units::si::f64::AngularVelocity;
233
234    fn point(t: f64, q: f64, v: f64) -> CuTrajectoryPoint<1> {
235        CuTrajectoryPoint {
236            time_from_start: Time::new::<second>(t),
237            positions: [CuJointPosition::Revolute(Angle::new::<radian>(q))],
238            velocities: [CuJointVelocity::Revolute(AngularVelocity::new::<
239                radian_per_second,
240            >(v))],
241            ..Default::default()
242        }
243    }
244
245    fn cubic() -> CuFollowTrajectoryParams {
246        CuFollowTrajectoryParams {
247            interpolation: Some(CuTrajectoryInterpolation::Cubic),
248        }
249    }
250
251    fn two_point_traj() -> CuTrajectoryPayload<1> {
252        let mut traj = CuTrajectoryPayload::new();
253        traj.push(point(0.0, 0.0, 0.0)).unwrap();
254        traj.push(point(2.0, 2.0, 4.0)).unwrap();
255        traj
256    }
257
258    fn msg_at(traj: Option<CuTrajectoryPayload<1>>, secs: u64) -> CuMsg<CuTrajectoryPayload<1>> {
259        let mut msg = CuMsg::new(traj);
260        msg.tov = Tov::Time(CuTime::from_secs(secs));
261        msg
262    }
263
264    #[test]
265    fn test_idle_before_any_trajectory() {
266        let (ctx, _) = CuContext::new_mock_clock();
267        let mut task = CuFollowTrajectoryTask::<1>::default();
268        let input = CuMsg::new(None);
269        let mut output = CuMsg::new(None);
270        task.process(&ctx, &input, &mut output).unwrap();
271        let out = output.payload().unwrap();
272        assert_eq!(out.status, CuFollowStatus::Idle);
273        assert_eq!(out.positions[0].si_value(), 0.0);
274    }
275
276    #[test]
277    fn test_midpoint_is_running() {
278        let (ctx, _) = CuContext::new_mock_clock();
279        let mut task = CuFollowTrajectoryTask::<1>::default();
280        let mut output = CuMsg::new(None);
281        task.process(&ctx, &msg_at(Some(two_point_traj()), 100), &mut output)
282            .unwrap();
283        assert_eq!(output.payload().unwrap().status, CuFollowStatus::Running);
284        task.process(&ctx, &msg_at(Some(two_point_traj()), 101), &mut output)
285            .unwrap();
286        let out = output.payload().unwrap();
287        assert_eq!(out.status, CuFollowStatus::Running);
288        assert!((out.positions[0].si_value() - 1.0).abs() < 1e-12);
289        assert!((out.velocities[0].si_value() - 2.0).abs() < 1e-12);
290    }
291
292    #[test]
293    fn cubic_follower_samples_position_and_velocity_consistently() {
294        let (ctx, _) = CuContext::new_mock_clock();
295        let mut task = CuFollowTrajectoryTask::<1>::from_params(cubic()).unwrap();
296        let mut output = CuMsg::new(None);
297        task.process(&ctx, &msg_at(Some(two_point_traj()), 100), &mut output)
298            .unwrap();
299        task.process(&ctx, &msg_at(Some(two_point_traj()), 101), &mut output)
300            .unwrap();
301        let out = output.payload().unwrap();
302        assert!((out.positions[0].si_value() - 0.0).abs() < 1e-12);
303        assert!((out.velocities[0].si_value() - 0.5).abs() < 1e-12);
304    }
305
306    #[test]
307    fn freeze_thaw_preserves_cubic_mode_and_clock() {
308        let (ctx, _) = CuContext::new_mock_clock();
309        let mut original = CuFollowTrajectoryTask::<1>::from_params(cubic()).unwrap();
310        let mut output = CuMsg::new(None);
311        original
312            .process(&ctx, &msg_at(Some(two_point_traj()), 100), &mut output)
313            .unwrap();
314        let mut bytes = [0u8; 8192];
315        let mut encoder = bincode::enc::EncoderImpl::new(
316            bincode::enc::write::SliceWriter::new(&mut bytes),
317            bincode::config::standard(),
318        );
319        original.freeze(&mut encoder).unwrap();
320        let len = encoder.into_writer().bytes_written();
321        let mut decoder = bincode::de::DecoderImpl::new(
322            bincode::de::read::SliceReader::new(&bytes[..len]),
323            bincode::config::standard(),
324            (),
325        );
326        let mut restored = CuFollowTrajectoryTask::<1>::default();
327        restored.thaw(&mut decoder).unwrap();
328        restored
329            .process(&ctx, &msg_at(None, 101), &mut output)
330            .unwrap();
331        let out = output.payload().unwrap();
332        assert!((out.positions[0].si_value() - 0.0).abs() < 1e-12);
333        assert!((out.velocities[0].si_value() - 0.5).abs() < 1e-12);
334    }
335
336    #[test]
337    fn test_finished_holds_final_point() {
338        let (ctx, _) = CuContext::new_mock_clock();
339        let mut task = CuFollowTrajectoryTask::<1>::default();
340        let mut output = CuMsg::new(None);
341        task.process(&ctx, &msg_at(Some(two_point_traj()), 100), &mut output)
342            .unwrap();
343        task.process(&ctx, &msg_at(Some(two_point_traj()), 102), &mut output)
344            .unwrap();
345        assert_eq!(output.payload().unwrap().status, CuFollowStatus::Finished);
346        task.process(&ctx, &msg_at(Some(two_point_traj()), 110), &mut output)
347            .unwrap();
348        let out = output.payload().unwrap();
349        assert_eq!(out.status, CuFollowStatus::Finished);
350        assert_eq!(out.positions[0].si_value(), 2.0);
351        assert_eq!(out.velocities[0].si_value(), 4.0);
352    }
353
354    #[test]
355    fn test_new_trajectory_preempts_and_resets_clock() {
356        let (ctx, _) = CuContext::new_mock_clock();
357        let mut task = CuFollowTrajectoryTask::<1>::default();
358        let mut output = CuMsg::new(None);
359        task.process(&ctx, &msg_at(Some(two_point_traj()), 100), &mut output)
360            .unwrap();
361        task.process(&ctx, &msg_at(Some(two_point_traj()), 101), &mut output)
362            .unwrap();
363        let mut other = CuTrajectoryPayload::new();
364        other.push(point(0.0, 10.0, 0.0)).unwrap();
365        other.push(point(4.0, 14.0, 0.0)).unwrap();
366        task.process(&ctx, &msg_at(Some(other), 150), &mut output)
367            .unwrap();
368        let out = output.payload().unwrap();
369        assert_eq!(out.status, CuFollowStatus::Running);
370        assert_eq!(out.positions[0].si_value(), 10.0);
371    }
372
373    #[test]
374    fn test_empty_trajectory_reports_error() {
375        let (ctx, _) = CuContext::new_mock_clock();
376        let mut task = CuFollowTrajectoryTask::<1>::default();
377        let mut output = CuMsg::new(None);
378        task.process(
379            &ctx,
380            &msg_at(Some(CuTrajectoryPayload::new()), 100),
381            &mut output,
382        )
383        .unwrap();
384        assert_eq!(
385            output.payload().unwrap().status,
386            CuFollowStatus::ErrorEmptyTrajectory
387        );
388        task.process(&ctx, &CuMsg::new(None), &mut output).unwrap();
389        assert_eq!(output.payload().unwrap().status, CuFollowStatus::Idle);
390    }
391
392    #[test]
393    fn default_follower_uses_the_payload_interpolation() {
394        let (ctx, _) = CuContext::new_mock_clock();
395        let mut task = CuFollowTrajectoryTask::<1>::default();
396        let mut traj = two_point_traj();
397        traj.interpolation = CuTrajectoryInterpolation::Cubic;
398        let mut output = CuMsg::new(None);
399        task.process(&ctx, &msg_at(Some(traj.clone()), 100), &mut output)
400            .unwrap();
401        task.process(&ctx, &msg_at(Some(traj), 101), &mut output)
402            .unwrap();
403        let out = output.payload().unwrap();
404        assert!((out.positions[0].si_value() - 0.0).abs() < 1e-12);
405        assert!((out.velocities[0].si_value() - 0.5).abs() < 1e-12);
406    }
407
408    #[test]
409    fn explicit_interpolation_overrides_the_payload() {
410        let (ctx, _) = CuContext::new_mock_clock();
411        let mut task = CuFollowTrajectoryTask::<1>::from_params(CuFollowTrajectoryParams {
412            interpolation: Some(CuTrajectoryInterpolation::Linear),
413        })
414        .unwrap();
415        let mut traj = two_point_traj();
416        traj.interpolation = CuTrajectoryInterpolation::Cubic;
417        let mut output = CuMsg::new(None);
418        task.process(&ctx, &msg_at(Some(traj.clone()), 100), &mut output)
419            .unwrap();
420        task.process(&ctx, &msg_at(Some(traj), 101), &mut output)
421            .unwrap();
422        let out = output.payload().unwrap();
423        assert!((out.positions[0].si_value() - 1.0).abs() < 1e-12);
424        assert!((out.velocities[0].si_value() - 2.0).abs() < 1e-12);
425    }
426
427    #[cfg(feature = "std")]
428    #[test]
429    fn reads_config() {
430        let cfg: ComponentConfig =
431            ron::from_str(r#"#![enable(unwrap_newtypes)] {"interpolation": "cubic"}"#).unwrap();
432        let task = CuFollowTrajectoryTask::<1>::new(Some(&cfg), ()).unwrap();
433        assert_eq!(task.params(), &cubic());
434        let task = CuFollowTrajectoryTask::<1>::new(None, ()).unwrap();
435        assert_eq!(task.params().interpolation, None);
436        let typo: ComponentConfig =
437            ron::from_str(r#"#![enable(unwrap_newtypes)] {"interpolaton": "cubic"}"#).unwrap();
438        assert!(CuFollowTrajectoryTask::<1>::new(Some(&typo), ()).is_err());
439    }
440}
repose_trajectory/task.rs

repose_trajectory/
task.rs
