robot_description_builder/link/builder/
collision_builder.rs

1use nalgebra::Matrix3;
2
3use crate::{
4	identifiers::GroupIDChanger,
5	link::{
6		builder::VisualBuilder,
7		collision::Collision,
8		geometry::{GeometryInterface, GeometryShapeData},
9	},
10	transform::{Mirror, Transform},
11};
12
13/// The builder for `Collision` components.
14///
15/// The `CollisionBuilder` is used to construct [`Collision`] elements of [`Links`](crate::link::Link).
16///
17/// This will configure the collision data:
18/// - **[`geometry`](crate::link_data::geometry)**: The geometry used for collision checking[^mesh-warning].
19/// - **[`transform`](crate::Transform)** (Optional): The transform from the [`Link`] frame to the `geometry`.
20/// - **`name`** (Optional): The [_string identifier_](crate::identifiers) (or name) of this collision element. For practical purposes, it is recommended to use unique identifiers/names.
21///
22/// They can be added to a [`LinkBuilder`](super::LinkBuilder) while constructing a [`Link`] by calling [`add_collider`](crate::link::builder::LinkBuilder::add_collider).
23///
24/// A `CollisionBuilder` can be converted to a [`VisualBuilder`] to make defining [`Visual`](crate::link::visual::Visual) easier.
25/// If this is used, it might be easier to first create the [`VisualBuilder`], and convert that back to a `CollisionBuilder`, since it contains more information.
26///
27/// [^mesh-warning]: **WARNING:** It is not recommended to use high-detail meshes for collision geometries, since this will slow down the collision checking process.
28/// Also, keep in mind, that some simulators only support the use of convex meshes for collisions, if at all.
29///
30/// [`Link`]: crate::link::Link
31// TODO: Consider making the structfields public
32#[derive(Debug)]
33pub struct CollisionBuilder {
34	/// The [_string identifier_](crate::identifiers) or name of this collision element.
35	///
36	/// For practical purposes, it is recommended to use unique identifiers/names.
37	pub(crate) name: Option<String>,
38	/// The transform from the origin of the parent `Link` to the origin of this `Collision`.
39	///
40	/// This is the reference for the placement of the `geometry`.
41	///
42	/// In URDF this field is refered to as `<origin>`.
43	pub(crate) transform: Option<Transform>,
44	/// The geometry of this Collision element.
45	pub(crate) geometry: Box<dyn GeometryInterface + Sync + Send>,
46}
47
48impl CollisionBuilder {
49	/// Create a new [`CollisionBuilder`] with the specified [`Geometry`](crate::link_data::geometry).
50	pub fn new(geometry: impl Into<Box<dyn GeometryInterface + Sync + Send>>) -> Self {
51		Self {
52			name: None,
53			transform: None,
54			geometry: geometry.into(),
55		}
56	}
57	/// Create a new [`CollisionBuilder`] with all fields specified.
58	pub fn new_full(
59		name: Option<String>,
60		transform: Option<Transform>,
61		geometry: impl Into<Box<dyn GeometryInterface + Sync + Send>>,
62	) -> Self {
63		Self {
64			name,
65			transform,
66			geometry: geometry.into(),
67		}
68	}
69
70	/// Sets the `name` of this `CollisionBuilder`.
71	pub fn named(mut self, name: impl Into<String>) -> Self {
72		self.name = Some(name.into());
73		self
74	}
75
76	/// Specify a `transform` for this `CollisionBuilder`.
77	///
78	/// The default is a no transformation (The frame of the `Collision` will be the same as the frame of the parent `Link`).
79	pub fn transformed(mut self, transform: Transform) -> Self {
80		self.transform = Some(transform);
81		self
82	}
83
84	// TODO: IMPROVE DOCS
85	/// Creates a `VisualBuilder` from this `CollisionBuilder` reference by upgrading.
86	///
87	/// Creates a [`VisualBuilder`] from the `CollisionBuilder` by cloning the following fields:
88	///  - `name`
89	///  - `transform`
90	///  - `geometry`
91	/// The other fields are left empty, since they are optional.
92	pub fn to_visual(&self) -> VisualBuilder {
93		VisualBuilder {
94			name: self.name.clone(),
95			transform: self.transform,
96			geometry: self.geometry.boxed_clone(),
97			material_description: None,
98		}
99	}
100
101	pub(crate) fn build(self) -> Collision {
102		Collision {
103			name: self.name,
104			transform: self.transform,
105			geometry: self.geometry,
106		}
107	}
108
109	// TODO: BETTER NAME
110	pub(crate) fn get_geometry_data(&self) -> GeometryShapeData {
111		GeometryShapeData {
112			transform: self.transform.unwrap_or_default(),
113			geometry: self.geometry.shape_container(),
114		}
115	}
116}
117
118impl Mirror for CollisionBuilder {
119	fn mirrored(&self, mirror_matrix: &Matrix3<f32>) -> Self {
120		Self {
121			name: self.name.as_ref().cloned(), // TODO: Rename?
122			transform: self
123				.transform
124				.as_ref()
125				.map(|transform| transform.mirrored(mirror_matrix)),
126			geometry: self.geometry.boxed_mirrored(mirror_matrix),
127		}
128	}
129}
130
131/// Non-builder methods
132impl CollisionBuilder {
133	/// Gets an optional reference to the `name` of this `CollisionBuilder`.
134	pub fn name(&self) -> Option<&String> {
135		self.name.as_ref()
136	}
137
138	/// Gets an optional reference to the `transform` of this `CollisionBuilder`.
139	pub fn transform(&self) -> Option<&Transform> {
140		self.transform.as_ref()
141	}
142
143	/// Gets a reference to the [`geometry`](crate::link_data::geometry) of this `CollisionBuilder`.
144	pub fn geometry(&self) -> &Box<dyn GeometryInterface + Sync + Send> {
145		&self.geometry
146	}
147}
148
149impl GroupIDChanger for CollisionBuilder {
150	unsafe fn change_group_id_unchecked(&mut self, new_group_id: &str) {
151		if let Some(name) = self.name.as_mut() {
152			name.change_group_id_unchecked(new_group_id);
153		}
154	}
155
156	fn apply_group_id(&mut self) {
157		if let Some(name) = self.name.as_mut() {
158			name.apply_group_id();
159		}
160	}
161}
162
163impl PartialEq for CollisionBuilder {
164	fn eq(&self, other: &Self) -> bool {
165		self.name == other.name
166			&& self.transform == other.transform
167			&& *self.geometry == *other.geometry
168	}
169}
170
171impl Clone for CollisionBuilder {
172	fn clone(&self) -> Self {
173		Self {
174			name: self.name.clone(),
175			transform: self.transform,
176			geometry: self.geometry.boxed_clone(),
177		}
178	}
179}
180
181// TODO: Decide if this is ok?
182impl From<CollisionBuilder> for Collision {
183	fn from(value: CollisionBuilder) -> Self {
184		value.build()
185	}
186}
187
188#[cfg(test)]
189mod tests {
190	use super::CollisionBuilder;
191	use crate::link::link_data::geometry::{BoxGeometry, CylinderGeometry, SphereGeometry};
192	use test_log::test;
193	// TODO: Write tests
194
195	mod group_id_changer {
196		use super::{test, BoxGeometry, CollisionBuilder, CylinderGeometry, SphereGeometry};
197		use crate::identifiers::{GroupIDChanger, GroupIDError};
198
199		#[test]
200		fn change_group_id_unchecked() {
201			#[inline]
202			fn test(collision_builder: CollisionBuilder, new_group_id: &str, name: Option<&str>) {
203				let mut collision_builder = collision_builder;
204				unsafe {
205					collision_builder.change_group_id_unchecked(new_group_id);
206				}
207				assert_eq!(
208					collision_builder.name,
209					name.and_then(|name| Some(name.to_owned()))
210				)
211			}
212
213			// No Name
214			test(
215				CollisionBuilder::new(BoxGeometry::new(1., 2., 3.)),
216				"7",
217				None,
218			);
219			test(
220				CollisionBuilder::new(CylinderGeometry::new(32., 5.)),
221				"[[invalid]]",
222				None,
223			);
224			test(CollisionBuilder::new(SphereGeometry::new(3.3e9)), "", None);
225
226			// Named, but no GroupID
227			test(
228				CollisionBuilder::new(BoxGeometry::new(1., 2., 3.)).named("ThisCoolName"),
229				"7",
230				Some("ThisCoolName"),
231			);
232			test(
233				CollisionBuilder::new(CylinderGeometry::new(32., 5.)).named("ADAdsadsdasdDS[]"),
234				"valid4",
235				Some("ADAdsadsdasdDS[]"),
236			);
237			test(
238				CollisionBuilder::new(SphereGeometry::new(3.3e9)).named("Bal"),
239				"bol",
240				Some("Bal"),
241			);
242
243			// Named with GroupID and valid
244			test(
245				CollisionBuilder::new(BoxGeometry::new(1., 2., 3.)).named("Leg_[[L01]]_l04_col"),
246				"7",
247				Some("Leg_[[7]]_l04_col"),
248			);
249			test(
250				CollisionBuilder::new(CylinderGeometry::new(32., 5.))
251					.named("Arm_[[B01d]]_link_0313c"),
252				"valid4",
253				Some("Arm_[[valid4]]_link_0313c"),
254			);
255			test(
256				CollisionBuilder::new(SphereGeometry::new(3.3e9))
257					.named("Bal_[[F900]]_this_doesn't_matter"),
258				"G0-02",
259				Some("Bal_[[G0-02]]_this_doesn't_matter"),
260			);
261
262			// Named with GroupID and invalid
263			test(
264				CollisionBuilder::new(BoxGeometry::new(1., 2., 3.)).named("Leg_[[L01]]_l04_col"),
265				"[[7",
266				Some("Leg_[[[[7]]_l04_col"),
267			);
268			test(
269				CollisionBuilder::new(CylinderGeometry::new(32., 5.))
270					.named("Arm_[[B01d]]_link_0313c"),
271				"[[invalid]]",
272				Some("Arm_[[[[invalid]]]]_link_0313c"),
273			);
274			test(
275				CollisionBuilder::new(SphereGeometry::new(3.3e9))
276					.named("Bal_[[F900]]_this_doesn't_matter"),
277				"",
278				Some("Bal_[[]]_this_doesn't_matter"),
279			);
280		}
281
282		#[test]
283		fn change_group_id() {
284			#[inline]
285			fn test(
286				collision_builder: CollisionBuilder,
287				new_group_id: &str,
288				result_change: Result<(), GroupIDError>,
289				name: Option<&str>,
290			) {
291				let mut collision_builder = collision_builder;
292				assert_eq!(
293					collision_builder.change_group_id(new_group_id),
294					result_change
295				);
296				assert_eq!(
297					collision_builder.name,
298					name.and_then(|name| Some(name.to_owned()))
299				)
300			}
301
302			// No Name, valid
303			test(
304				CollisionBuilder::new(BoxGeometry::new(1., 2., 3.)),
305				"7",
306				Ok(()),
307				None,
308			);
309			test(
310				CollisionBuilder::new(CylinderGeometry::new(32., 5.)),
311				"valid5",
312				Ok(()),
313				None,
314			);
315			test(
316				CollisionBuilder::new(SphereGeometry::new(7.)),
317				"R04",
318				Ok(()),
319				None,
320			);
321
322			// No Name, invalid
323			test(
324				CollisionBuilder::new(BoxGeometry::new(1., 2., 3.)),
325				"7]]",
326				Err(GroupIDError::new_close("7]]")),
327				None,
328			);
329			test(
330				CollisionBuilder::new(CylinderGeometry::new(32., 5.)),
331				"[[invalid]]",
332				Err(GroupIDError::new_open("[[invalid]]")),
333				None,
334			);
335			test(
336				CollisionBuilder::new(SphereGeometry::new(3.3e9)),
337				"",
338				Err(GroupIDError::new_empty()),
339				None,
340			);
341
342			// Named, but no GroupID
343			test(
344				CollisionBuilder::new(BoxGeometry::new(1., 2., 3.)).named("ThisCoolName"),
345				"7",
346				Ok(()),
347				Some("ThisCoolName"),
348			);
349			test(
350				CollisionBuilder::new(CylinderGeometry::new(32., 5.)).named("ADAdsadsdasdDS[]"),
351				"valid4",
352				Ok(()),
353				Some("ADAdsadsdasdDS[]"),
354			);
355			test(
356				CollisionBuilder::new(SphereGeometry::new(3.3e9)).named("Bal"),
357				"bol",
358				Ok(()),
359				Some("Bal"),
360			);
361
362			// Named, but no GroupID and invalid
363			test(
364				CollisionBuilder::new(BoxGeometry::new(1., 2., 3.)).named("ThisCoolName"),
365				"7]]",
366				Err(GroupIDError::new_close("7]]")),
367				Some("ThisCoolName"),
368			);
369			test(
370				CollisionBuilder::new(CylinderGeometry::new(32., 5.)).named("ADAdsadsdasdDS[]"),
371				"[[invalid]]",
372				Err(GroupIDError::new_open("[[invalid]]")),
373				Some("ADAdsadsdasdDS[]"),
374			);
375			test(
376				CollisionBuilder::new(SphereGeometry::new(3.3e9)).named("Bal"),
377				"",
378				Err(GroupIDError::new_empty()),
379				Some("Bal"),
380			);
381
382			// Named with GroupID and valid
383			test(
384				CollisionBuilder::new(BoxGeometry::new(1., 2., 3.)).named("Leg_[[L01]]_l04_col"),
385				"7",
386				Ok(()),
387				Some("Leg_[[7]]_l04_col"),
388			);
389			test(
390				CollisionBuilder::new(CylinderGeometry::new(32., 5.))
391					.named("Arm_[[B01d]]_link_0313c"),
392				"valid4",
393				Ok(()),
394				Some("Arm_[[valid4]]_link_0313c"),
395			);
396			test(
397				CollisionBuilder::new(SphereGeometry::new(3.3e9))
398					.named("Bal_[[F900]]_this_doesn't_matter"),
399				"G0-02",
400				Ok(()),
401				Some("Bal_[[G0-02]]_this_doesn't_matter"),
402			);
403
404			// Named with GroupID and invalid
405			test(
406				CollisionBuilder::new(BoxGeometry::new(1., 2., 3.)).named("Leg_[[L01]]_l04_col"),
407				"[[7",
408				Err(GroupIDError::new_open("[[7")),
409				Some("Leg_[[L01]]_l04_col"),
410			);
411			test(
412				CollisionBuilder::new(CylinderGeometry::new(32., 5.))
413					.named("Arm_[[B01d]]_link_0313c"),
414				"[[invalid]]",
415				Err(GroupIDError::new_open("[[invalid]]")),
416				Some("Arm_[[B01d]]_link_0313c"),
417			);
418			test(
419				CollisionBuilder::new(SphereGeometry::new(3.3e9))
420					.named("Bal_[[F900]]_this_doesn't_matter"),
421				"",
422				Err(GroupIDError::new_empty()),
423				Some("Bal_[[F900]]_this_doesn't_matter"),
424			);
425		}
426
427		#[test]
428		fn apply_group_id() {
429			#[inline]
430			fn test(collision_builder: CollisionBuilder, name: Option<&str>) {
431				let mut collision_builder = collision_builder;
432				collision_builder.apply_group_id();
433				assert_eq!(
434					collision_builder.name,
435					name.and_then(|name| Some(name.to_owned()))
436				)
437			}
438
439			// No Name
440			test(CollisionBuilder::new(BoxGeometry::new(1., 2., 3.)), None);
441			test(CollisionBuilder::new(CylinderGeometry::new(32., 5.)), None);
442			test(CollisionBuilder::new(SphereGeometry::new(7.)), None);
443
444			// Named, but no GroupID
445			test(
446				CollisionBuilder::new(BoxGeometry::new(1., 2., 3.)).named("ThisCoolName"),
447				Some("ThisCoolName"),
448			);
449			test(
450				CollisionBuilder::new(CylinderGeometry::new(32., 5.)).named("ADAdsadsdasdDS[]"),
451				Some("ADAdsadsdasdDS[]"),
452			);
453			test(
454				CollisionBuilder::new(SphereGeometry::new(3.3e9)).named("Bal"),
455				Some("Bal"),
456			);
457
458			// Named, but escaped
459			test(
460				CollisionBuilder::new(BoxGeometry::new(1., 2., 3.)).named("This[\\[Cool]\\]Name"),
461				Some("This[[Cool]]Name"),
462			);
463			test(
464				CollisionBuilder::new(CylinderGeometry::new(32., 5.)).named("ADAdsadsdasdDS[\\[]"),
465				Some("ADAdsadsdasdDS[[]"),
466			);
467			test(
468				CollisionBuilder::new(SphereGeometry::new(3.3e9)).named("Bal]\\]"),
469				Some("Bal]]"),
470			);
471
472			// Named with GroupID and valid
473			test(
474				CollisionBuilder::new(BoxGeometry::new(1., 2., 3.)).named("Leg_[[L01]]_l04_col"),
475				Some("Leg_L01_l04_col"),
476			);
477			test(
478				CollisionBuilder::new(CylinderGeometry::new(32., 5.))
479					.named("Arm_[[B01d]]_link_0313c"),
480				Some("Arm_B01d_link_0313c"),
481			);
482			test(
483				CollisionBuilder::new(SphereGeometry::new(3.3e9))
484					.named("Bal_[[F900]]_this_doesn't_matter"),
485				Some("Bal_F900_this_doesn't_matter"),
486			);
487
488			// Named with mixed
489			test(
490				CollisionBuilder::new(BoxGeometry::new(1., 2., 3.))
491					.named("Leg_[\\[L01]\\]_[[l04]]_col"),
492				Some("Leg_[[L01]]_l04_col"),
493			);
494			test(
495				CollisionBuilder::new(CylinderGeometry::new(32., 5.))
496					.named("Arm_[[B01d]\\]_[\\[link_0313c]]"),
497				Some("Arm_B01d]]_[[link_0313c"),
498			);
499			test(
500				CollisionBuilder::new(SphereGeometry::new(3.3e9))
501					.named("Bal_[[F900]]_this_[\\[doesn't]\\]_matter"),
502				Some("Bal_F900_this_[[doesn't]]_matter"),
503			);
504		}
505	}
506}
robot_description_builder/link/builder/collision_builder.rs

robot_description_builder/link/builder/
collision_builder.rs
