robot_description_builder/cluster_objects/
kinematic_tree.rs

1use std::{
2	collections::HashMap,
3	sync::{Arc, PoisonError, RwLockWriteGuard},
4};
5
6use crate::{
7	cluster_objects::{
8		kinematic_data_errors::AddTransmissionError, kinematic_data_tree::KinematicDataTree,
9		robot::Robot, KinematicInterface,
10	},
11	joint::Joint,
12	link::Link,
13	material::{data::MaterialData, Material},
14	transmission::{
15		transmission_builder_state::{WithActuator, WithJoints},
16		Transmission, TransmissionBuilder,
17	},
18	utils::{ArcLock, WeakLock},
19};
20
21/// A representation of a basic Kinematic structure.
22///
23/// `KinematicTree`'s can be made from a single [`LinkBuilder`](crate::link::builder::LinkBuilder) by its [`build_tree()`](crate::link::builder::LinkBuilder::build_tree) method.
24///
25/// A `KinematicTree` can be upgraded to a [`Robot`] by calling [`to_robot`](KinematicTree::to_robot).
26/// This will allow the `Robot` to be exported to [all supported Robot Description Formats](crate::to_rdf).
27#[derive(Debug)]
28pub struct KinematicTree(Arc<KinematicDataTree>);
29
30impl KinematicTree {
31	pub(crate) fn new(data: Arc<KinematicDataTree>) -> KinematicTree {
32		KinematicTree(data)
33	}
34
35	/// Converts this unnamed `KinematicTree` to a [`Robot`], which can be represents a more finalized robot structure.
36	pub fn to_robot(self, name: impl Into<String>) -> Robot {
37		Robot::new(name, self.0)
38	}
39}
40
41impl KinematicInterface for KinematicTree {
42	fn get_root_link(&self) -> ArcLock<Link> {
43		Arc::clone(&self.0.root_link)
44	}
45
46	fn get_newest_link(&self) -> ArcLock<Link> {
47		self.0.newest_link.read().unwrap().upgrade().unwrap() // FIXME: Unwrapping might not be ok
48	}
49
50	fn get_links(&self) -> ArcLock<HashMap<String, WeakLock<Link>>> {
51		Arc::clone(&self.0.links)
52	}
53
54	fn get_joints(&self) -> ArcLock<HashMap<String, WeakLock<Joint>>> {
55		Arc::clone(&self.0.joints)
56	}
57
58	fn get_materials(&self) -> ArcLock<HashMap<String, ArcLock<MaterialData>>> {
59		Arc::clone(&self.0.material_index)
60	}
61
62	fn get_transmissions(&self) -> ArcLock<HashMap<String, ArcLock<Transmission>>> {
63		Arc::clone(&self.0.transmissions)
64	}
65
66	fn get_link(&self, name: &str) -> Option<ArcLock<Link>> {
67		self.0
68			.links
69			.read()
70			/* In the future the lock could be saved by overwriting with a newly generated index,
71			however waiting for "This is a nightly-only experimental API. (mutex_unpoison #96469)" */
72			.expect("The RwLock of the Link Index was poisoned. In the future this will be recoverable (mutex_unpoison).")
73			.get(name)
74			.and_then(|weak_link| weak_link.upgrade())
75	}
76
77	fn get_joint(&self, name: &str) -> Option<ArcLock<Joint>> {
78		self.0
79			.joints
80			.read()
81			/* In the future the lock could be saved by overwriting with a newly generated index,
82			however waiting for "This is a nightly-only experimental API. (mutex_unpoison #96469)" */
83			.expect("The RwLock of the Joint Index was poisoned. In the future this will be recoverable (mutex_unpoison).")
84			.get(name)
85			.and_then(|weak_joint| weak_joint.upgrade())
86	}
87
88	fn get_material(&self, name: &str) -> Option<Material> {
89		self.0
90			.material_index
91			.read()
92			.unwrap() // FIXME: Unwrapping might not be ok
93			.get(name)
94			.map(Arc::clone)
95			.map(|data| Material::new_named_inited(name, data))
96	}
97
98	fn get_transmission(&self, name: &str) -> Option<ArcLock<Transmission>> {
99		self.0
100			.transmissions
101			.read()
102			.unwrap() // FIXME: Unwrapping might not be ok
103			.get(name)
104			.map(Arc::clone)
105	}
106
107	fn try_add_transmission(
108		&self,
109		transmission: TransmissionBuilder<WithJoints, WithActuator>,
110	) -> Result<(), AddTransmissionError> {
111		self.0.try_add_transmission(transmission)
112	}
113
114	fn purge_links(&self) {
115		self.0.purge_links()
116	}
117
118	fn purge_joints(&self) {
119		self.0.purge_joints()
120	}
121
122	fn purge_materials(
123		&self,
124	) -> Result<(), PoisonError<RwLockWriteGuard<HashMap<String, ArcLock<MaterialData>>>>> {
125		self.0.purge_materials()
126	}
127
128	fn purge_transmissions(
129		&self,
130	) -> Result<(), PoisonError<RwLockWriteGuard<HashMap<String, ArcLock<Transmission>>>>> {
131		self.0.purge_transmissions()
132	}
133}
134
135impl Clone for KinematicTree {
136	fn clone(&self) -> Self {
137		let root_link = self
138			.get_root_link()
139			.read()
140			.unwrap() // FIXME: UNWRAP MIGHTN NOT BE OK HERE
141			.rebuild_branch_continued()
142			.unwrap(); // FIXME: UNWRAP MIGHTN NOT BE OK HERE
143
144		root_link.build_tree()
145	}
146}
147
148#[cfg(test)]
149mod tests {
150	use log::trace;
151	use std::sync::{Arc, Weak};
152	use test_log::test;
153
154	use crate::{
155		joint::{JointBuilder, JointType},
156		link::{builder::LinkBuilder, link_data::LinkParent, Link},
157		KinematicInterface,
158	};
159
160	#[test]
161	fn clone_single() {
162		let tree = Link::builder("example-link").build_tree();
163		let cloned_tree = tree.clone();
164
165		trace!(
166			target: "RDB-RS::test::KineTree::clone_single",
167			"tree->data        | ptr: {:#?}",
168			Arc::as_ptr(&tree.0)
169		);
170		trace!(
171			target: "RDB-RS::test::KineTree::clone_single",
172			"cloned_tree->data | ptr: {:#?}\n",
173			Arc::as_ptr(&cloned_tree.0)
174		);
175		assert!(!Arc::ptr_eq(&tree.0, &cloned_tree.0));
176
177		trace!(
178			target: "RDB-RS::test::KineTree::clone_single",
179			"tree->..->root_link        | ptr: {:#?}",
180			Arc::as_ptr(&tree.get_root_link())
181		);
182		trace!(
183			target: "RDB-RS::test::KineTree::clone_single",
184			"cloned_tree->..->root_link | ptr: {:#?}\n",
185			Arc::as_ptr(&cloned_tree.get_root_link())
186		);
187		assert!(!Arc::ptr_eq(
188			&tree.get_root_link(),
189			&cloned_tree.get_root_link()
190		));
191
192		// Note: This may not be permanent behavior
193		trace!(
194			target: "RDB-RS::test::KineTree::clone_single",
195			"tree->..->root_link->name        | ptr: {:#?}",
196			&tree.get_root_link().try_read().unwrap().name().as_ptr()
197		);
198		trace!(
199			target: "RDB-RS::test::KineTree::clone_single",
200			"cloned_tree->..->root_link->name | ptr: {:#?}\n",
201			&cloned_tree
202				.get_root_link()
203				.try_read()
204				.unwrap()
205				.name()
206				.as_ptr()
207		);
208		assert_eq!(
209			&tree.get_root_link().try_read().unwrap().name(),
210			&cloned_tree.get_root_link().try_read().unwrap().name()
211		);
212
213		trace!(
214			target: "RDB-RS::test::KineTree::clone_single",
215			"tree->..->root_link->tree        | ptr: {:#?}",
216			Weak::as_ptr(&tree.get_root_link().try_read().unwrap().tree)
217		);
218		trace!(
219			target: "RDB-RS::test::KineTree::clone_single",
220			"cloned_tree->..->root_link->tree | ptr: {:#?}\n",
221			Weak::as_ptr(&cloned_tree.get_root_link().try_read().unwrap().tree)
222		);
223		assert!(!Weak::ptr_eq(
224			&tree.get_root_link().try_read().unwrap().tree,
225			&cloned_tree.get_root_link().try_read().unwrap().tree
226		));
227
228		trace!(
229			target: "RDB-RS::test::KineTree::clone_single",
230			"tree->..->root_link->direct_parent->0        | ptr: {:#?}",
231			Weak::as_ptr(
232				match &tree
233					.get_root_link()
234					.try_read()
235					.unwrap()
236					.parent()
237				{
238					LinkParent::KinematicTree(weak_tree) => weak_tree,
239					LinkParent::Joint(_) => panic!("This should not return a Joint Parent"),
240				}
241			)
242		);
243		trace!(
244			target: "RDB-RS::test::KineTree::clone_single",
245			"cloned_tree->..->root_link->direct_parent->0 | ptr: {:#?}\n",
246			Weak::as_ptr(
247				match &cloned_tree
248					.get_root_link()
249					.try_read()
250					.unwrap()
251					.parent()
252				{
253					LinkParent::KinematicTree(weak_tree) => weak_tree,
254					LinkParent::Joint(_) => panic!("This should not return a Joint Parent"),
255				}
256			)
257		);
258		assert_ne!(
259			&tree.get_root_link().try_read().unwrap().parent(),
260			&cloned_tree.get_root_link().try_read().unwrap().parent()
261		);
262
263		trace!(
264			target: "RDB-RS::test::KineTree::clone_single",
265			"tree->..->root_link->child_joints:        {:#?}",
266			&tree.get_root_link().try_read().unwrap().joints()
267		);
268		trace!(
269			target: "RDB-RS::test::KineTree::clone_single",
270			"cloned_tree->..->root_link->child_joints: {:#?}\n",
271			&cloned_tree.get_root_link().try_read().unwrap().joints()
272		);
273		assert_eq!(
274			tree.get_root_link().try_read().unwrap().joints().len(),
275			cloned_tree
276				.get_root_link()
277				.try_read()
278				.unwrap()
279				.joints()
280				.len()
281		);
282
283		trace!(
284			target: "RDB-RS::test::KineTree::clone_single",
285			"tree->..->links        | ptr: {:#?}",
286			Arc::as_ptr(&tree.get_links())
287		);
288		trace!(
289			target: "RDB-RS::test::KineTree::clone_single",
290			"cloned_tree->..->links | ptr: {:#?}\n",
291			Arc::as_ptr(&cloned_tree.get_links())
292		);
293		assert!(!Arc::ptr_eq(&tree.get_links(), &cloned_tree.get_links()));
294		assert_eq!(
295			tree.get_links().try_read().unwrap().len(),
296			cloned_tree.get_links().try_read().unwrap().len()
297		);
298
299		trace!(
300			target: "RDB-RS::test::KineTree::clone_single",
301			"tree->..->links[\"example-link\"]        | ptr: {:#?}",
302			Weak::as_ptr(
303				&tree
304					.get_links()
305					.try_read()
306					.unwrap()
307					.get("example-link")
308					.unwrap()
309			)
310		);
311		trace!(
312			target: "RDB-RS::test::KineTree::clone_single",
313			"cloned_tree->..->links[\"example-link\"] | ptr: {:#?}\n",
314			Weak::as_ptr(
315				&cloned_tree
316					.get_links()
317					.try_read()
318					.unwrap().get("example-link")
319					.unwrap()
320			)
321		);
322		assert!(!Weak::ptr_eq(
323			&tree
324				.get_links()
325				.try_read()
326				.unwrap()
327				.get("example-link")
328				.unwrap(),
329			&cloned_tree
330				.get_links()
331				.try_read()
332				.unwrap()
333				.get("example-link")
334				.unwrap()
335		));
336
337		trace!(
338			target: "RDB-RS::test::KineTree::clone_single",
339			"tree->..->root_link->child_joints:        {:#?}",
340			&tree.get_root_link().try_read().unwrap().joints()
341		);
342		trace!(
343			target: "RDB-RS::test::KineTree::clone_single",
344			"cloned_tree->..->root_link->child_joints: {:#?}\n",
345			&cloned_tree.get_root_link().try_read().unwrap().joints()
346		);
347		assert_eq!(
348			tree.get_root_link().try_read().unwrap().joints().len(),
349			cloned_tree
350				.get_root_link()
351				.try_read()
352				.unwrap()
353				.joints()
354				.len()
355		);
356
357		trace!(
358			target: "RDB-RS::test::KineTree::clone_single",
359			"tree->..->joints        | ptr: {:#?}",
360			Arc::as_ptr(&tree.get_joints())
361		);
362		trace!(
363			target: "RDB-RS::test::KineTree::clone_single",
364			"cloned_tree->..->joints | ptr: {:#?}\n",
365			Arc::as_ptr(&cloned_tree.get_joints())
366		);
367		assert!(!Arc::ptr_eq(&tree.get_joints(), &cloned_tree.get_joints()));
368		assert_eq!(
369			tree.get_joints().try_read().unwrap().len(),
370			cloned_tree.get_joints().try_read().unwrap().len()
371		);
372
373		trace!(
374			target: "RDB-RS::test::KineTree::clone_single",
375			"tree->..->newest_link        | ptr: {:#?}",
376			Arc::as_ptr(&tree.get_newest_link())
377		);
378		trace!(
379			target: "RDB-RS::test::KineTree::clone_single",
380			"cloned_tree->..->newest_link | ptr: {:#?}\n",
381			Arc::as_ptr(&cloned_tree.get_newest_link())
382		);
383		assert!(!Arc::ptr_eq(
384			&tree.get_newest_link(),
385			&cloned_tree.get_newest_link()
386		));
387	}
388
389	#[test]
390	fn clone_multi() {
391		let tree = LinkBuilder::new("example-link").build_tree();
392		let other_tree = LinkBuilder::new("other-link").build_tree();
393		other_tree
394			.get_newest_link()
395			.try_write()
396			.unwrap()
397			.try_attach_child(
398				JointBuilder::new("other-child-joint", JointType::Fixed),
399				LinkBuilder::new("other-child").build_tree(),
400			)
401			.unwrap();
402
403		tree.get_root_link()
404			.try_write()
405			.unwrap()
406			.try_attach_child(
407				JointBuilder::new("other-joint", JointType::Fixed),
408				other_tree,
409			)
410			.unwrap();
411
412		tree.get_root_link()
413			.try_write()
414			.unwrap()
415			.try_attach_child(
416				JointBuilder::new("three", JointType::Fixed),
417				LinkBuilder::new("3"),
418			)
419			.unwrap();
420
421		let cloned_tree = tree.clone();
422
423		trace!(
424			target: "RDB-RS::test::KineTree::clone_multi",
425			"tree->data        | ptr: {:#?}",
426			 Arc::as_ptr(&tree.0)
427		);
428		trace!(
429			target: "RDB-RS::test::KineTree::clone_multi",
430			"cloned_tree->data | ptr: {:#?}\n",
431			Arc::as_ptr(&cloned_tree.0)
432		);
433		assert!(!Arc::ptr_eq(&tree.0, &cloned_tree.0));
434
435		trace!(
436			target: "RDB-RS::test::KineTree::clone_multi",
437			"tree->..->root_link        | ptr: {:#?}",
438			Arc::as_ptr(&tree.get_root_link())
439		);
440		trace!(
441			target: "RDB-RS::test::KineTree::clone_multi",
442			"cloned_tree->..->root_link | ptr: {:#?}\n",
443			Arc::as_ptr(&cloned_tree.get_root_link())
444		);
445		assert!(!Arc::ptr_eq(
446			&tree.get_root_link(),
447			&cloned_tree.get_root_link()
448		));
449
450		// Note: This may not be permanent behavior
451		trace!(
452			target: "RDB-RS::test::KineTree::clone_multi",
453			"tree->..->root_link->name        | ptr: {:#?}",
454			&tree.get_root_link().try_read().unwrap().name().as_ptr()
455		);
456		trace!(
457			target: "RDB-RS::test::KineTree::clone_multi",
458			"cloned_tree->..->root_link->name | ptr: {:#?}\n",
459			&cloned_tree
460				.get_root_link()
461				.try_read()
462				.unwrap()
463				.name()
464				.as_ptr()
465		);
466		assert_eq!(
467			&tree.get_root_link().try_read().unwrap().name(),
468			&cloned_tree.get_root_link().try_read().unwrap().name()
469		);
470
471		trace!(
472			target: "RDB-RS::test::KineTree::clone_multi",
473			"tree->..->root_link->tree        | ptr: {:#?}",
474			Weak::as_ptr(&tree.get_root_link().try_read().unwrap().tree)
475		);
476		trace!(
477			target: "RDB-RS::test::KineTree::clone_multi",
478			"cloned_tree->..->root_link->tree | ptr: {:#?}\n",
479			Weak::as_ptr(&cloned_tree.get_root_link().try_read().unwrap().tree)
480		);
481		assert!(!Weak::ptr_eq(
482			&tree.get_root_link().try_read().unwrap().tree,
483			&cloned_tree.get_root_link().try_read().unwrap().tree
484		));
485
486		trace!(
487			target: "RDB-RS::test::KineTree::clone_multi",
488			"tree->..->root_link->direct_parent->0        | ptr: {:#?}",
489			Weak::as_ptr(
490				match &tree
491					.get_root_link()
492					.try_read()
493					.unwrap()
494					.parent()
495				{
496					LinkParent::KinematicTree(weak_tree) => weak_tree,
497					LinkParent::Joint(_) => panic!("This should not return a Joint Parent"),
498				}
499			)
500		);
501		trace!(
502			target: "RDB-RS::test::KineTree::clone_multi",
503			"cloned_tree->..->root_link->direct_parent->0 | ptr: {:#?}\n",
504			Weak::as_ptr(
505				match &cloned_tree
506					.get_root_link()
507					.try_read()
508					.unwrap()
509					.parent()
510				{
511					LinkParent::KinematicTree(weak_tree) => weak_tree,
512					LinkParent::Joint(_) => panic!("This should not return a Joint Parent"),
513				}
514			)
515		);
516		assert_ne!(
517			&tree.get_root_link().try_read().unwrap().parent(),
518			&cloned_tree.get_root_link().try_read().unwrap().parent()
519		);
520
521		trace!(
522			target: "RDB-RS::test::KineTree::clone_multi",
523			"tree->..->root_link->child_joints:        {:?}",
524			&tree
525				.get_root_link()
526				.try_read()
527				.unwrap()
528				.joints()
529				.iter()
530				.map(|joint| joint.read().unwrap().name().clone())
531				.collect::<Vec<String>>()
532		);
533		trace!(
534			target: "RDB-RS::test::KineTree::clone_multi",
535			"cloned_tree->..->root_link->child_joints: {:?}\n",
536			&cloned_tree
537				.get_root_link()
538				.try_read()
539				.unwrap()
540				.joints()
541				.iter()
542				.map(|joint| joint.read().unwrap().name().clone())
543				.collect::<Vec<String>>()
544		);
545		assert_eq!(
546			tree.get_root_link().read().unwrap().joints().len(),
547			cloned_tree
548				.get_root_link()
549				.try_read()
550				.unwrap()
551				.joints()
552				.len()
553		);
554
555		trace!(
556			target: "RDB-RS::test::KineTree::clone_multi",
557			"tree->..->links        | ptr: {:#?} | keys: {:?}",
558			Arc::as_ptr(&tree.get_links()),
559			&tree
560				.get_links()
561				.try_read()
562				.unwrap()
563				.keys()
564				.map(|key| key.clone())
565				.collect::<Vec<String>>()
566		);
567		trace!(
568			target: "RDB-RS::test::KineTree::clone_multi",
569			"cloned_tree->..->links | ptr: {:#?} | keys: {:?}\n",
570			Arc::as_ptr(&cloned_tree.get_links()),
571			&cloned_tree
572				.get_links()
573				.try_read()
574				.unwrap()
575				.keys()
576				.map(|key| key.clone())
577				.collect::<Vec<String>>()
578		);
579		assert!(!Arc::ptr_eq(&tree.get_links(), &cloned_tree.get_links()));
580		assert_eq!(
581			tree.get_links().try_read().unwrap().len(),
582			cloned_tree.get_links().try_read().unwrap().len()
583		);
584
585		trace!(
586			target: "RDB-RS::test::KineTree::clone_multi",
587			"tree->..->links[\"example-link\"]        | ptr: {:#?}",
588			Weak::as_ptr(
589				&tree
590					.get_links()
591					.try_read()
592					.unwrap()
593					.get("example-link")
594					.unwrap()
595			)
596		);
597		trace!(
598			target: "RDB-RS::test::KineTree::clone_multi",
599			"cloned_tree->..->links[\"example-link\"] | ptr: {:#?}\n",
600			Weak::as_ptr(
601				&cloned_tree
602					.get_links()
603					.try_read()
604					.unwrap()
605					.get("example-link")
606					.unwrap()
607			)
608		);
609		assert!(!Weak::ptr_eq(
610			&tree
611				.get_links()
612				.try_read()
613				.unwrap()
614				.get("example-link")
615				.unwrap(),
616			&cloned_tree
617				.get_links()
618				.try_read()
619				.unwrap()
620				.get("example-link")
621				.unwrap()
622		));
623
624		trace!(
625			target: "RDB-RS::test::KineTree::clone_multi",
626			"tree->..->root_link->child_joints:        {:#?}",
627			&tree.get_root_link().try_read().unwrap().joints()
628		);
629		trace!(
630			target: "RDB::test::KineTree::clone_multi",
631			"cloned_tree->..->root_link->child_joints: {:#?}\n",
632			&cloned_tree.get_root_link().try_read().unwrap().joints()
633		);
634		assert_eq!(
635			tree.get_root_link().try_read().unwrap().joints().len(),
636			cloned_tree
637				.get_root_link()
638				.try_read()
639				.unwrap()
640				.joints()
641				.len()
642		);
643
644		trace!(
645			target: "RDB-RS::test::KineTree::clone_multi",
646			"tree->..->joints        | ptr: {:#?}",
647			Arc::as_ptr(&tree.get_joints())
648		);
649		trace!(
650			target: "RDB-RS::test::KineTree::clone_multi",
651			"cloned_tree->..->joints | ptr: {:#?}\n",
652			Arc::as_ptr(&cloned_tree.get_joints())
653		);
654		assert!(!Arc::ptr_eq(&tree.get_joints(), &cloned_tree.get_joints()));
655		assert_eq!(
656			tree.get_joints().try_read().unwrap().len(),
657			cloned_tree.get_joints().try_read().unwrap().len()
658		);
659
660		trace!(
661			target: "RDB-RS::test::KineTree::clone_multi",
662			"tree->..->newest_link        | ptr: {:#?}",
663			Arc::as_ptr(&tree.get_newest_link())
664		);
665		trace!(
666			target: "RDB-RS::test::KineTree::clone_multi",
667			"cloned_tree->..->newest_link | ptr: {:#?}\n",
668			Arc::as_ptr(&cloned_tree.get_newest_link())
669		);
670		assert!(!Arc::ptr_eq(
671			&tree.get_newest_link(),
672			&cloned_tree.get_newest_link()
673		));
674	}
675}
robot_description_builder/cluster_objects/kinematic_tree.rs

robot_description_builder/cluster_objects/
kinematic_tree.rs
