deke_types/
traj.rs

1use std::time::Duration;
2
3use num_traits::Float;
4
5use crate::{DekeError, DekeResult, RobotPath, SRobotPath, SRobotQ};
6
7/// Dynamically-sized robot trajectory: a dense [`RobotPath`] sampled uniformly at `dt`.
8#[derive(Debug, Clone)]
9pub struct RobotTraj<F: Float = f32> {
10    dt: Duration,
11    path: RobotPath<F>,
12}
13
14impl<F: Float> RobotTraj<F> {
15    pub fn new(dt: Duration, path: RobotPath<F>) -> Self {
16        Self { dt, path }
17    }
18
19    pub fn dt(&self) -> Duration {
20        self.dt
21    }
22
23    pub fn path(&self) -> &RobotPath<F> {
24        &self.path
25    }
26
27    pub fn path_mut(&mut self) -> &mut RobotPath<F> {
28        &mut self.path
29    }
30
31    pub fn into_path(self) -> RobotPath<F> {
32        self.path
33    }
34
35    pub fn len(&self) -> usize {
36        self.path.nrows()
37    }
38
39    pub fn is_empty(&self) -> bool {
40        self.path.nrows() == 0
41    }
42
43    pub fn dof(&self) -> usize {
44        self.path.ncols()
45    }
46
47    pub fn duration(&self) -> Duration {
48        self.dt.saturating_mul(self.len().saturating_sub(1) as u32)
49    }
50
51    pub fn time_at(&self, index: usize) -> Duration {
52        self.dt.saturating_mul(index as u32)
53    }
54}
55
56/// Statically-sized robot trajectory: an [`SRobotPath`] sampled uniformly at `dt`.
57#[derive(Debug, Clone)]
58pub struct SRobotTraj<const N: usize, F: Float = f32> {
59    dt: Duration,
60    path: SRobotPath<N, F>,
61}
62
63impl<const N: usize, F: Float> SRobotTraj<N, F> {
64    pub fn new(dt: Duration, path: SRobotPath<N, F>) -> Self {
65        Self { dt, path }
66    }
67
68    pub fn try_from_waypoints(dt: Duration, waypoints: Vec<SRobotQ<N, F>>) -> DekeResult<Self> {
69        Ok(Self {
70            dt,
71            path: SRobotPath::try_new(waypoints)?,
72        })
73    }
74
75    pub fn dt(&self) -> Duration {
76        self.dt
77    }
78
79    pub fn set_dt(&mut self, dt: Duration) {
80        self.dt = dt;
81    }
82
83    pub fn path(&self) -> &SRobotPath<N, F> {
84        &self.path
85    }
86
87    pub fn path_mut(&mut self) -> &mut SRobotPath<N, F> {
88        &mut self.path
89    }
90
91    pub fn into_path(self) -> SRobotPath<N, F> {
92        self.path
93    }
94
95    pub fn len(&self) -> usize {
96        self.path.len()
97    }
98
99    pub fn is_empty(&self) -> bool {
100        self.len() == 0
101    }
102
103    pub fn dof(&self) -> usize {
104        N
105    }
106
107    pub fn first(&self) -> &SRobotQ<N, F> {
108        self.path.first()
109    }
110
111    pub fn last(&self) -> &SRobotQ<N, F> {
112        self.path.last()
113    }
114
115    pub fn get(&self, index: usize) -> Option<&SRobotQ<N, F>> {
116        self.path.get(index)
117    }
118
119    pub fn duration(&self) -> Duration {
120        self.dt.saturating_mul(self.len().saturating_sub(1) as u32)
121    }
122
123    pub fn time_at(&self, index: usize) -> Duration {
124        self.dt.saturating_mul(index as u32)
125    }
126
127    pub fn iter(&self) -> std::slice::Iter<'_, SRobotQ<N, F>> {
128        self.path.iter()
129    }
130
131    pub fn iter_timed(&self) -> impl Iterator<Item = (Duration, &SRobotQ<N, F>)> + '_ {
132        let dt = self.dt;
133        self.path
134            .iter()
135            .enumerate()
136            .map(move |(i, q)| (dt.saturating_mul(i as u32), q))
137    }
138
139    /// Samples the trajectory at a given wall-clock time via linear interpolation between
140    /// adjacent waypoints. Times outside `[0, duration()]` are clamped.
141    pub fn sample_at_time(&self, t: Duration) -> Option<SRobotQ<N, F>> {
142        let n = self.len();
143        if n == 0 {
144            return None;
145        }
146        if n == 1 {
147            return Some(*self.first());
148        }
149
150        let dt_secs = self.dt.as_secs_f64();
151        if dt_secs <= 0.0 {
152            return Some(*self.first());
153        }
154
155        let t_secs = t.as_secs_f64();
156        let max_secs = dt_secs * (n - 1) as f64;
157        let clamped = t_secs.clamp(0.0, max_secs);
158
159        let f = clamped / dt_secs;
160        let i = (f.floor() as usize).min(n - 2);
161        let local = F::from(f - i as f64).unwrap_or_else(F::zero);
162
163        let a = self.path.get(i)?;
164        let b = self.path.get(i + 1)?;
165        Some(a.interpolate(b, local))
166    }
167
168    /// Finite-difference velocity at sample index `i`, in joint-units per second.
169    /// Uses forward difference at the start, backward at the end, central elsewhere.
170    pub fn velocity_at(&self, i: usize) -> Option<SRobotQ<N, F>> {
171        let n = self.len();
172        if n < 2 || i >= n {
173            return None;
174        }
175        let dt_secs = F::from(self.dt.as_secs_f64()).unwrap_or_else(F::zero);
176        if dt_secs == F::zero() {
177            return Some(SRobotQ::zeros());
178        }
179
180        let (a, b, span) = if i == 0 {
181            (*self.path.get(0)?, *self.path.get(1)?, F::one())
182        } else if i == n - 1 {
183            (*self.path.get(n - 2)?, *self.path.get(n - 1)?, F::one())
184        } else {
185            (
186                *self.path.get(i - 1)?,
187                *self.path.get(i + 1)?,
188                F::from(2.0).unwrap_or_else(F::one),
189            )
190        };
191
192        Some((b - a) * (F::one() / (span * dt_secs)))
193    }
194
195    /// Finite-difference acceleration at sample index `i`, in joint-units per second^2.
196    pub fn acceleration_at(&self, i: usize) -> Option<SRobotQ<N, F>> {
197        let n = self.len();
198        if n < 3 || i >= n {
199            return None;
200        }
201        let dt_secs = F::from(self.dt.as_secs_f64()).unwrap_or_else(F::zero);
202        if dt_secs == F::zero() {
203            return Some(SRobotQ::zeros());
204        }
205
206        let idx = i.clamp(1, n - 2);
207        let a = *self.path.get(idx - 1)?;
208        let b = *self.path.get(idx)?;
209        let c = *self.path.get(idx + 1)?;
210        let inv_dt_sq = F::one() / (dt_secs * dt_secs);
211        Some((a - b * F::from(2.0).unwrap_or_else(F::one) + c) * inv_dt_sq)
212    }
213
214    pub fn max_joint_velocity(&self) -> F {
215        let mut peak = F::zero();
216        for i in 0..self.len() {
217            if let Some(v) = self.velocity_at(i) {
218                peak = peak.max(v.linf_norm());
219            }
220        }
221        peak
222    }
223
224    pub fn max_joint_acceleration(&self) -> F {
225        let mut peak = F::zero();
226        for i in 0..self.len() {
227            if let Some(a) = self.acceleration_at(i) {
228                peak = peak.max(a.linf_norm());
229            }
230        }
231        peak
232    }
233
234    /// Reverses the trajectory in place (swaps start and end, preserves `dt`).
235    pub fn reverse(&mut self) {
236        self.path.reverse();
237    }
238
239    pub fn reversed(&self) -> Self {
240        Self {
241            dt: self.dt,
242            path: self.path.reversed(),
243        }
244    }
245
246    /// Rescales the trajectory by changing `dt`. `scale > 1.0` slows it down; `< 1.0` speeds it up.
247    pub fn rescale_time(&mut self, scale: f64) {
248        let new_secs = self.dt.as_secs_f64() * scale.max(0.0);
249        self.dt = Duration::from_secs_f64(new_secs);
250    }
251
252    pub fn to_robot_traj(&self) -> RobotTraj<F> {
253        RobotTraj {
254            dt: self.dt,
255            path: self.path.to_robot_path(),
256        }
257    }
258}
259
260impl<const N: usize, F: Float> std::ops::Index<usize> for SRobotTraj<N, F> {
261    type Output = SRobotQ<N, F>;
262    #[inline]
263    fn index(&self, i: usize) -> &SRobotQ<N, F> {
264        &self.path[i]
265    }
266}
267
268impl<'a, const N: usize, F: Float> IntoIterator for &'a SRobotTraj<N, F> {
269    type Item = &'a SRobotQ<N, F>;
270    type IntoIter = std::slice::Iter<'a, SRobotQ<N, F>>;
271
272    fn into_iter(self) -> Self::IntoIter {
273        self.path.iter()
274    }
275}
276
277impl<const N: usize, F: Float> AsRef<SRobotPath<N, F>> for SRobotTraj<N, F> {
278    fn as_ref(&self) -> &SRobotPath<N, F> {
279        &self.path
280    }
281}
282
283impl<const N: usize, F: Float> From<SRobotTraj<N, F>> for SRobotPath<N, F> {
284    fn from(traj: SRobotTraj<N, F>) -> Self {
285        traj.path
286    }
287}
288
289impl<const N: usize, F: Float> From<SRobotTraj<N, F>> for RobotTraj<F> {
290    fn from(traj: SRobotTraj<N, F>) -> Self {
291        traj.to_robot_traj()
292    }
293}
294
295impl<const N: usize, F: Float> TryFrom<RobotTraj<F>> for SRobotTraj<N, F> {
296    type Error = DekeError;
297
298    fn try_from(traj: RobotTraj<F>) -> Result<Self, Self::Error> {
299        Ok(Self {
300            dt: traj.dt,
301            path: SRobotPath::<N, F>::try_from(traj.path)?,
302        })
303    }
304}
deke_types/traj.rs

deke_types/
traj.rs
