robot_description_builder/
joint.rs

1mod joint_tranform_mode;
2mod jointbuilder;
3
4#[cfg(not(feature = "smart-joint-extension"))]
5mod smartjointbuilder;
6#[cfg(feature = "smart-joint-extension")]
7pub mod smartjointbuilder;
8
9/// TODO: Pub(crate) for now
10/// pub(crate)
11/// PUB FOR NOW FOR DOC TESTS????
12pub mod joint_data;
13
14pub use joint_tranform_mode::JointTransformMode;
15pub(crate) use jointbuilder::BuildJointChain;
16pub use jointbuilder::{BuildJoint, JointBuilder};
17pub use smartjointbuilder::SmartJointBuilder;
18
19#[cfg(feature = "xml")]
20use std::borrow::Cow;
21use std::sync::{Arc, Weak};
22
23#[cfg(feature = "urdf")]
24use crate::to_rdf::to_urdf::ToURDF;
25use crate::{
26	chained::Chained,
27	cluster_objects::kinematic_data_tree::KinematicDataTree,
28	identifiers::GroupID,
29	link::Link,
30	transform::Transform,
31	utils::{ArcLock, ArcRW, WeakLock},
32	yank_errors::{RebuildBranchError, YankJointError},
33};
34
35#[cfg(feature = "xml")]
36use quick_xml::{events::attributes::Attribute, name::QName};
37
38#[derive(Debug)]
39pub struct Joint {
40	/// The name of the `Joint`
41	pub(crate) name: String,
42	/// A Reference to the parent Kinematic Tree
43	pub(crate) tree: Weak<KinematicDataTree>,
44	/// A Reference to the parent `Link`
45	pub(crate) parent_link: WeakLock<Link>,
46	pub(crate) child_link: ArcLock<Link>, //temp pub TODO: THIS PROBABLY ISN'T THE NICEST WAY TO DO THIS.
47	/// The information specific to the JointType: TODO: DECIDE IF THIS SHOULD BE PUBLIC
48	pub(crate) joint_type: JointType,
49	/// The transform from the origin of the parent `Link` to this `Joint`'s origin.
50	///
51	/// In URDF this field is refered to as `<origin>`
52	transform: Transform,
53	axis: Option<(f32, f32, f32)>,
54	calibration: joint_data::CalibrationData,
55	dynamics: joint_data::DynamicsData,
56	limit: Option<joint_data::LimitData>,
57	/// TODO: Should be editable
58	mimic: Option<joint_data::MimicData>,
59	safety_controller: Option<joint_data::SafetyControllerData>,
60
61	me: WeakLock<Joint>,
62}
63
64impl Joint {
65	pub fn name(&self) -> &String {
66		&self.name
67	}
68
69	pub fn joint_type(&self) -> JointType {
70		self.joint_type
71	}
72
73	/// Returns a reference to the parent `Link`
74	///
75	/// TODO: ADD EXAMPLE
76	///
77	/// For now pub crate, this should maybe go to joint trait
78	pub fn parent_link(&self) -> ArcLock<Link> {
79		// If this panics, the Joint is not initialized propperly.
80		self.parent_link
81			.upgrade()
82			.expect("Joint's parent link should be set")
83	}
84
85	/// For now pub crate, this should maybe go to joint trait
86	/// Is this even necessary?
87	pub fn child_link(&self) -> ArcLock<Link> {
88		Arc::clone(&self.child_link)
89	}
90
91	/// FIXME: pub(crate) for now
92	pub(crate) fn child_link_ref(&self) -> &ArcLock<Link> {
93		&self.child_link
94	}
95
96	pub fn transform(&self) -> &Transform {
97		&self.transform
98	}
99
100	pub fn axis(&self) -> Option<(f32, f32, f32)> {
101		// This is fine since it implements Copy
102		self.axis
103	}
104
105	/// Make a `JointBuilder` to build a 'Clone' of the `Joint`.
106	///
107	/// This method does not clone the child of the [`Joint`], only the `Joint` is self.
108	///
109	/// If the whole branch needs to be copied use [`rebuild_branch`](Self::rebuild_branch()).
110	pub fn rebuild(&self) -> JointBuilder {
111		#[cfg(any(feature = "logging", test))]
112		log::info!(target: "JointBuilder","Rebuilding: {}", self.name());
113		JointBuilder {
114			name: self.name.clone(),
115			joint_type: self.joint_type,
116			// child is None, since this method only rebuilds the current Joint
117			child: None,
118			transform: self.transform.into(),
119			axis: self.axis,
120			calibration: self.calibration,
121			dynamics: self.dynamics,
122			limit: self.limit,
123			mimic: self.mimic.clone().map(|mimic| mimic.into()),
124			safety_controller: self.safety_controller,
125		}
126	}
127
128	pub(crate) fn rebuild_branch_continued(&self) -> Result<JointBuilder, RebuildBranchError> {
129		#[cfg(any(feature = "logging", test))]
130		log::info!(target: "JointBuilder","Rebuilding: {}", self.name());
131		Ok(JointBuilder {
132			child: Some(self.child_link.mread()?.rebuild_branch_continued()?),
133			..self.rebuild()
134		})
135	}
136
137	// TODO: DOCS
138	pub fn rebuild_branch(&self) -> Result<Chained<JointBuilder>, RebuildBranchError> {
139		#[cfg(any(feature = "logging", test))]
140		log::info!(target: "JointBuilder","Starting Branch Rebuilding: {}", self.name());
141		Ok(Chained(self.rebuild_branch_continued()?))
142	}
143
144	/// TODO:Find a way to make these builders special?
145	/// - Fix Documentation
146	///
147	/// Still need to purge
148	///
149	/// NOTE: you must get the link from the rep by cloning.
150	/// TODO: Maybe add a `first` argument to only set the `newest_link` if it is the first in the call stack
151	pub(crate) fn yank(&self) -> Result<JointBuilder, YankJointError> {
152		let builder = self.rebuild_branch_continued()?;
153
154		#[cfg(any(feature = "logging", test))]
155		log::info!("Yanked Joint \"{}\"", self.name());
156
157		self.parent_link()
158			.mwrite()?
159			.joints_mut()
160			.retain(|joint| !Arc::ptr_eq(&self.get_self(), joint));
161
162		// TODO: This is most-likely Ok, however it could error on the write.
163		*self.tree.upgrade().unwrap().newest_link.write().unwrap() = Weak::clone(&self.parent_link);
164
165		Ok(builder)
166	}
167
168	/// Gets a (strong) refence to the current `Joint`. (An `Arc<RwLock<Joint>>`)
169	pub fn get_self(&self) -> ArcLock<Joint> {
170		// Unwrapping is Ok here, because if the Joint exists, its self refence should exist.
171		Weak::upgrade(&self.me).unwrap()
172	}
173
174	/// Gets a weak refence to the current `Joint`. (An `Weak<RwLock<Joint>>`)
175	pub fn get_weak_self(&self) -> WeakLock<Joint> {
176		Weak::clone(&self.me)
177	}
178}
179
180#[cfg(feature = "urdf")]
181impl ToURDF for Joint {
182	fn to_urdf(
183		&self,
184		writer: &mut quick_xml::Writer<std::io::Cursor<Vec<u8>>>,
185		urdf_config: &crate::to_rdf::to_urdf::URDFConfig,
186	) -> Result<(), quick_xml::Error> {
187		let element = writer
188			.create_element("joint")
189			.with_attribute(Attribute {
190				key: QName(b"name"),
191				value: self.name().display().as_bytes().into(),
192			})
193			.with_attribute(Attribute {
194				key: QName(b"type"),
195				value: self.joint_type().into(),
196			});
197
198		element.write_inner_content(|writer| -> quick_xml::Result<()> {
199			let transform = self.transform();
200			if transform.contains_some() {
201				transform.to_urdf(writer, urdf_config)?;
202			}
203
204			writer
205				.create_element("parent")
206				.with_attribute(Attribute {
207					key: QName(b"link"),
208					value: self
209						.parent_link()
210						.read()
211						.unwrap() // FIXME: Is unwrap Ok HEre?
212						.name()
213						.display()
214						.as_bytes()
215						.into(),
216				})
217				.write_empty()?;
218
219			writer
220				.create_element("child")
221				.with_attribute(Attribute {
222					key: QName(b"link"),
223					value: self
224						.child_link()
225						.read()
226						.unwrap() // FIXME: Is unwrap Ok HEre?
227						.name()
228						.display()
229						.as_bytes()
230						.into(),
231				})
232				.write_empty()?;
233
234			if let Some((x, y, z)) = &self.axis {
235				// TODO: Fix '<axis xyz="-0 -0 -1"/>' after mirror.
236				writer
237					.create_element("axis")
238					.with_attribute(Attribute {
239						key: QName(b"xyz"),
240						value: format!("{} {} {}", x, y, z).as_bytes().into(),
241					})
242					.write_empty()?;
243			}
244
245			self.calibration.to_urdf(writer, urdf_config)?;
246			self.dynamics.to_urdf(writer, urdf_config)?;
247
248			if let Some(limit) = &self.limit {
249				limit.to_urdf(writer, urdf_config)?;
250			}
251
252			// TODO: TEST INTEGRATION OF THESE
253			if let Some(mimic) = &self.mimic {
254				mimic.to_urdf(writer, urdf_config)?;
255			}
256
257			if let Some(safety_contoller) = &self.safety_controller {
258				safety_contoller.to_urdf(writer, urdf_config)?;
259			}
260
261			Ok(())
262		})?;
263
264		self.child_link()
265			.read()
266			.unwrap() // FIXME: Is unwrap Ok HEre?
267			.to_urdf(writer, urdf_config)?;
268		Ok(())
269	}
270}
271
272/// TODO: Maybe remove some fields from check, since it will always match if name and tree are true
273impl PartialEq for Joint {
274	fn eq(&self, other: &Self) -> bool {
275		Weak::ptr_eq(&self.me, &other.me)
276			&& self.name == other.name
277			&& Weak::ptr_eq(&self.tree, &other.tree)
278			&& Weak::ptr_eq(&self.parent_link, &other.parent_link)
279			&& Arc::ptr_eq(&self.child_link, &other.child_link)
280			&& self.joint_type == other.joint_type
281			&& self.transform == other.transform
282			&& self.axis() == other.axis()
283			&& self.calibration == other.calibration
284			&& self.dynamics == other.dynamics
285			&& self.limit == other.limit
286			&& self.mimic == other.mimic
287			&& self.safety_controller == other.safety_controller
288		// `self.me` field is not covered however if they are on the same tree it is impossible to not match `self.me` while matching all the other fields
289	}
290}
291
292// TODO: Might add data of specif joint type to Struct Spaces.
293// TODO: Expand Jointtype specifics
294/// An enum to represent the the types of `Joint`.
295///
296/// Currently, only URDF types are listed.
297///
298/// It is important to note that multi-axis jointtypes, like [`Floating`](JointType::Floating) and [`Planar`](JointType::Planar), are often incompatible with lots of different tooling for ROS/URDF, like `sensor_msgs/JointState` messages from for example [`joint_state_publisher`](https://github.com/ros/joint_state_publisher/blob/7cb7069d2d78ebe4b8b80adc6bd859df0c8ccfc9/joint_state_publisher/src/joint_state_publisher/__init__.py#L83-L85C29) which has chosen to ignore it.
299/// This is a result of most programs (like [`kdl_parser`](https://github.com/ros/kdl_parser/blob/74d4ee3bc6938de8ae40a700997baef06114ea1b/kdl_parser/src/kdl_parser.cpp#L103) and `joint_state_publisher`) only supporting single axis joints.
300/// Gazebo/[SDFormat](http://sdformat.org/spec?ver=1.10&elem=joint#joint_type) supports multi-access joints, but do not have an `Floating` or `Planar` equivalent.
301/// However, these `JointType`s (Mostly [`Planar`](JointType::Planar)) can be approximated by a combination of other joints.  
302///
303/// The sections cited from the URDF specification are accurate as of [2023-08-21](https://wiki.ros.org/urdf/XML/joint#Attributes).
304#[derive(Debug, PartialEq, Eq, Clone, Copy, Default)]
305pub enum JointType {
306	/// A Fixed joint.
307	///
308	/// The URDF Specification says the following:
309	/// > fixed — this is not really a joint because it cannot move. All degrees of freedom are locked. This type of joint does not require the `<axis>`, `<calibration>`, `<dynamics>`, `<limits>` or `<safety_controller>`.
310	#[default]
311	Fixed,
312	/// A Revolute joint. (A limited rotational joint, like a weld)
313	///
314	/// This joint rotates a limited range around a specified (or default) axis.
315	///
316	/// The URDF Specification says the following:
317	/// > revolute — a hinge joint that rotates along the axis and has a limited range specified by the upper and lower limits.
318	Revolute,
319	/// A Continuous rotational joint. (A bearing or motor connection, like a wheel)
320	///
321	/// This joint rotates around a specified (or default) axis.
322	///
323	/// Since this Jointtype is unlimited in its movement, the [Limit](joint_data::LimitData) should not be specified (??TODO: [`effort`](joint_data::LimitData::effort) and [`velocity`](joint_data::LimitData::velocity) migth be useable??).
324	///
325	/// The URDF Specification says the following:
326	/// > continuous — a continuous hinge joint that rotates around the axis and has no upper and lower limits.
327	Continuous,
328	/// A Prismitic joint. (A limited sliding joint, like a drawer rail or a linear actuator)
329	///
330	/// This joint slides a limited range along a specified (or default) axis.
331	///
332	/// The URDF Specification says the following:
333	/// > prismatic — a sliding joint that slides along the axis, and has a limited range specified by the upper and lower limits.
334	Prismatic,
335	/// A Floating joint. (Or a non-connection)
336	///
337	/// This is a joint to represent an unconnected link.
338	/// Most parsers do not handle this `JointType`, since it's not an actual joint and it causes problems with lots of different tooling for ROS/URDF, like `sensor_msgs/JointState` messages from for example [`joint_state_publisher`](https://github.com/ros/robot_model/issues/188) which has chosen to ignore it.
339	///
340	/// The URDF Specification says the following:
341	/// > floating — this joint allows motion for all 6 degrees of freedom.
342	Floating,
343	/// A Planar joint. (A plane contact, like a magnet on a metal sheet)
344	///
345	/// This joint slides on the plane perpendicular the specified (or default) axis.
346	/// This `JointType` might cause problems with lots of different tooling for ROS/URDF, like `sensor_msgs/JointState` messages from for example [`joint_state_publisher`](https://github.com/ros/joint_state_publisher/blob/7cb7069d2d78ebe4b8b80adc6bd859df0c8ccfc9/joint_state_publisher/src/joint_state_publisher/__init__.py#L83-L85C29) which has chosen to ignore it.
347	///
348	/// The URDF Specification says the following:
349	/// > planar — this joint allows motion in a plane perpendicular to the axis.
350	Planar,
351}
352
353impl ToString for JointType {
354	fn to_string(&self) -> String {
355		match self {
356			JointType::Fixed => String::from("fixed"),
357			JointType::Revolute => String::from("revolute"),
358			JointType::Continuous => String::from("continuous"),
359			JointType::Prismatic => String::from("prismatic"),
360			JointType::Floating => String::from("floating"),
361			JointType::Planar => String::from("planar"),
362		}
363	}
364}
365
366#[cfg(feature = "xml")]
367impl From<JointType> for Cow<'_, [u8]> {
368	fn from(value: JointType) -> Self {
369		value.to_string().into_bytes().into()
370	}
371}
372
373#[cfg(test)]
374mod tests {
375
376	use crate::{
377		cluster_objects::KinematicInterface,
378		joint::{joint_data, smartjointbuilder::SmartJointBuilder, JointBuilder, JointType},
379		link::{
380			builder::LinkBuilder,
381			link_data::{
382				geometry::{BoxGeometry, CylinderGeometry, SphereGeometry},
383				Visual,
384			},
385		},
386		linkbuilding::{CollisionBuilder, VisualBuilder},
387		material::MaterialDescriptor,
388		transform::Transform,
389	};
390	use test_log::test;
391
392	#[test]
393	fn rebuild() {
394		let tree = LinkBuilder::new("root").build_tree();
395		tree.get_newest_link()
396			.try_write()
397			.unwrap()
398			.try_attach_child(
399				SmartJointBuilder::new("Joint1")
400					.fixed()
401					.add_transform(Transform::new_translation(2.0, 3.0, 5.0)),
402				LinkBuilder::new("child"),
403			)
404			.unwrap();
405
406		let rebuilder = tree
407			.get_joint("Joint1")
408			.unwrap()
409			.try_read()
410			.unwrap()
411			.rebuild();
412		assert_eq!(
413			rebuilder,
414			JointBuilder::new("Joint1", crate::JointType::Fixed).add_origin_offset((2.0, 3.0, 5.0))
415		)
416	}
417
418	#[test]
419	fn yank_simple() {
420		let material_red = MaterialDescriptor::new_color(1., 0., 0., 1.).named("Red");
421
422		let tree = LinkBuilder::new("link-0")
423			.add_collider(CollisionBuilder::new(BoxGeometry::new(1.0, 2.0, 3.0)))
424			.add_visual(
425				Visual::builder(BoxGeometry::new(1.0, 2.0, 3.0)).materialized(material_red.clone()),
426			)
427			.build_tree();
428
429		tree.get_root_link()
430			.try_write()
431			.unwrap()
432			.try_attach_child(
433				SmartJointBuilder::new("joint-0")
434					.add_transform(Transform::new_translation(1.0, 0., 0.))
435					.fixed(),
436				LinkBuilder::new("link-1")
437					.add_collider(
438						CollisionBuilder::new(SphereGeometry::new(4.))
439							.transformed(Transform::new_translation(2., 0., 0.)),
440					)
441					.add_visual(
442						Visual::builder(SphereGeometry::new(4.))
443							.transformed(Transform::new_translation(2., 0., 0.))
444							.materialized(material_red.clone()),
445					),
446			)
447			.unwrap();
448
449		assert_eq!(tree.get_links().try_read().unwrap().len(), 2);
450		assert_eq!(tree.get_joints().try_read().unwrap().len(), 1);
451		assert_eq!(tree.get_materials().try_read().unwrap().len(), 1);
452
453		// let joint =Arc::clone(tree.get_root_link().try_read().unwrap().get_joints().last().unwrap());
454		// let builder = joint.try_read().unwrap().yank();
455
456		// let builder = tree
457		// 	.get_joint("joint-0")
458		// 	.unwrap()
459		// 	.try_read()
460		// 	.unwrap()
461		// 	.yank();
462		let builder = tree.yank_joint("joint-0").unwrap();
463
464		assert_eq!(tree.get_links().try_read().unwrap().len(), 1);
465		assert_eq!(tree.get_joints().try_read().unwrap().len(), 0);
466
467		assert_eq!(
468			builder.0,
469			JointBuilder {
470				name: "joint-0".into(),
471				joint_type: JointType::Fixed,
472				transform: Transform {
473					translation: Some((1., 0., 0.)),
474					rotation: None
475				}
476				.into(),
477				child: Some(LinkBuilder {
478					name: "link-1".into(),
479					visuals: vec![VisualBuilder::new_full(
480						None,
481						Some(Transform {
482							translation: Some((2., 0., 0.)),
483							rotation: None
484						}),
485						SphereGeometry::new(4.),
486						Some(material_red.clone())
487					)],
488					colliders: vec![CollisionBuilder::new_full(
489						None,
490						Some(Transform {
491							translation: Some((2., 0., 0.)),
492							rotation: None
493						}),
494						SphereGeometry::new(4.)
495					)],
496					..Default::default()
497				}),
498				..Default::default() // TODO: Decide if this is Ok to do in a test
499			}
500		);
501
502		// TODO: Maybe the test is to simple
503	}
504
505	#[test]
506	fn yank_less_simple() {
507		let tree = {
508			let material_red = MaterialDescriptor::new_color(1., 0., 0., 1.).named("Red");
509
510			LinkBuilder::new("link-0")
511				.add_collider(CollisionBuilder::new(BoxGeometry::new(1.0, 2.0, 3.0)))
512				.add_visual(
513					Visual::builder(BoxGeometry::new(1.0, 2.0, 3.0))
514						.materialized(material_red.clone()),
515				)
516				.build_tree()
517		};
518
519		tree.get_root_link()
520			.try_write()
521			.unwrap()
522			.try_attach_child(
523				SmartJointBuilder::new("joint-0")
524					.add_transform(Transform::new_translation(1.0, 0., 0.))
525					.fixed(),
526				{
527					let tmp_tree = LinkBuilder::new("link-1")
528						.add_collider(
529							CollisionBuilder::new(SphereGeometry::new(4.))
530								.transformed(Transform::new_translation(2., 0., 0.)),
531						)
532						.add_visual(
533							Visual::builder(SphereGeometry::new(4.))
534								.transformed(Transform::new_translation(2., 0., 0.))
535								.materialized(
536									MaterialDescriptor::new_color(0., 0., 1., 1.).named("Blue"),
537								),
538						)
539						.build_tree();
540
541					tmp_tree
542						.get_root_link()
543						.write()
544						.unwrap()
545						.try_attach_child(
546							SmartJointBuilder::new("joint-1-1")
547								.revolute()
548								.add_transform(Transform::new_translation(4., 0., 0.))
549								.with_axis((0., 0., 1.))
550								.with_limit(100., 1000.)
551								.set_upper_limit(std::f32::consts::FRAC_PI_6)
552								.set_lower_limit(-std::f32::consts::FRAC_PI_6),
553							LinkBuilder::new("link-1-1").add_visual(
554								Visual::builder(CylinderGeometry::new(0.5, 18.))
555									.named("link-1-1-vis")
556									.transformed(Transform::new_translation(9., 0.5, 0.))
557									.materialized(MaterialDescriptor::new_color(
558										0.5, 0.5, 0.5, 0.75,
559									)),
560							),
561						)
562						.unwrap();
563
564					tmp_tree
565				},
566			)
567			.unwrap();
568
569		tree.get_root_link()
570			.write()
571			.unwrap()
572			.try_attach_child(
573				JointBuilder::new("joint-2", JointType::Fixed).add_origin_offset((0., 0., 1.5)),
574				LinkBuilder::new("link-2").build_tree(),
575			)
576			.unwrap();
577
578		assert_eq!(tree.get_links().try_read().unwrap().len(), 4);
579		assert_eq!(tree.get_joints().try_read().unwrap().len(), 3);
580		assert_eq!(tree.get_materials().try_read().unwrap().len(), 2);
581
582		assert_eq!(tree.get_root_link().try_read().unwrap().name(), "link-0");
583		assert_eq!(tree.get_newest_link().try_read().unwrap().name(), "link-2");
584
585		{
586			let tree = tree.clone();
587			let yanked_branch = tree.yank_joint("joint-2");
588
589			assert!(yanked_branch.is_some());
590
591			assert_eq!(tree.get_links().try_read().unwrap().len(), 3);
592			assert_eq!(tree.get_joints().try_read().unwrap().len(), 2);
593			assert_eq!(tree.get_materials().try_read().unwrap().len(), 2);
594
595			{
596				let mut link_keys: Vec<String> = tree
597					.get_links()
598					.try_read()
599					.unwrap()
600					.keys()
601					.cloned()
602					.collect();
603				link_keys.sort();
604
605				assert_eq!(link_keys, vec!["link-0", "link-1", "link-1-1"]);
606			}
607			{
608				let mut joint_keys: Vec<String> = tree
609					.get_joints()
610					.try_read()
611					.unwrap()
612					.keys()
613					.cloned()
614					.collect();
615				joint_keys.sort();
616
617				assert_eq!(joint_keys, vec!["joint-0", "joint-1-1"]);
618			}
619			{
620				let mut material_keys: Vec<String> = tree
621					.get_materials()
622					.try_read()
623					.unwrap()
624					.keys()
625					.cloned()
626					.collect();
627				material_keys.sort();
628
629				assert_eq!(material_keys, vec!["Blue", "Red"]);
630			}
631
632			assert_eq!(tree.get_root_link().read().unwrap().name(), "link-0");
633			assert_eq!(tree.get_newest_link().read().unwrap().name(), "link-0");
634
635			assert_eq!(
636				yanked_branch.unwrap().0,
637				JointBuilder {
638					name: "joint-2".into(),
639					joint_type: JointType::Fixed,
640					transform: Transform {
641						translation: Some((0., 0., 1.5)),
642						..Default::default()
643					}
644					.into(),
645					child: Some(LinkBuilder::new("link-2")),
646					..Default::default()
647				}
648			)
649		}
650
651		{
652			let tree = tree.clone();
653			let yanked_branch = tree.yank_joint("joint-1-1");
654
655			assert!(yanked_branch.is_some());
656
657			assert_eq!(tree.get_links().try_read().unwrap().len(), 3);
658			assert_eq!(tree.get_joints().try_read().unwrap().len(), 2);
659			assert_eq!(tree.get_materials().try_read().unwrap().len(), 2);
660
661			{
662				let mut link_keys: Vec<String> = tree
663					.get_links()
664					.try_read()
665					.unwrap()
666					.keys()
667					.cloned()
668					.collect();
669				link_keys.sort();
670
671				assert_eq!(link_keys, vec!["link-0", "link-1", "link-2"]);
672			}
673			{
674				let mut joint_keys: Vec<String> = tree
675					.get_joints()
676					.try_read()
677					.unwrap()
678					.keys()
679					.cloned()
680					.collect();
681				joint_keys.sort();
682
683				assert_eq!(joint_keys, vec!["joint-0", "joint-2"]);
684			}
685			{
686				let mut material_keys: Vec<String> = tree
687					.get_materials()
688					.try_read()
689					.unwrap()
690					.keys()
691					.cloned()
692					.collect();
693				material_keys.sort();
694
695				assert_eq!(material_keys, vec!["Blue", "Red"]);
696			}
697
698			assert_eq!(tree.get_root_link().read().unwrap().name(), "link-0");
699			assert_eq!(tree.get_newest_link().read().unwrap().name(), "link-1");
700
701			assert_eq!(
702				yanked_branch.unwrap().0,
703				JointBuilder {
704					name: "joint-1-1".into(),
705					joint_type: JointType::Revolute,
706					transform: Transform {
707						translation: Some((4., 0., 0.)),
708						..Default::default()
709					}
710					.into(),
711					child: Some(LinkBuilder {
712						name: "link-1-1".into(),
713						visuals: vec![VisualBuilder::new_full(
714							Some("link-1-1-vis".into()),
715							Some(Transform {
716								translation: Some((9., 0.5, 0.)),
717								..Default::default()
718							}),
719							CylinderGeometry::new(0.5, 18.),
720							Some(MaterialDescriptor::new_color(0.5, 0.5, 0.5, 0.75))
721						)],
722						..Default::default()
723					}),
724					axis: Some((0., 0., 1.)),
725					limit: Some(joint_data::LimitData {
726						effort: 100.,
727						velocity: 1000.,
728						lower: Some(-std::f32::consts::FRAC_PI_6),
729						upper: Some(std::f32::consts::FRAC_PI_6),
730					}),
731					..Default::default()
732				}
733			)
734		}
735
736		{
737			let tree = tree.clone();
738			let yanked_branch = tree.yank_joint("joint-0");
739
740			assert!(yanked_branch.is_some());
741
742			assert_eq!(tree.get_links().try_read().unwrap().len(), 2);
743			assert_eq!(tree.get_joints().try_read().unwrap().len(), 1);
744			assert_eq!(tree.get_materials().try_read().unwrap().len(), 2);
745
746			{
747				let mut link_keys: Vec<String> = tree
748					.get_links()
749					.try_read()
750					.unwrap()
751					.keys()
752					.cloned()
753					.collect();
754				link_keys.sort();
755
756				assert_eq!(link_keys, vec!["link-0", "link-2"]);
757			}
758			{
759				let mut joint_keys: Vec<String> = tree
760					.get_joints()
761					.try_read()
762					.unwrap()
763					.keys()
764					.cloned()
765					.collect();
766				joint_keys.sort();
767
768				assert_eq!(joint_keys, vec!["joint-2"]);
769			}
770			{
771				let mut material_keys: Vec<String> = tree
772					.get_materials()
773					.try_read()
774					.unwrap()
775					.keys()
776					.cloned()
777					.collect();
778				material_keys.sort();
779
780				assert_eq!(material_keys, vec!["Blue", "Red"]);
781			}
782
783			assert_eq!(tree.get_root_link().read().unwrap().name(), "link-0");
784			assert_eq!(tree.get_newest_link().read().unwrap().name(), "link-0");
785
786			assert_eq!(
787				yanked_branch.unwrap().0,
788				JointBuilder {
789					name: "joint-0".into(),
790					transform: Transform {
791						translation: Some((1., 0., 0.)),
792						..Default::default()
793					}
794					.into(),
795					child: Some(LinkBuilder {
796						name: "link-1".into(),
797						visuals: vec![VisualBuilder::new_full(
798							None,
799							Some(Transform {
800								translation: Some((2., 0., 0.)),
801								..Default::default()
802							}),
803							SphereGeometry::new(4.),
804							Some(MaterialDescriptor::new_color(0., 0., 1., 1.,).named("Blue"))
805						)],
806						colliders: vec![CollisionBuilder::new(SphereGeometry::new(4.))
807							.transformed(Transform::new_translation(2., 0., 0.))],
808						joints: vec![JointBuilder {
809							name: "joint-1-1".into(),
810							joint_type: JointType::Revolute,
811							transform: Transform {
812								translation: Some((4., 0., 0.)),
813								..Default::default()
814							}
815							.into(),
816							child: Some(LinkBuilder {
817								name: "link-1-1".into(),
818								visuals: vec![VisualBuilder::new_full(
819									Some("link-1-1-vis".into()),
820									Some(Transform {
821										translation: Some((9., 0.5, 0.)),
822										..Default::default()
823									}),
824									CylinderGeometry::new(0.5, 18.),
825									Some(MaterialDescriptor::new_color(0.5, 0.5, 0.5, 0.75))
826								)],
827								..Default::default()
828							}),
829							axis: Some((0., 0., 1.)),
830							limit: Some(joint_data::LimitData {
831								effort: 100.,
832								velocity: 1000.,
833								lower: Some(-std::f32::consts::FRAC_PI_6),
834								upper: Some(std::f32::consts::FRAC_PI_6),
835							}),
836							..Default::default()
837						}],
838						..Default::default()
839					}),
840					..Default::default()
841				}
842			)
843		}
844	}
845}
robot_description_builder/joint.rs

robot_description_builder/
joint.rs
