arci/clients/
joint_velocity_limiter.rs

1use tracing::debug;
2
3use crate::{
4    error::Error,
5    traits::{JointTrajectoryClient, TrajectoryPoint},
6    waits::WaitFuture,
7};
8
9/// JointVelocityLimiter limits the duration to make all joints velocities lower than the given
10/// velocities limits at each TrajectoryPoint.
11///
12/// It does not change TrajectoryPoint velocities.
13/// The duration for a TrajectoryPoint\[i\] is set to
14/// ```Text
15/// duration[i] = max(limited_duration_i[j=0], ...,  limited_duration_i[j=J-1], input_duration[i])
16/// where
17///  j : joint_index (0 <= j < J),
18///  limited_duration_i[j] =
19///   abs(TrajectoryPoint[i].positions[j]  - TrajectoryPoint[i-1].positions[j]) / velocity_limits[j]
20/// ```
21#[derive(Debug)]
22pub struct JointVelocityLimiter<C>
23where
24    C: JointTrajectoryClient,
25{
26    client: C,
27    velocity_limits: Vec<f64>,
28}
29
30impl<C> JointVelocityLimiter<C>
31where
32    C: JointTrajectoryClient,
33{
34    /// Creates a new `JointVelocityLimiter` with the given velocity limits.
35    ///
36    /// # Panics
37    ///
38    /// Panics if the lengths of `velocity_limits` and joints that `client` handles are different.
39    #[track_caller]
40    pub fn new(client: C, velocity_limits: Vec<f64>) -> Self {
41        assert!(client.joint_names().len() == velocity_limits.len());
42        Self {
43            client,
44            velocity_limits,
45        }
46    }
47
48    /// Creates a new `JointVelocityLimiter` with the velocity limits defined in URDF.
49    pub fn from_urdf(client: C, joints: &[urdf_rs::Joint]) -> Result<Self, Error> {
50        let mut velocity_limits = Vec::new();
51        for joint_name in client.joint_names() {
52            if let Some(i) = joints.iter().position(|j| j.name == *joint_name) {
53                let limit = joints[i].limit.velocity;
54                velocity_limits.push(limit);
55            } else {
56                return Err(Error::NoJoint(joint_name));
57            }
58        }
59
60        Ok(Self {
61            client,
62            velocity_limits,
63        })
64    }
65}
66
67impl<C> JointTrajectoryClient for JointVelocityLimiter<C>
68where
69    C: JointTrajectoryClient,
70{
71    fn joint_names(&self) -> Vec<String> {
72        self.client.joint_names()
73    }
74
75    fn current_joint_positions(&self) -> Result<Vec<f64>, Error> {
76        self.client.current_joint_positions()
77    }
78
79    fn send_joint_positions(
80        &self,
81        positions: Vec<f64>,
82        duration: std::time::Duration,
83    ) -> Result<WaitFuture, Error> {
84        self.send_joint_trajectory(vec![TrajectoryPoint {
85            positions,
86            velocities: None,
87            time_from_start: duration,
88        }])
89    }
90
91    fn send_joint_trajectory(&self, trajectory: Vec<TrajectoryPoint>) -> Result<WaitFuture, Error> {
92        let mut prev_positions = self.current_joint_positions()?;
93
94        let mut limited_trajectory = vec![];
95        let mut limited_duration_from_start = std::time::Duration::from_secs(0);
96        let mut original_duration_from_start = std::time::Duration::from_secs(0);
97        for (sequence_index, original_trajectory_point) in trajectory.iter().enumerate() {
98            let mut limited_duration_from_prev = std::time::Duration::from_secs(0);
99            let mut dominant_joint_index = 0;
100            for (joint_index, prev_position) in prev_positions.iter().enumerate() {
101                let single_duration = std::time::Duration::from_secs_f64(
102                    (prev_position - original_trajectory_point.positions[joint_index]).abs()
103                        / self.velocity_limits[joint_index],
104                );
105                limited_duration_from_prev = if single_duration > limited_duration_from_prev {
106                    dominant_joint_index = joint_index;
107                    single_duration
108                } else {
109                    limited_duration_from_prev
110                }
111            }
112            let original_duration_from_prev =
113                original_trajectory_point.time_from_start - original_duration_from_start;
114            original_duration_from_start = original_trajectory_point.time_from_start;
115
116            let use_limited = limited_duration_from_prev > original_duration_from_prev;
117            let selected_duration = if use_limited {
118                limited_duration_from_prev
119            } else {
120                original_duration_from_prev
121            };
122            limited_duration_from_start += selected_duration;
123            limited_trajectory.push(TrajectoryPoint {
124                positions: original_trajectory_point.positions.clone(),
125                velocities: original_trajectory_point.velocities.clone(),
126                time_from_start: limited_duration_from_start,
127            });
128            prev_positions.clone_from(&original_trajectory_point.positions);
129            debug!(
130                "Sequence{sequence_index} dominant joint_index {dominant_joint_index} duration limited : {limited_duration_from_prev:?}{} original : {original_duration_from_prev:?}{}",
131                if use_limited { "(O)" } else { "" },
132                if use_limited { "" } else { "(O)" }
133            );
134        }
135
136        debug!("OriginalTrajectory {trajectory:?}");
137        debug!("LimitedTrajectory {limited_trajectory:?}");
138
139        self.client.send_joint_trajectory(limited_trajectory)
140    }
141}
142
143#[cfg(test)]
144mod tests {
145    use std::sync::Arc;
146
147    use assert_approx_eq::assert_approx_eq;
148
149    use super::*;
150    use crate::DummyJointTrajectoryClient;
151    #[test]
152    #[should_panic]
153    fn mismatch_size() {
154        let client = DummyJointTrajectoryClient::new(vec!["a".to_owned()]);
155        JointVelocityLimiter::new(client, vec![1.0, 2.0]);
156    }
157    #[test]
158    fn joint_names() {
159        let client = DummyJointTrajectoryClient::new(vec!["a".to_owned(), "b".to_owned()]);
160        let limiter = JointVelocityLimiter::new(client, vec![1.0, 2.0]);
161        let joint_names = limiter.joint_names();
162        assert_eq!(joint_names.len(), 2);
163        assert_eq!(joint_names[0], "a");
164        assert_eq!(joint_names[1], "b");
165    }
166    fn test_send_joint_positions(limits: Vec<f64>, expected_duration_secs: f64) {
167        let client = Arc::new(DummyJointTrajectoryClient::new(vec![
168            "a".to_owned(),
169            "b".to_owned(),
170        ]));
171        let limiter = JointVelocityLimiter::new(client.clone(), limits);
172        assert!(tokio_test::block_on(
173            limiter
174                .send_joint_positions(vec![1.0, 2.0], std::time::Duration::from_secs_f64(4.0))
175                .unwrap()
176        )
177        .is_ok());
178        let joint_positions = limiter.current_joint_positions().unwrap();
179        assert_eq!(joint_positions.len(), 2);
180        assert_approx_eq!(joint_positions[0], 1.0);
181        assert_approx_eq!(joint_positions[1], 2.0);
182        let trajectory = client.last_trajectory.lock().unwrap();
183        assert_eq!(trajectory.len(), 1);
184        assert_eq!(trajectory[0].positions.len(), 2);
185        assert_approx_eq!(trajectory[0].positions[0], 1.0);
186        assert_approx_eq!(trajectory[0].positions[1], 2.0);
187        assert!(trajectory[0].velocities.is_none());
188        assert_approx_eq!(
189            trajectory[0].time_from_start.as_secs_f64(),
190            expected_duration_secs
191        );
192    }
193
194    #[test]
195    fn send_joint_positions_none_limited() {
196        test_send_joint_positions(vec![1.0, 2.0], 4.0);
197    }
198
199    #[test]
200    fn send_joint_positions_limited() {
201        // joint0 is over limit
202        test_send_joint_positions(vec![0.1, 2.0], 10.0);
203        // joint1 is over limit
204        test_send_joint_positions(vec![1.0, 0.2], 10.0);
205        // joint0/1 are over limit, joint0 is dominant
206        test_send_joint_positions(vec![0.1, 0.6], 10.0);
207        // joint0/1 are over limit, joint1 is dominant
208        test_send_joint_positions(vec![0.3, 0.2], 10.0);
209    }
210
211    fn test_send_joint_trajectory(limits: Vec<f64>, expected_durations_secs: [f64; 2]) {
212        let client = Arc::new(DummyJointTrajectoryClient::new(vec![
213            "a".to_owned(),
214            "b".to_owned(),
215        ]));
216        let limiter = JointVelocityLimiter::new(client.clone(), limits);
217        assert!(tokio_test::block_on(
218            limiter
219                .send_joint_trajectory(vec![
220                    TrajectoryPoint {
221                        positions: vec![1.0, 2.0],
222                        velocities: Some(vec![3.0, 4.0]),
223                        time_from_start: std::time::Duration::from_secs_f64(4.0)
224                    },
225                    TrajectoryPoint {
226                        positions: vec![3.0, 6.0],
227                        velocities: Some(vec![3.0, 4.0]),
228                        time_from_start: std::time::Duration::from_secs_f64(8.0)
229                    }
230                ])
231                .unwrap()
232        )
233        .is_ok());
234        let joint_positions = limiter.current_joint_positions().unwrap();
235        assert_eq!(joint_positions.len(), 2);
236        assert_approx_eq!(joint_positions[0], 3.0);
237        assert_approx_eq!(joint_positions[1], 6.0);
238
239        let trajectory = client.last_trajectory.lock().unwrap();
240        assert_eq!(trajectory.len(), 2);
241        assert_eq!(trajectory[0].positions.len(), 2);
242        assert_approx_eq!(trajectory[0].positions[0], 1.0);
243        assert_approx_eq!(trajectory[0].positions[1], 2.0);
244        assert!(trajectory[0].velocities.is_some());
245        assert_approx_eq!(trajectory[0].velocities.as_ref().unwrap()[0], 3.0);
246        assert_approx_eq!(trajectory[0].velocities.as_ref().unwrap()[1], 4.0);
247
248        assert_eq!(trajectory[1].positions.len(), 2);
249        assert_approx_eq!(trajectory[1].positions[0], 3.0);
250        assert_approx_eq!(trajectory[1].positions[1], 6.0);
251        assert!(trajectory[1].velocities.is_some());
252        assert_approx_eq!(trajectory[1].velocities.as_ref().unwrap()[0], 3.0);
253        assert_approx_eq!(trajectory[1].velocities.as_ref().unwrap()[1], 4.0);
254
255        assert_approx_eq!(
256            trajectory[0].time_from_start.as_secs_f64(),
257            expected_durations_secs[0]
258        );
259
260        assert_approx_eq!(
261            trajectory[1].time_from_start.as_secs_f64(),
262            expected_durations_secs[1]
263        );
264    }
265
266    #[test]
267    fn send_joint_trajectory_none_limited() {
268        test_send_joint_trajectory(vec![1.0, 2.0], [4.0, 8.0]);
269    }
270
271    #[test]
272    fn send_joint_trajectory_limited() {
273        // joint0 is over limit
274        test_send_joint_trajectory(vec![0.1, 2.0], [10.0, 30.0]);
275        // joint1 is over limit
276        test_send_joint_trajectory(vec![1.0, 0.2], [10.0, 30.0]);
277        // joint0/1 are over limit, joint0 is dominant
278        test_send_joint_trajectory(vec![0.1, 0.6], [10.0, 30.0]);
279        // joint0/1 are over limit, joint1 is dominant
280        test_send_joint_trajectory(vec![0.3, 0.2], [10.0, 30.0]);
281        // joint0 / point1 is over limit
282        test_send_joint_trajectory(vec![0.3, 2.0], [4.0, 4.0 + 2.0 / 0.3]);
283        // joint1 / point1 is over limit
284        test_send_joint_trajectory(vec![1.0, 0.8], [4.0, 4.0 + 4.0 / 0.8]);
285    }
286
287    #[test]
288    fn from_urdf() {
289        let s = r#"
290            <robot name="robot">
291                <joint name="a" type="revolute">
292                    <origin xyz="0.0 0.0 0.0" />
293                    <parent link="b" />
294                    <child link="c" />
295                    <axis xyz="0 1 0" />
296                    <limit lower="-2" upper="1.0" effort="0" velocity="1.0"/>
297                </joint>
298            </robot>
299        "#;
300        let urdf_robot = urdf_rs::read_from_string(s).unwrap();
301        let client = DummyJointTrajectoryClient::new(vec!["a".to_owned()]);
302        let limiter = JointVelocityLimiter::from_urdf(client, &urdf_robot.joints).unwrap();
303        assert_approx_eq!(limiter.velocity_limits[0], 1.0);
304
305        // joint name mismatch
306        let urdf_robot = urdf_rs::read_from_string(s).unwrap();
307        let client = DummyJointTrajectoryClient::new(vec!["unknown".to_owned()]);
308        let e = JointVelocityLimiter::from_urdf(client, &urdf_robot.joints)
309            .err()
310            .unwrap();
311        assert!(matches!(e, Error::NoJoint(..)));
312    }
313}
arci/clients/joint_velocity_limiter.rs

arci/clients/
joint_velocity_limiter.rs
