cartesian_trajectories/
velocity_profile.rs

1// Copyright (c) 2021 Marco Boneberger
2// Licensed under the EUPL-1.2-or-later
3use s_curve::{
4    s_curve_generator, Derivative, SCurveConstraints, SCurveInput, SCurveStartConditions,
5};
6use std::f64::consts::PI;
7use std::time::Duration;
8
9/// output of the get function of a [`VelocityProfileMapping`](`VelocityProfileMapping`)
10#[derive(Debug)]
11pub struct VelocityProfileOutput {
12    /// progress between 0 and 1
13    pub progress: f64,
14    /// should be true when the progress is >= 1
15    pub finished: bool,
16}
17/// Custom Velocity Profile
18pub struct VelocityProfileMapping {
19    function: Box<dyn Fn(Duration) -> VelocityProfileOutput>,
20    total_duration: Duration,
21}
22impl VelocityProfileMapping {
23    /// evaluates the velocity profile function
24    pub fn get(&mut self, time: Duration) -> VelocityProfileOutput {
25        (self.function)(time)
26    }
27    /// creates a new VelocityProfileMapping by defining a velocity profile function.
28    /// This function should map a duration monotonically to progress between 0 and 1.
29    /// Further the total duration of the trajectory should be specified as it helps the user.
30    pub fn new(
31        function: Box<dyn Fn(Duration) -> VelocityProfileOutput>,
32        total_duration: Duration,
33    ) -> VelocityProfileMapping {
34        VelocityProfileMapping {
35            function,
36            total_duration,
37        }
38    }
39    /// Returns the total duration of the trajectory.
40    pub fn get_total_duration(&self) -> Duration {
41        self.total_duration
42    }
43}
44/// Generates a simple Linear Velocity Profile. It linearly maps the duration to a progress.
45pub fn generate_linear_velocity_profile(total_duration: Duration) -> VelocityProfileMapping {
46    let velocity_profile = move |time: Duration| -> VelocityProfileOutput {
47        VelocityProfileOutput {
48            progress: time.as_secs_f64() / total_duration.as_secs_f64(),
49            finished: time >= total_duration,
50        }
51    };
52    VelocityProfileMapping::new(Box::new(velocity_profile), total_duration)
53}
54/// Generates a smooth cosine velocity profile which is indefinitely often continuously differentiable.
55pub fn generate_cosine_velocity_profile(total_duration: Duration) -> VelocityProfileMapping {
56    let velocity_profile = move |time: Duration| -> VelocityProfileOutput {
57        let progress =
58            (1. - f64::cos(PI * time.as_secs_f64() / total_duration.as_secs_f64())) / 2.0;
59        VelocityProfileOutput {
60            progress,
61            finished: time >= total_duration,
62        }
63    };
64    VelocityProfileMapping::new(Box::new(velocity_profile), total_duration)
65}
66
67/// creates an S-Curve velocity profile which is subject to jerk, acceleration and velocity constraints
68/// # Arguments
69/// * `total_length` - a rough estimation of the total length of the PoseGenerator.
70/// Use [`get_approximate_length`](`crate::PoseGenerator::get_approximate_length`)
71/// * `constraints` - jerk, acceleration and velocity constraints for the trajectory
72pub fn generate_s_curve_profile(
73    total_length: f64,
74    constraints: SCurveConstraints,
75) -> VelocityProfileMapping {
76    let start_conditions = SCurveStartConditions {
77        q0: 0.0,
78        q1: total_length,
79        v0: 0.0,
80        v1: 0.0,
81    };
82    let input = SCurveInput {
83        constraints,
84        start_conditions,
85    };
86    let (params, s_curve) = s_curve_generator(&input, Derivative::Position);
87    let total_duration = Duration::from_secs_f64(params.time_intervals.total_duration());
88    let velocity_profile = move |time: Duration| -> VelocityProfileOutput {
89        let progress = s_curve(time.as_secs_f64()) / total_length;
90        VelocityProfileOutput {
91            progress,
92            finished: progress >= 1.,
93        }
94    };
95    VelocityProfileMapping::new(Box::new(velocity_profile), total_duration)
96}
97
98#[cfg(test)]
99mod tests {
100    use crate::velocity_profile::{
101        generate_cosine_velocity_profile, generate_linear_velocity_profile,
102        generate_s_curve_profile,
103    };
104    use s_curve::SCurveConstraints;
105    use std::f64::consts::PI;
106    use std::time::Duration;
107
108    #[test]
109    fn test_linear_profile() {
110        let mut profile = generate_linear_velocity_profile(Duration::from_secs_f64(10.));
111        for i in 0..=100 {
112            let time = Duration::from_secs_f64(i as f64 / 10.);
113            let output = profile.get(time);
114            println!("{:?}", output);
115            assert!(f64::abs(output.progress - i as f64 / 100.) < 1e-7);
116            if i != 100 {
117                assert!(!output.finished);
118            } else {
119                assert!(output.finished);
120            }
121        }
122    }
123    #[test]
124    fn test_cosine_profile() {
125        let mut profile = generate_cosine_velocity_profile(Duration::from_secs_f64(10.));
126        for i in 0..=100 {
127            let time = Duration::from_secs_f64(i as f64 / 10.);
128            let output = profile.get(time);
129            println!("{:?}", output);
130            assert!(f64::abs(output.progress - (1. - f64::cos(PI * i as f64 / 100.)) / 2.) < 1e-7);
131            if i != 100 {
132                assert!(!output.finished);
133            } else {
134                assert!(output.finished);
135            }
136        }
137    }
138    #[test]
139    fn test_s_curve_profile() {
140        let constraints = SCurveConstraints {
141            max_jerk: 5.,
142            max_acceleration: 5.,
143            max_velocity: 5.,
144        };
145        let mut profile = generate_s_curve_profile(7.4, constraints);
146
147        for &i in [0., 0.5, 1.0000000000000001].iter() {
148            let time = profile.total_duration.mul_f64(i);
149            let output = profile.get(time);
150            println!("{:?}", output);
151            assert!(f64::abs(output.progress - i) < 1e-7);
152            if i < 1. {
153                assert!(!output.finished);
154            } else {
155                assert!(output.finished);
156            }
157        }
158    }
159}
cartesian_trajectories/velocity_profile.rs

cartesian_trajectories/
velocity_profile.rs
