robot_description_builder/joint/joint_data/
safety_controller_data.rs

1#[cfg(feature = "xml")]
2use quick_xml::{events::attributes::Attribute, name::QName};
3
4#[derive(Debug, PartialEq, Clone, Copy, Default)]
5pub struct SafetyControllerData {
6	//(optional, defaults to 0)
7	//
8	// An attribute specifying the lower joint boundary where the safety controller starts limiting the position of the joint. This limit needs to be larger than the lower joint limit (see above). See See safety limits for more details.
9	// TODO: FIX DOCUMENTATION
10	pub soft_lower_limit: Option<f32>,
11	// (optional, defaults to 0)
12	//
13	// An attribute specifying the upper joint boundary where the safety controller starts limiting the position of the joint. This limit needs to be smaller than the upper joint limit (see above). See See safety limits for more details.
14	// TODO: FIX DOCUMENTATION
15	pub soft_upper_limit: Option<f32>,
16	//  (optional, defaults to 0)
17	//
18	// An attribute specifying the relation between position and velocity limits. See See safety limits for more details.
19	// TODO: FIX DOCUMENTATION
20	pub k_position: Option<f32>,
21	// An attribute specifying the relation between effort and velocity limits. See See safety limits for more details.
22	pub k_velocity: f32,
23}
24
25#[cfg(feature = "urdf")]
26impl crate::to_rdf::to_urdf::ToURDF for SafetyControllerData {
27	fn to_urdf(
28		&self,
29		writer: &mut quick_xml::Writer<std::io::Cursor<Vec<u8>>>,
30		_urdf_config: &crate::to_rdf::to_urdf::URDFConfig,
31	) -> Result<(), quick_xml::Error> {
32		let mut element = writer
33			.create_element("safety_controller")
34			.with_attribute(Attribute {
35				key: QName(b"k_velocity"),
36				value: self.k_velocity.to_string().as_bytes().into(),
37			});
38
39		if let Some(k_position) = self.k_position {
40			element = element.with_attribute(Attribute {
41				key: QName(b"k_position"),
42				value: k_position.to_string().as_bytes().into(),
43			});
44		}
45
46		if let Some(soft_lower_limit) = self.soft_lower_limit {
47			element = element.with_attribute(Attribute {
48				key: QName(b"soft_lower_limit"),
49				value: soft_lower_limit.to_string().as_bytes().into(),
50			})
51		}
52
53		if let Some(soft_upper_limit) = self.soft_upper_limit {
54			element = element.with_attribute(Attribute {
55				key: QName(b"soft_upper_limit"),
56				value: soft_upper_limit.to_string().as_bytes().into(),
57			})
58		}
59
60		element.write_empty()?;
61
62		Ok(())
63	}
64}
65
66#[cfg(test)]
67mod tests {
68	use crate::joint::joint_data::SafetyControllerData;
69	use test_log::test;
70
71	#[cfg(feature = "urdf")]
72	mod to_urdf {
73
74		use std::io::Seek;
75
76		use super::{test, SafetyControllerData};
77		use crate::to_rdf::to_urdf::{ToURDF, URDFConfig};
78
79		fn test_to_urdf_safety_contoller(
80			safety_controller: SafetyControllerData,
81			result: String,
82			urdf_config: &URDFConfig,
83		) {
84			let mut writer = quick_xml::Writer::new(std::io::Cursor::new(Vec::new()));
85
86			assert!(safety_controller.to_urdf(&mut writer, urdf_config).is_ok());
87
88			writer.get_mut().rewind().unwrap();
89
90			assert_eq!(
91				std::io::read_to_string(writer.into_inner()).unwrap(),
92				result
93			);
94		}
95
96		#[test]
97		fn only_k_velocity() {
98			test_to_urdf_safety_contoller(
99				SafetyControllerData {
100					k_velocity: 10.,
101					..Default::default()
102				},
103				String::from(r#"<safety_controller k_velocity="10"/>"#),
104				&URDFConfig::default(),
105			);
106
107			test_to_urdf_safety_contoller(
108				SafetyControllerData {
109					k_velocity: 1000000.,
110					..Default::default()
111				},
112				String::from(r#"<safety_controller k_velocity="1000000"/>"#),
113				&URDFConfig::default(),
114			);
115
116			test_to_urdf_safety_contoller(
117				SafetyControllerData {
118					k_velocity: 0.00123,
119					..Default::default()
120				},
121				String::from(r#"<safety_controller k_velocity="0.00123"/>"#),
122				&URDFConfig::default(),
123			);
124		}
125
126		#[test]
127		fn k_position() {
128			test_to_urdf_safety_contoller(
129				SafetyControllerData {
130					k_velocity: 10.,
131					k_position: Some(100.),
132					..Default::default()
133				},
134				String::from(r#"<safety_controller k_velocity="10" k_position="100"/>"#),
135				&URDFConfig::default(),
136			);
137
138			test_to_urdf_safety_contoller(
139				SafetyControllerData {
140					k_velocity: 1000000.,
141					k_position: Some(-0.0000987),
142					..Default::default()
143				},
144				String::from(
145					r#"<safety_controller k_velocity="1000000" k_position="-0.0000987"/>"#,
146				),
147				&URDFConfig::default(),
148			);
149
150			test_to_urdf_safety_contoller(
151				SafetyControllerData {
152					k_velocity: 0.00123,
153					k_position: Some(988000000.),
154					..Default::default()
155				},
156				String::from(r#"<safety_controller k_velocity="0.00123" k_position="988000000"/>"#),
157				&URDFConfig::default(),
158			);
159		}
160
161		#[test]
162		fn soft_lower_limit() {
163			test_to_urdf_safety_contoller(
164				SafetyControllerData {
165					k_velocity: 10.,
166					soft_lower_limit: Some(-100.),
167					..Default::default()
168				},
169				String::from(r#"<safety_controller k_velocity="10" soft_lower_limit="-100"/>"#),
170				&URDFConfig::default(),
171			);
172
173			test_to_urdf_safety_contoller(
174				SafetyControllerData {
175					k_velocity: 1000000.,
176					soft_lower_limit: Some(-0.0000987),
177					..Default::default()
178				},
179				String::from(
180					r#"<safety_controller k_velocity="1000000" soft_lower_limit="-0.0000987"/>"#,
181				),
182				&URDFConfig::default(),
183			);
184
185			test_to_urdf_safety_contoller(
186				SafetyControllerData {
187					k_velocity: 0.00123,
188					soft_lower_limit: Some(988000000.),
189					..Default::default()
190				},
191				String::from(
192					r#"<safety_controller k_velocity="0.00123" soft_lower_limit="988000000"/>"#,
193				),
194				&URDFConfig::default(),
195			);
196		}
197
198		#[test]
199		fn soft_upper_limit() {
200			test_to_urdf_safety_contoller(
201				SafetyControllerData {
202					k_velocity: 10.,
203					soft_upper_limit: Some(100.),
204					..Default::default()
205				},
206				String::from(r#"<safety_controller k_velocity="10" soft_upper_limit="100"/>"#),
207				&URDFConfig::default(),
208			);
209
210			test_to_urdf_safety_contoller(
211				SafetyControllerData {
212					k_velocity: 1000000.,
213					soft_upper_limit: Some(0.0000987),
214					..Default::default()
215				},
216				String::from(
217					r#"<safety_controller k_velocity="1000000" soft_upper_limit="0.0000987"/>"#,
218				),
219				&URDFConfig::default(),
220			);
221
222			test_to_urdf_safety_contoller(
223				SafetyControllerData {
224					k_velocity: 0.00123,
225					soft_upper_limit: Some(-988000000.),
226					..Default::default()
227				},
228				String::from(
229					r#"<safety_controller k_velocity="0.00123" soft_upper_limit="-988000000"/>"#,
230				),
231				&URDFConfig::default(),
232			);
233		}
234
235		#[test]
236		fn full() {
237			test_to_urdf_safety_contoller(
238				SafetyControllerData {
239					k_velocity: 10.,
240					k_position: Some(23.),
241					soft_lower_limit: Some(-100.),
242					soft_upper_limit: Some(100.),
243				},
244				String::from(
245					r#"<safety_controller k_velocity="10" k_position="23" soft_lower_limit="-100" soft_upper_limit="100"/>"#,
246				),
247				&URDFConfig::default(),
248			);
249
250			test_to_urdf_safety_contoller(
251				SafetyControllerData {
252					k_velocity: 1000000.,
253					k_position: Some(-999.),
254					soft_lower_limit: Some(-0.0000987),
255					soft_upper_limit: Some(0.0000987),
256				},
257				String::from(
258					r#"<safety_controller k_velocity="1000000" k_position="-999" soft_lower_limit="-0.0000987" soft_upper_limit="0.0000987"/>"#,
259				),
260				&URDFConfig::default(),
261			);
262
263			test_to_urdf_safety_contoller(
264				SafetyControllerData {
265					k_velocity: 0.00123,
266					k_position: Some(10000000000000.),
267					soft_upper_limit: Some(-988000000.),
268					soft_lower_limit: Some(988000000.), // TODO: Maybe it should be checked if they are reversed?
269				},
270				String::from(
271					r#"<safety_controller k_velocity="0.00123" k_position="10000000000000" soft_lower_limit="988000000" soft_upper_limit="-988000000"/>"#,
272				),
273				&URDFConfig::default(),
274			);
275		}
276	}
277}
robot_description_builder/joint/joint_data/safety_controller_data.rs

robot_description_builder/joint/joint_data/
safety_controller_data.rs
