robot_description_builder/joint/joint_data/
mimic_data.rs

1use std::sync::Weak;
2
3#[cfg(feature = "xml")]
4use quick_xml::{events::attributes::Attribute, name::QName};
5
6use crate::{
7	cluster_objects::kinematic_data_tree::KinematicDataTree, identifiers::GroupID, joint::Joint,
8	utils::WeakLock,
9};
10
11#[derive(Debug, Clone)]
12pub struct MimicData {
13	pub joint: WeakLock<Joint>,
14	pub multiplier: Option<f32>,
15	pub offset: Option<f32>,
16}
17
18#[cfg(feature = "urdf")]
19impl crate::to_rdf::to_urdf::ToURDF for MimicData {
20	fn to_urdf(
21		&self,
22		writer: &mut quick_xml::Writer<std::io::Cursor<Vec<u8>>>,
23		_urdf_config: &crate::to_rdf::to_urdf::URDFConfig,
24	) -> Result<(), quick_xml::Error> {
25		let mut element = writer.create_element("mimic").with_attribute(Attribute {
26			key: QName(b"joint"),
27			value: self
28				.joint
29				.upgrade()
30				.unwrap() // FIXME: Is unwrap Ok here?
31				.read()
32				.unwrap() // FIXME: Is unwrap Ok here?
33				.name()
34				.display()
35				.as_bytes()
36				.into(),
37		});
38
39		if let Some(multiplier) = self.multiplier {
40			element = element.with_attribute(Attribute {
41				key: QName(b"multiplier"),
42				value: multiplier.to_string().as_bytes().into(),
43			})
44		}
45
46		if let Some(offset) = self.offset {
47			element = element.with_attribute(Attribute {
48				key: QName(b"offset"),
49				value: offset.to_string().as_bytes().into(),
50			})
51		}
52
53		element.write_empty()?;
54		Ok(())
55	}
56}
57
58impl PartialEq for MimicData {
59	fn eq(&self, other: &Self) -> bool {
60		Weak::ptr_eq(&self.joint, &other.joint)
61			&& self.multiplier == other.multiplier
62			&& self.offset == other.offset
63	}
64}
65
66impl From<MimicData> for MimicBuilderData {
67	fn from(value: MimicData) -> Self {
68		Self {
69			joint_name: value
70				.joint
71				.upgrade()
72				.unwrap() // FIXME: Is unwrap Ok?
73				.try_read()
74				.unwrap() // FIXME: Is unwrap Ok?
75				.name()
76				.clone(),
77			multiplier: value.multiplier,
78			offset: value.offset,
79		}
80	}
81}
82
83#[derive(Debug, PartialEq, Clone)]
84pub struct MimicBuilderData {
85	pub joint_name: String,
86	pub multiplier: Option<f32>,
87	pub offset: Option<f32>,
88}
89
90impl MimicBuilderData {
91	pub(crate) fn to_mimic_data(&self, tree: &Weak<KinematicDataTree>) -> MimicData {
92		MimicData {
93			joint: Weak::clone(
94				tree.upgrade()
95					.unwrap() // This unwrap is Ok
96					.joints
97					.try_read()
98					.unwrap() // FIXME: Is this unwrap OK?
99					.get(&self.joint_name)
100					.unwrap(), // FIXME: Is this unwrap OK?
101			),
102			multiplier: self.multiplier,
103			offset: self.offset,
104		}
105	}
106}
107
108#[cfg(test)]
109mod tests {
110	// use crate::joint::joint_data::MimicData;
111	use test_log::test;
112
113	#[cfg(feature = "urdf")]
114	mod to_urdf {
115		use std::io::Seek;
116
117		use super::test;
118
119		use crate::{
120			cluster_objects::KinematicInterface,
121			joint::{jointbuilder::BuildJoint, smartjointbuilder::SmartJointBuilder},
122			link::Link,
123			to_rdf::to_urdf::{ToURDF, URDFConfig},
124		};
125
126		fn test_to_urdf_mimic(
127			joint_builder: impl BuildJoint,
128			result: String,
129			urdf_config: &URDFConfig,
130		) {
131			let tree = Link::builder("root").build_tree();
132			tree.get_root_link()
133				.try_write()
134				.unwrap()
135				.try_attach_child(
136					SmartJointBuilder::new_fixed("joint-s"),
137					Link::builder("child_link").build_tree(),
138				)
139				.unwrap();
140
141			tree.get_root_link()
142				.try_write()
143				.unwrap()
144				.try_attach_child(joint_builder, Link::builder("child_link_2"))
145				.unwrap();
146
147			let mut writer = quick_xml::Writer::new(std::io::Cursor::new(Vec::new()));
148
149			assert!(tree
150				.get_joint("joint-t")
151				.unwrap()
152				.try_read()
153				.unwrap()
154				.mimic
155				.as_ref()
156				.unwrap()
157				.to_urdf(&mut writer, urdf_config)
158				.is_ok());
159
160			writer.get_mut().rewind().unwrap();
161
162			assert_eq!(
163				std::io::read_to_string(writer.into_inner()).unwrap(),
164				result
165			);
166		}
167
168		#[test]
169		fn only_joint() {
170			test_to_urdf_mimic(
171				SmartJointBuilder::new_continuous("joint-t").with_mimic("joint-s"),
172				String::from(r#"<mimic joint="joint-s"/>"#),
173				&URDFConfig::default(),
174			);
175		}
176
177		#[test]
178		fn multiplier() {
179			test_to_urdf_mimic(
180				SmartJointBuilder::new_continuous("joint-t")
181					.with_mimic("joint-s")
182					.set_mimic_multiplier(20.),
183				String::from(r#"<mimic joint="joint-s" multiplier="20"/>"#),
184				&URDFConfig::default(),
185			);
186
187			test_to_urdf_mimic(
188				SmartJointBuilder::new_continuous("joint-t")
189					.with_mimic("joint-s")
190					.set_mimic_multiplier(0.00001),
191				String::from(r#"<mimic joint="joint-s" multiplier="0.00001"/>"#),
192				&URDFConfig::default(),
193			);
194
195			test_to_urdf_mimic(
196				SmartJointBuilder::new_continuous("joint-t")
197					.with_mimic("joint-s")
198					.set_mimic_multiplier(90000.3),
199				String::from(r#"<mimic joint="joint-s" multiplier="90000.3"/>"#),
200				&URDFConfig::default(),
201			);
202		}
203
204		#[test]
205		fn offset() {
206			test_to_urdf_mimic(
207				SmartJointBuilder::new_continuous("joint-t")
208					.with_mimic("joint-s")
209					.set_mimic_offset(20.),
210				String::from(r#"<mimic joint="joint-s" offset="20"/>"#),
211				&URDFConfig::default(),
212			);
213
214			test_to_urdf_mimic(
215				SmartJointBuilder::new_continuous("joint-t")
216					.with_mimic("joint-s")
217					.set_mimic_offset(0.00001),
218				String::from(r#"<mimic joint="joint-s" offset="0.00001"/>"#),
219				&URDFConfig::default(),
220			);
221
222			test_to_urdf_mimic(
223				SmartJointBuilder::new_continuous("joint-t")
224					.with_mimic("joint-s")
225					.set_mimic_offset(9000000.),
226				String::from(r#"<mimic joint="joint-s" offset="9000000"/>"#),
227				&URDFConfig::default(),
228			);
229		}
230
231		#[test]
232		fn multiplier_offset() {
233			test_to_urdf_mimic(
234				SmartJointBuilder::new_continuous("joint-t")
235					.with_mimic("joint-s")
236					.set_mimic_offset(20.)
237					.set_mimic_multiplier(-18.),
238				String::from(r#"<mimic joint="joint-s" multiplier="-18" offset="20"/>"#),
239				&URDFConfig::default(),
240			);
241
242			test_to_urdf_mimic(
243				SmartJointBuilder::new("joint-t")
244					.continuous()
245					.with_mimic("joint-s")
246					.set_mimic_multiplier(100000.)
247					.set_mimic_offset(0.00001),
248				String::from(r#"<mimic joint="joint-s" multiplier="100000" offset="0.00001"/>"#),
249				&URDFConfig::default(),
250			);
251
252			test_to_urdf_mimic(
253				SmartJointBuilder::new("joint-t")
254					.continuous()
255					.with_mimic("joint-s")
256					.set_mimic_multiplier(0.00000123)
257					.set_mimic_offset(9000000.),
258				String::from(
259					r#"<mimic joint="joint-s" multiplier="0.00000123" offset="9000000"/>"#,
260				),
261				&URDFConfig::default(),
262			);
263		}
264	}
265}
robot_description_builder/joint/joint_data/mimic_data.rs

robot_description_builder/joint/joint_data/
mimic_data.rs
