1use std::sync::Arc;
2
3use anyhow::format_err;
4use arci::{
5 Error, JointPositionDifferenceLimiter, JointPositionLimit, JointPositionLimiter,
6 JointPositionLimiterStrategy, JointTrajectoryClient, JointVelocityLimiter,
7};
8use schemars::JsonSchema;
9use serde::{Deserialize, Serialize};
10
11fn new_joint_position_difference_limiter<C>(
12 client: C,
13 position_difference_limits: Option<Vec<f64>>,
14) -> Result<JointPositionDifferenceLimiter<C>, Error>
15where
16 C: JointTrajectoryClient,
17{
18 match position_difference_limits {
19 Some(position_difference_limits) => Ok(JointPositionDifferenceLimiter::new(
20 client,
21 position_difference_limits,
22 )?),
23 None => Err(Error::Other(anyhow::format_err!(
24 "No position_difference_limits is specified"
25 ))),
26 }
27}
28
29fn new_joint_position_limiter<C>(
30 client: C,
31 position_limits: Option<Vec<JointPositionLimit>>,
32 strategy: JointPositionLimiterStrategy,
33 urdf_robot: Option<&urdf_rs::Robot>,
34) -> Result<JointPositionLimiter<C>, Error>
35where
36 C: JointTrajectoryClient,
37{
38 match position_limits {
39 Some(position_limits) => Ok(JointPositionLimiter::new_with_strategy(
40 client,
41 position_limits,
42 strategy,
43 )),
44 None => JointPositionLimiter::from_urdf(client, &urdf_robot.unwrap().joints),
45 }
46}
47
48fn new_joint_velocity_limiter<C>(
49 client: C,
50 velocity_limits: Option<Vec<f64>>,
51 urdf_robot: Option<&urdf_rs::Robot>,
52) -> Result<JointVelocityLimiter<C>, Error>
53where
54 C: JointTrajectoryClient,
55{
56 match velocity_limits {
57 Some(velocity_limits) => Ok(JointVelocityLimiter::new(client, velocity_limits)),
58 None => JointVelocityLimiter::from_urdf(client, &urdf_robot.unwrap().joints),
59 }
60}
61
62#[derive(Debug, Serialize, Deserialize, Clone, JsonSchema)]
63#[serde(deny_unknown_fields)]
64pub struct JointTrajectoryClientWrapperConfig {
65 #[serde(default)]
66 pub wrap_with_joint_position_limiter: bool,
67 #[serde(default)]
68 pub joint_position_limiter_strategy: JointPositionLimiterStrategy,
69
70 #[serde(default)]
71 pub wrap_with_joint_velocity_limiter: bool,
72 pub joint_velocity_limits: Option<Vec<f64>>,
73
74 #[serde(default)]
75 pub wrap_with_joint_position_difference_limiter: bool,
76 pub joint_position_difference_limits: Option<Vec<f64>>,
77
78 pub joint_position_limits: Option<Vec<JointPositionLimit>>,
82}
83
84impl JointTrajectoryClientWrapperConfig {
85 pub(crate) fn check_urdf_is_not_necessary(&self) -> Result<(), arci::Error> {
86 if self.wrap_with_joint_position_limiter && self.joint_position_limits.is_none() {
87 return Err(format_err!(
88 "`wrap_with_joint_position_limiter=true` requires urdf or joint_position_limits \
89 is specified",
90 )
91 .into());
92 }
93 if self.wrap_with_joint_velocity_limiter && self.joint_velocity_limits.is_none() {
94 return Err(format_err!(
95 "`wrap_with_joint_velocity_limiter=true` requires urdf or joint_velocity_limits \
96 is specified",
97 )
98 .into());
99 }
100 Ok(())
101 }
102}
103
104pub fn wrap_joint_trajectory_client(
105 config: JointTrajectoryClientWrapperConfig,
106 client: Arc<dyn JointTrajectoryClient>,
107 urdf_robot: Option<&urdf_rs::Robot>,
108) -> Result<Arc<dyn JointTrajectoryClient>, arci::Error> {
109 Ok(if config.wrap_with_joint_velocity_limiter {
110 if config.wrap_with_joint_position_limiter {
111 if config.wrap_with_joint_position_difference_limiter {
112 let client = new_joint_position_difference_limiter(
113 client,
114 config.joint_position_difference_limits,
115 )?;
116 let client =
117 new_joint_velocity_limiter(client, config.joint_velocity_limits, urdf_robot)?;
118 let client = new_joint_position_limiter(
119 client,
120 config.joint_position_limits,
121 config.joint_position_limiter_strategy,
122 urdf_robot,
123 )?;
124 Arc::new(client)
125 } else {
126 let client =
127 new_joint_velocity_limiter(client, config.joint_velocity_limits, urdf_robot)?;
128 let client = new_joint_position_limiter(
129 client,
130 config.joint_position_limits,
131 config.joint_position_limiter_strategy,
132 urdf_robot,
133 )?;
134 Arc::new(client)
135 }
136 } else if config.wrap_with_joint_position_difference_limiter {
137 Arc::new(new_joint_velocity_limiter(
138 new_joint_position_difference_limiter(
139 client,
140 config.joint_position_difference_limits,
141 )?,
142 config.joint_velocity_limits,
143 urdf_robot,
144 )?)
145 } else {
146 Arc::new(new_joint_velocity_limiter(
147 client,
148 config.joint_velocity_limits,
149 urdf_robot,
150 )?)
151 }
152 } else if config.wrap_with_joint_position_limiter {
153 if config.wrap_with_joint_position_difference_limiter {
154 Arc::new(new_joint_position_limiter(
155 new_joint_position_difference_limiter(
156 client,
157 config.joint_position_difference_limits,
158 )?,
159 config.joint_position_limits,
160 config.joint_position_limiter_strategy,
161 urdf_robot,
162 )?)
163 } else {
164 Arc::new(new_joint_position_limiter(
165 client,
166 config.joint_position_limits,
167 config.joint_position_limiter_strategy,
168 urdf_robot,
169 )?)
170 }
171 } else if config.wrap_with_joint_position_difference_limiter {
172 Arc::new(new_joint_position_difference_limiter(
173 client,
174 config.joint_position_difference_limits,
175 )?)
176 } else {
177 Arc::new(client)
178 })
179}
180
181#[cfg(test)]
182mod tests {
183 use super::*;
184
185 #[test]
186 fn test_new_joint_position_limiter() {
187 use arci::{DummyJointTrajectoryClient, JointPositionLimit, JointPositionLimiter};
188
189 let client = DummyJointTrajectoryClient::new(vec!["a".to_owned(), "b".to_owned()]);
190 let limits = vec![
191 JointPositionLimit::new(-5.0, 5.0),
192 JointPositionLimit::new(-5.0, 5.0),
193 ];
194
195 let result = new_joint_position_limiter(
196 client,
197 Some(limits.clone()),
198 JointPositionLimiterStrategy::Clamp,
199 None,
200 );
201 assert!(result.is_ok());
202 let _result = result.unwrap();
203
204 let client = DummyJointTrajectoryClient::new(vec!["a".to_owned(), "b".to_owned()]);
205 let _correct = JointPositionLimiter::new(client, limits);
206 }
207
208 #[test]
209 #[should_panic]
210 fn test_new_joint_position_limiter_error() {
211 use arci::DummyJointTrajectoryClient;
212
213 let client = DummyJointTrajectoryClient::new(vec!["a".to_owned(), "b".to_owned()]);
214
215 let _ = new_joint_position_limiter(client, None, JointPositionLimiterStrategy::Clamp, None);
216 }
217}