repose_trajectory/
lib.rs

1//! Fixed-capacity joint-space trajectory payload and allocation-free sampling.
2//!
3//! [`CuTrajectoryPayload`] stores up to `MAX_POINTS` timestamped joint waypoints
4//! and the [`CuTrajectoryInterpolation`] that reconstructs the motion between
5//! them. [`CuTrajectoryPayload::sample`] interpolates joint positions and
6//! velocities in place into caller-provided buffers without allocating.
7//!
8//! Joint-space waypoints are timed with [`parameterize`]; Cartesian paths are
9//! built with [`blend_cartesian_path`] and timed with
10//! [`parameterize_cartesian_path`]. Both timing functions share
11//! [`CuTimingParams`] and report a [`CuTimingStatus`], and both set the
12//! payload's interpolation so [`CuFollowTrajectoryTask`] executes the motion
13//! that was validated.
14
15#![cfg_attr(not(feature = "std"), no_std)]
16
17mod cartesian_blend;
18mod parameterize;
19mod path_timing;
20mod quintic;
21mod serde_arrays;
22mod task;
23#[cfg(test)]
24mod test_support;
25
26pub use cartesian_blend::CuCartesianBlendParams;
27pub use cartesian_blend::CuCartesianBlendPath;
28pub use cartesian_blend::CuCartesianBlendStatus;
29pub use cartesian_blend::CuCartesianPathPoint;
30pub use cartesian_blend::CuCartesianWaypoint;
31pub use cartesian_blend::CuTcpZone;
32pub use cartesian_blend::blend_cartesian_path;
33pub use parameterize::CuJointTrajectoryLimits;
34pub use parameterize::CuTimingOverrides;
35pub use parameterize::CuTimingParams;
36pub use parameterize::CuTimingStatus;
37pub use parameterize::CuTrajectoryProfile;
38pub use parameterize::parameterize;
39pub use path_timing::CuPathTimingParams;
40pub use path_timing::parameterize_cartesian_path;
41pub use task::CuFollowStatus;
42pub use task::CuFollowTrajectoryParams;
43pub use task::CuFollowTrajectoryTask;
44pub use task::CuTrajectoryFollowOutput;
45
46use bincode::Decode;
47use bincode::Encode;
48use cu29::prelude::*;
49use cu29::units::si::angular_velocity::radian_per_second;
50use cu29::units::si::f64::AngularVelocity;
51use cu29::units::si::f64::Time;
52use cu29::units::si::f64::Velocity;
53use cu29::units::si::time::second;
54use cu29::units::si::velocity::meter_per_second;
55use repose_kinematics::CuJointAcceleration;
56use repose_kinematics::CuJointPosition;
57use repose_kinematics::CuJointVelocity;
58use serde::Deserialize;
59use serde::Serialize;
60
61/// Maximum number of waypoints in a [`CuTrajectoryPayload`].
62pub const MAX_TRAJECTORY_POINTS: usize = 64;
63
64/// Single timestamped joint waypoint with positions and velocities.
65#[derive(Debug, Clone, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect)]
66#[reflect(no_field_bounds, from_reflect = false)]
67pub struct CuTrajectoryPoint<const N: usize> {
68    /// Elapsed time since trajectory start.
69    pub time_from_start: Time,
70    #[serde(with = "repose_kinematics::serde_arrays::position_array")]
71    pub positions: [CuJointPosition; N],
72    #[serde(with = "repose_kinematics::serde_arrays::velocity_array")]
73    pub velocities: [CuJointVelocity; N],
74    /// Joint accelerations (`rad/s^2` revolute, `m/s^2` prismatic).
75    ///
76    /// Only [`CuTrajectoryPayload::sample_quintic`] reads them; trajectories
77    /// that rest at, or have zero acceleration at, every waypoint store zero.
78    #[serde(with = "repose_kinematics::serde_arrays::acceleration_array")]
79    pub accelerations: [CuJointAcceleration; N],
80}
81
82impl<const N: usize> Default for CuTrajectoryPoint<N> {
83    fn default() -> Self {
84        Self {
85            time_from_start: Time::default(),
86            positions: [CuJointPosition::default(); N],
87            velocities: [CuJointVelocity::default(); N],
88            accelerations: [CuJointAcceleration::default(); N],
89        }
90    }
91}
92
93/// Reconstruction of the motion between stored trajectory points.
94///
95/// Stored on every [`CuTrajectoryPayload`] (set by the parameterizer that
96/// produced it) and used by [`CuTrajectoryPayload::sample`].
97#[derive(
98    Default, Debug, Clone, Copy, PartialEq, Eq, Encode, Decode, Serialize, Deserialize, Reflect,
99)]
100#[serde(rename_all = "snake_case")]
101pub enum CuTrajectoryInterpolation {
102    /// Positions and velocities are interpolated linearly
103    /// ([`CuTrajectoryPayload::sample_linear`]).
104    #[default]
105    Linear,
106    /// Position and velocity are reconstructed using cubic Hermite interpolation
107    /// ([`CuTrajectoryPayload::sample_cubic`]).
108    Cubic,
109    /// Position and velocity are reconstructed with the quintic Hermite motion
110    /// through the stored positions, velocities and accelerations
111    /// ([`CuTrajectoryPayload::sample_quintic`]).
112    Quintic,
113}
114
115/// Fixed-capacity timestamped joint trajectory (`CuMsgPayload`-compliant).
116#[derive(Debug, Clone, PartialEq, Serialize, Deserialize, Reflect)]
117#[reflect(no_field_bounds, from_reflect = false)]
118pub struct CuTrajectoryPayload<const N: usize, const MAX_POINTS: usize = MAX_TRAJECTORY_POINTS> {
119    #[serde(with = "crate::serde_arrays::trajectory_point_array")]
120    pub points: [CuTrajectoryPoint<N>; MAX_POINTS],
121    /// Number of valid waypoints in `points[..len]`.
122    pub len: u32,
123    /// How [`CuTrajectoryPayload::sample`] reconstructs the motion between
124    /// points. [`parameterize`] and [`parameterize_cartesian_path`] set it to
125    /// the reconstruction their limits were validated on; hand-built
126    /// trajectories default to [`CuTrajectoryInterpolation::Linear`].
127    #[serde(default)]
128    pub interpolation: CuTrajectoryInterpolation,
129}
130
131// The bincode derive currently emits invalid impl generics for const defaults.
132impl<const N: usize, const MAX_POINTS: usize> Encode for CuTrajectoryPayload<N, MAX_POINTS> {
133    fn encode<E: bincode::enc::Encoder>(
134        &self,
135        encoder: &mut E,
136    ) -> Result<(), bincode::error::EncodeError> {
137        Encode::encode(&self.points, encoder)?;
138        Encode::encode(&self.len, encoder)?;
139        Encode::encode(&self.interpolation, encoder)
140    }
141}
142impl<C, const N: usize, const MAX_POINTS: usize> Decode<C> for CuTrajectoryPayload<N, MAX_POINTS> {
143    fn decode<D: bincode::de::Decoder<Context = C>>(
144        decoder: &mut D,
145    ) -> Result<Self, bincode::error::DecodeError> {
146        Ok(Self {
147            points: Decode::decode(decoder)?,
148            len: Decode::decode(decoder)?,
149            interpolation: Decode::decode(decoder)?,
150        })
151    }
152}
153impl<'de, C, const N: usize, const MAX_POINTS: usize> bincode::BorrowDecode<'de, C>
154    for CuTrajectoryPayload<N, MAX_POINTS>
155{
156    fn borrow_decode<D: bincode::de::BorrowDecoder<'de, Context = C>>(
157        decoder: &mut D,
158    ) -> Result<Self, bincode::error::DecodeError> {
159        Decode::decode(decoder)
160    }
161}
162
163impl<const N: usize, const MAX_POINTS: usize> Default for CuTrajectoryPayload<N, MAX_POINTS> {
164    fn default() -> Self {
165        Self::new()
166    }
167}
168
169impl<const N: usize, const MAX_POINTS: usize> CuTrajectoryPayload<N, MAX_POINTS> {
170    /// Creates an empty trajectory with [`CuTrajectoryInterpolation::Linear`].
171    pub fn new() -> Self {
172        Self {
173            points: core::array::from_fn(|_| CuTrajectoryPoint::default()),
174            len: 0,
175            interpolation: CuTrajectoryInterpolation::Linear,
176        }
177    }
178
179    /// Returns `true` when the trajectory holds no waypoints.
180    pub const fn is_empty(&self) -> bool {
181        self.len == 0
182    }
183
184    /// Returns the number of valid waypoints.
185    pub const fn len(&self) -> usize {
186        self.len as usize
187    }
188
189    /// Returns the last waypoint time, or zero when empty.
190    pub fn duration(&self) -> Time {
191        if self.is_empty() {
192            Time::default()
193        } else {
194            self.points[self.len as usize - 1].time_from_start
195        }
196    }
197
198    /// Borrows the valid waypoint prefix.
199    pub fn points(&self) -> &[CuTrajectoryPoint<N>] {
200        &self.points[..self.len as usize]
201    }
202
203    /// Appends a waypoint; times must be strictly increasing.
204    pub fn push(&mut self, point: CuTrajectoryPoint<N>) -> CuResult<()> {
205        if self.len as usize >= MAX_POINTS {
206            return Err(CuError::from("Trajectory is full"));
207        }
208        if !self.is_empty()
209            && point.time_from_start <= self.points[self.len as usize - 1].time_from_start
210        {
211            return Err(CuError::from(
212                "Trajectory times must be strictly increasing",
213            ));
214        }
215        self.points[self.len as usize] = point;
216        self.len += 1;
217        Ok(())
218    }
219
220    /// Drops all waypoints; the interpolation is kept.
221    pub fn clear(&mut self) {
222        self.len = 0;
223    }
224
225    /// Samples joint positions and velocities at `t` in place with the
226    /// payload's [`interpolation`](Self::interpolation).
227    ///
228    /// Dispatches to [`sample_linear`](Self::sample_linear),
229    /// [`sample_cubic`](Self::sample_cubic) or
230    /// [`sample_quintic`](Self::sample_quintic). Sampling before the first or
231    /// after the last waypoint holds that endpoint.
232    pub fn sample(
233        &self,
234        t: Time,
235        out_positions: &mut [CuJointPosition; N],
236        out_velocities: &mut [CuJointVelocity; N],
237    ) -> CuResult<()> {
238        self.sample_with(self.interpolation, t, out_positions, out_velocities)
239    }
240
241    /// Samples at `t` with an explicit `interpolation`, ignoring the payload's.
242    pub fn sample_with(
243        &self,
244        interpolation: CuTrajectoryInterpolation,
245        t: Time,
246        out_positions: &mut [CuJointPosition; N],
247        out_velocities: &mut [CuJointVelocity; N],
248    ) -> CuResult<()> {
249        match interpolation {
250            CuTrajectoryInterpolation::Linear => {
251                self.sample_linear(t, out_positions, out_velocities)
252            }
253            CuTrajectoryInterpolation::Cubic => self.sample_cubic(t, out_positions, out_velocities),
254            CuTrajectoryInterpolation::Quintic => {
255                self.sample_quintic(t, out_positions, out_velocities)
256            }
257        }
258    }
259
260    /// Samples joint positions and velocities at `t` in place by linear
261    /// interpolation between the bracketing waypoints.
262    ///
263    /// `t` is clamped to `[0, duration]`; sampling before the first or after
264    /// the last waypoint holds that endpoint. Errors on an empty trajectory or
265    /// on mismatched joint coordinate types.
266    pub fn sample_linear(
267        &self,
268        t: Time,
269        out_positions: &mut [CuJointPosition; N],
270        out_velocities: &mut [CuJointVelocity; N],
271    ) -> CuResult<()> {
272        if self.is_empty() {
273            return Err(CuError::from("Cannot sample an empty trajectory"));
274        }
275        if self.len() == 1 {
276            *out_positions = self.points[0].positions;
277            *out_velocities = self.points[0].velocities;
278            return Ok(());
279        }
280        let pts = self.points();
281        let duration = self.duration();
282        let zero = Time::default();
283        let clamped = if t <= zero {
284            zero
285        } else if t >= duration {
286            duration
287        } else {
288            t
289        };
290        let t_s = clamped.get::<second>();
291        let last = pts.len() - 1;
292        if t_s <= pts[0].time_from_start.get::<second>() {
293            *out_positions = pts[0].positions;
294            *out_velocities = pts[0].velocities;
295            return Ok(());
296        }
297        if t_s >= pts[last].time_from_start.get::<second>() {
298            *out_positions = pts[last].positions;
299            *out_velocities = pts[last].velocities;
300            return Ok(());
301        }
302        let mut i = 0;
303        while i < last && pts[i + 1].time_from_start.get::<second>() < t_s {
304            i += 1;
305        }
306        let t0 = pts[i].time_from_start.get::<second>();
307        let t1 = pts[i + 1].time_from_start.get::<second>();
308        let fraction = (t_s - t0) / (t1 - t0);
309        let mut j = 0;
310        while j < N {
311            out_positions[j] =
312                pts[i].positions[j].interpolate(pts[i + 1].positions[j], fraction)?;
313            out_velocities[j] =
314                lerp_velocity(pts[i].velocities[j], pts[i + 1].velocities[j], fraction)?;
315            j += 1;
316        }
317        Ok(())
318    }
319
320    /// Samples cubic Hermite interpolation using the stored positions and velocities.
321    ///
322    /// [`parameterize`] and the non-S-curve profiles of
323    /// [`parameterize_cartesian_path`] validate their limits on this
324    /// reconstruction; it preserves the continuous position and velocity at
325    /// each stored knot.
326    pub fn sample_cubic(
327        &self,
328        t: Time,
329        out_positions: &mut [CuJointPosition; N],
330        out_velocities: &mut [CuJointVelocity; N],
331    ) -> CuResult<()> {
332        if self.is_empty() || self.len() > MAX_POINTS || !t.get::<second>().is_finite() {
333            return Err(CuError::from("Invalid cubic trajectory or sample time"));
334        }
335        let pts = self.points();
336        let ts = t.get::<second>();
337        if ts <= pts[0].time_from_start.get::<second>() {
338            *out_positions = pts[0].positions;
339            *out_velocities = pts[0].velocities;
340            return Ok(());
341        }
342        let last = pts.len() - 1;
343        if ts >= pts[last].time_from_start.get::<second>() {
344            *out_positions = pts[last].positions;
345            *out_velocities = pts[last].velocities;
346            return Ok(());
347        }
348        let mut i = 0;
349        while i < last && pts[i + 1].time_from_start.get::<second>() < ts {
350            i += 1;
351        }
352        let a = &pts[i];
353        let b = &pts[i + 1];
354        let t0 = a.time_from_start.get::<second>();
355        let duration = b.time_from_start.get::<second>() - t0;
356        if !duration.is_finite() || duration <= 0.0 {
357            return Err(CuError::from("Invalid trajectory segment duration"));
358        }
359        let u = (ts - t0) / duration;
360        for j in 0..N {
361            if core::mem::discriminant(&a.positions[j]) != core::mem::discriminant(&b.positions[j])
362                || core::mem::discriminant(&a.velocities[j])
363                    != core::mem::discriminant(&b.velocities[j])
364                || !matches!(
365                    (a.positions[j], a.velocities[j]),
366                    (CuJointPosition::Revolute(_), CuJointVelocity::Revolute(_))
367                        | (CuJointPosition::Prismatic(_), CuJointVelocity::Prismatic(_))
368                        | (CuJointPosition::Fixed, CuJointVelocity::Fixed)
369                )
370            {
371                return Err(CuError::from("Trajectory joint kinds disagree"));
372            }
373            let q0 = a.positions[j].si_value();
374            let q1 = b.positions[j].si_value();
375            let v0 = a.velocities[j].si_value();
376            let v1 = b.velocities[j].si_value();
377            if !q0.is_finite() || !q1.is_finite() || !v0.is_finite() || !v1.is_finite() {
378                return Err(CuError::from("Non-finite trajectory knot"));
379            }
380            let d = q1 - q0;
381            let aa = v0 * duration;
382            let bb = 3.0 * d - 2.0 * aa - v1 * duration;
383            let cc = -2.0 * d + aa + v1 * duration;
384            out_positions[j] = a.positions[j].with_si_value(q0 + u * (aa + u * (bb + u * cc)));
385            out_velocities[j] = CuJointVelocity::from_coordinate(
386                a.positions[j],
387                (aa + u * (2.0 * bb + 3.0 * cc * u)) / duration,
388            );
389        }
390        Ok(())
391    }
392
393    /// Samples the quintic Hermite motion through the stored knots.
394    ///
395    /// Each segment is the quintic matching position, velocity and
396    /// acceleration at both knots, so all three are continuous across the
397    /// trajectory and jerk is finite. [`parameterize_cartesian_path`] with the
398    /// S-curve profile verifies its limits on exactly this reconstruction. Sampling before the first
399    /// or after the last knot holds that endpoint.
400    pub fn sample_quintic(
401        &self,
402        t: Time,
403        out_positions: &mut [CuJointPosition; N],
404        out_velocities: &mut [CuJointVelocity; N],
405    ) -> CuResult<()> {
406        if self.is_empty() || self.len() > MAX_POINTS || !t.get::<second>().is_finite() {
407            return Err(CuError::from("Invalid quintic trajectory or sample time"));
408        }
409        let pts = self.points();
410        let ts = t.get::<second>();
411        let last = pts.len() - 1;
412        if ts <= pts[0].time_from_start.get::<second>() {
413            *out_positions = pts[0].positions;
414            *out_velocities = pts[0].velocities;
415            return Ok(());
416        }
417        if ts >= pts[last].time_from_start.get::<second>() {
418            *out_positions = pts[last].positions;
419            *out_velocities = pts[last].velocities;
420            return Ok(());
421        }
422        let mut i = 0;
423        while i < last && pts[i + 1].time_from_start.get::<second>() < ts {
424            i += 1;
425        }
426        let a = &pts[i];
427        let b = &pts[i + 1];
428        let t0 = a.time_from_start.get::<second>();
429        let duration = b.time_from_start.get::<second>() - t0;
430        if !duration.is_finite() || duration <= 0.0 {
431            return Err(CuError::from("Invalid trajectory segment duration"));
432        }
433        let u = (ts - t0) / duration;
434        for j in 0..N {
435            let kinds_agree = matches!(
436                (
437                    a.positions[j],
438                    b.positions[j],
439                    a.velocities[j],
440                    b.velocities[j]
441                ),
442                (
443                    CuJointPosition::Revolute(_),
444                    CuJointPosition::Revolute(_),
445                    CuJointVelocity::Revolute(_),
446                    CuJointVelocity::Revolute(_)
447                ) | (
448                    CuJointPosition::Prismatic(_),
449                    CuJointPosition::Prismatic(_),
450                    CuJointVelocity::Prismatic(_),
451                    CuJointVelocity::Prismatic(_)
452                ) | (
453                    CuJointPosition::Fixed,
454                    CuJointPosition::Fixed,
455                    CuJointVelocity::Fixed,
456                    CuJointVelocity::Fixed
457                )
458            );
459            if !kinds_agree {
460                return Err(CuError::from("Trajectory joint kinds disagree"));
461            }
462            let quintic = quintic::Quintic::new(
463                [a.positions[j].si_value(), b.positions[j].si_value()],
464                [a.velocities[j].si_value(), b.velocities[j].si_value()],
465                [a.accelerations[j].si_value(), b.accelerations[j].si_value()],
466                duration,
467            );
468            let (q, v, _) = quintic.eval(u);
469            if !q.is_finite() || !v.is_finite() {
470                return Err(CuError::from("Non-finite trajectory knot"));
471            }
472            out_positions[j] = a.positions[j].with_si_value(q);
473            out_velocities[j] = CuJointVelocity::from_coordinate(a.positions[j], v);
474        }
475        Ok(())
476    }
477}
478
479/// Linearly interpolates joint velocities of the same joint type.
480fn lerp_velocity(
481    a: CuJointVelocity,
482    b: CuJointVelocity,
483    fraction: f64,
484) -> CuResult<CuJointVelocity> {
485    if !fraction.is_finite() {
486        return Err(CuError::from("Non-finite interpolation fraction"));
487    }
488    match (a, b) {
489        (CuJointVelocity::Revolute(x), CuJointVelocity::Revolute(y)) => {
490            let xv = x.get::<radian_per_second>();
491            let yv = y.get::<radian_per_second>();
492            Ok(CuJointVelocity::Revolute(AngularVelocity::new::<
493                radian_per_second,
494            >(
495                xv + fraction * (yv - xv)
496            )))
497        }
498        (CuJointVelocity::Prismatic(x), CuJointVelocity::Prismatic(y)) => {
499            let xv = x.get::<meter_per_second>();
500            let yv = y.get::<meter_per_second>();
501            Ok(CuJointVelocity::Prismatic(
502                Velocity::new::<meter_per_second>(xv + fraction * (yv - xv)),
503            ))
504        }
505        (CuJointVelocity::Fixed, CuJointVelocity::Fixed) => Ok(CuJointVelocity::Fixed),
506        _ => Err(CuError::from("Mismatched joint velocity types")),
507    }
508}
509
510#[cfg(test)]
511mod tests {
512    use super::*;
513    use cu29::units::si::angle::radian;
514
515    fn point(t: f64, q: f64, v: f64) -> CuTrajectoryPoint<1> {
516        CuTrajectoryPoint {
517            time_from_start: Time::new::<second>(t),
518            positions: [CuJointPosition::Revolute(
519                cu29::units::si::f64::Angle::new::<radian>(q),
520            )],
521            velocities: [CuJointVelocity::Revolute(AngularVelocity::new::<
522                radian_per_second,
523            >(v))],
524            ..Default::default()
525        }
526    }
527
528    fn two_point_traj() -> CuTrajectoryPayload<1> {
529        let mut traj = CuTrajectoryPayload::new();
530        traj.push(point(0.0, 0.0, 0.0)).unwrap();
531        traj.push(point(2.0, 2.0, 4.0)).unwrap();
532        traj
533    }
534
535    #[test]
536    fn test_push_duration_and_points() {
537        let traj = two_point_traj();
538        assert_eq!(traj.len(), 2);
539        assert!(!traj.is_empty());
540        assert_eq!(traj.duration().get::<second>(), 2.0);
541        assert_eq!(traj.points().len(), 2);
542    }
543
544    #[test]
545    fn test_push_rejects_full_and_non_monotonic() {
546        let mut traj = CuTrajectoryPayload::<1, 2>::new();
547        traj.push(point(0.0, 0.0, 0.0)).unwrap();
548        traj.push(point(1.0, 1.0, 1.0)).unwrap();
549        assert!(traj.push(point(2.0, 2.0, 2.0)).is_err());
550        let mut traj = CuTrajectoryPayload::<1>::new();
551        traj.push(point(1.0, 0.0, 0.0)).unwrap();
552        assert!(traj.push(point(1.0, 1.0, 1.0)).is_err());
553        assert!(traj.push(point(0.5, 1.0, 1.0)).is_err());
554        assert_eq!(traj.len(), 1);
555        traj.clear();
556        assert!(traj.is_empty());
557        assert_eq!(traj.duration().get::<second>(), 0.0);
558    }
559
560    #[test]
561    fn test_sample_endpoints_midpoint_and_clamping() {
562        let traj = two_point_traj();
563        let mut q = [CuJointPosition::default(); 1];
564        let mut qd = [CuJointVelocity::default(); 1];
565        traj.sample(Time::new::<second>(0.0), &mut q, &mut qd)
566            .unwrap();
567        assert_eq!(q[0].si_value(), 0.0);
568        traj.sample(Time::new::<second>(1.0), &mut q, &mut qd)
569            .unwrap();
570        assert!((q[0].si_value() - 1.0).abs() < 1e-12);
571        assert!((qd[0].si_value() - 2.0).abs() < 1e-12);
572        traj.sample(Time::new::<second>(2.0), &mut q, &mut qd)
573            .unwrap();
574        assert_eq!(q[0].si_value(), 2.0);
575        traj.sample(Time::new::<second>(-5.0), &mut q, &mut qd)
576            .unwrap();
577        assert_eq!(q[0].si_value(), 0.0);
578        traj.sample(Time::new::<second>(99.0), &mut q, &mut qd)
579            .unwrap();
580        assert_eq!(q[0].si_value(), 2.0);
581        assert_eq!(qd[0].si_value(), 4.0);
582    }
583
584    #[test]
585    fn test_sample_empty_errors() {
586        let traj = CuTrajectoryPayload::<1>::new();
587        let mut q = [CuJointPosition::default(); 1];
588        let mut qd = [CuJointVelocity::default(); 1];
589        assert!(
590            traj.sample(Time::new::<second>(0.0), &mut q, &mut qd)
591                .is_err()
592        );
593    }
594
595    #[test]
596    fn test_sample_dispatches_on_payload_interpolation() {
597        let mut traj = two_point_traj();
598        let mut q = [CuJointPosition::default(); 1];
599        let mut qd = [CuJointVelocity::default(); 1];
600        let mut q2 = q;
601        let mut qd2 = qd;
602        let t = Time::new::<second>(1.0);
603        for interpolation in [
604            CuTrajectoryInterpolation::Linear,
605            CuTrajectoryInterpolation::Cubic,
606            CuTrajectoryInterpolation::Quintic,
607        ] {
608            traj.interpolation = interpolation;
609            traj.sample(t, &mut q, &mut qd).unwrap();
610            match interpolation {
611                CuTrajectoryInterpolation::Linear => traj.sample_linear(t, &mut q2, &mut qd2),
612                CuTrajectoryInterpolation::Cubic => traj.sample_cubic(t, &mut q2, &mut qd2),
613                CuTrajectoryInterpolation::Quintic => traj.sample_quintic(t, &mut q2, &mut qd2),
614            }
615            .unwrap();
616            assert_eq!((q, qd), (q2, qd2), "{interpolation:?}");
617        }
618    }
619
620    #[test]
621    fn test_bincode_round_trip_keeps_interpolation() {
622        let mut traj = CuTrajectoryPayload::<1, 4>::new();
623        traj.push(point(0.0, 0.0, 0.0)).unwrap();
624        traj.push(point(1.0, 1.0, 0.0)).unwrap();
625        traj.interpolation = CuTrajectoryInterpolation::Quintic;
626        let mut bytes = [0u8; 1024];
627        let len =
628            bincode::encode_into_slice(&traj, &mut bytes, bincode::config::standard()).unwrap();
629        let (decoded, _): (CuTrajectoryPayload<1, 4>, usize) =
630            bincode::decode_from_slice(&bytes[..len], bincode::config::standard()).unwrap();
631        assert_eq!(decoded, traj);
632    }
633
634    #[test]
635    fn test_stretch_time_scales_and_limit_ratio_drops() {
636        let mut traj = CuTrajectoryPayload::<1, 4>::new();
637        traj.push(point(0.0, 0.0, 0.0)).unwrap();
638        traj.push(point(1.0, 1.0, 0.0)).unwrap();
639        let limits = CuJointTrajectoryLimits::new(
640            [CuJointVelocity::Revolute(AngularVelocity::new::<
641                radian_per_second,
642            >(1.0))],
643            [CuJointAcceleration::from_coordinate(
644                traj.points()[0].positions[0],
645                10.0,
646            )],
647        );
648        let before = traj.quintic_limit_ratio(&limits).unwrap();
649        traj.stretch_time(2.0).unwrap();
650        assert_eq!(traj.duration().get::<second>(), 2.0);
651        let after = traj.quintic_limit_ratio(&limits).unwrap();
652        assert!(after < before);
653        assert!(traj.stretch_time(0.0).is_err());
654    }
655}
repose_trajectory/lib.rs

repose_trajectory/
lib.rs
