robot_description_builder/link/
collision.rs

1#[cfg(feature = "urdf")]
2use crate::to_rdf::to_urdf::ToURDF;
3#[cfg(feature = "xml")]
4use quick_xml::{events::attributes::Attribute, name::QName};
5
6use super::{builder::CollisionBuilder, geometry::GeometryInterface};
7use crate::{identifiers::GroupID, transform::Transform};
8
9/// A `Collision` geometry for a `Link`.
10///
11/// This struct holds one of (the many) Colliders for the associated [`Link`].
12///  It can be constructed via the [`CollisionBuilder`] (accessable via the [`builder`](Self::builder) method) and [added while building the `Link`](crate::link::builder::LinkBuilder::add_collider).
13/// It contains the following data:
14/// - **[`geometry`](crate::link_data::geometry)**: The geometry used for collision checking[^mesh-warning].
15/// - **[`transform`](crate::Transform)** (Optional): The transform from the [`Link`] frame to the `geometry`.
16/// - **`name`** (Optional): The identifiers/names of this collision element. For practical purposes, it is recommended to use unique identifiers/names.
17///
18/// [^mesh-warning]: **WARNING:** It is not recommended to use high-detail meshes for collision geometries, since this will slow down the collision checking process.
19/// Also, keep in mind, that some simulators only support the use of convex meshes for collisions, if at all.
20///
21/// [`Link`]: crate::link::Link
22#[derive(Debug)]
23pub struct Collision {
24	// TODO: Add export option which generates name.
25	/// The [_string identifier_](crate::identifiers) or name of this collision element.
26	///
27	/// For practical purposes, it is recommended to use unique identifiers/names.
28	pub(crate) name: Option<String>,
29	/// The transform from the origin of the parent `Link` to the origin of this `Collision`.
30	///
31	/// This is the reference for the placement of the `geometry`.
32	///
33	/// In URDF this field is refered to as `<origin>`.
34	pub(crate) transform: Option<Transform>,
35	/// The geometry of this Collision element.
36	pub(crate) geometry: Box<dyn GeometryInterface + Sync + Send>,
37}
38
39impl Collision {
40	/// Create a new [`CollisionBuilder`] with the specified [`Geometry`](crate::link_data::geometry).
41	pub fn builder(
42		geometry: impl Into<Box<dyn GeometryInterface + Sync + Send>>,
43	) -> CollisionBuilder {
44		CollisionBuilder::new(geometry)
45	}
46
47	/// Gets an optional reference to the `name` of this `Collision`.
48	pub fn name(&self) -> Option<&String> {
49		self.name.as_ref()
50	}
51
52	/// Gets an optional reference to the `transform` of this `Collision`.
53	pub fn transform(&self) -> Option<&Transform> {
54		self.transform.as_ref()
55	}
56
57	/// Gets a reference to the `geometry` of this `Collision`.
58	pub fn geometry(&self) -> &Box<dyn GeometryInterface + Sync + Send> {
59		&self.geometry
60	}
61
62	/// Recreates the [`CollisionBuilder`], which was used to create this `Collision`.
63	pub fn rebuild(&self) -> CollisionBuilder {
64		CollisionBuilder {
65			name: self.name.clone(),
66			transform: self.transform,
67			geometry: self.geometry.boxed_clone(),
68		}
69	}
70}
71
72#[cfg(feature = "urdf")]
73impl ToURDF for Collision {
74	fn to_urdf(
75		&self,
76		writer: &mut quick_xml::Writer<std::io::Cursor<Vec<u8>>>,
77		urdf_config: &crate::to_rdf::to_urdf::URDFConfig,
78	) -> Result<(), quick_xml::Error> {
79		let mut element = writer.create_element("collision");
80		if let Some(name) = self.name() {
81			element = element.with_attribute(Attribute {
82				key: QName(b"name"),
83				value: name.display().as_bytes().into(),
84			});
85		}
86
87		element.write_inner_content(|writer| -> quick_xml::Result<()> {
88			if let Some(transform) = self.transform() {
89				transform.to_urdf(writer, urdf_config)?
90			}
91
92			self.geometry()
93				.shape_container()
94				.to_urdf(writer, urdf_config)?;
95			Ok(())
96		})?;
97
98		Ok(())
99	}
100}
101
102impl PartialEq for Collision {
103	fn eq(&self, other: &Self) -> bool {
104		self.name == other.name
105			&& self.transform == other.transform
106			&& *self.geometry == *other.geometry
107	}
108}
109
110impl Clone for Collision {
111	fn clone(&self) -> Self {
112		Self {
113			name: self.name.clone(),
114			transform: self.transform,
115			geometry: self.geometry.boxed_clone(),
116		}
117	}
118}
119
120#[cfg(test)]
121mod tests {
122	use std::f32::consts::PI;
123	use test_log::test;
124
125	use crate::{
126		link::{
127			builder::CollisionBuilder,
128			collision::Collision,
129			geometry::{BoxGeometry, CylinderGeometry, SphereGeometry},
130		},
131		transform::Transform,
132	};
133
134	#[cfg(feature = "urdf")]
135	mod to_urdf {
136		use super::{test, *};
137		use crate::to_rdf::to_urdf::{ToURDF, URDFConfig};
138		use std::io::Seek;
139
140		fn test_to_urdf_collision(
141			collision: CollisionBuilder,
142			result: String,
143			urdf_config: &URDFConfig,
144		) {
145			let mut writer = quick_xml::Writer::new(std::io::Cursor::new(Vec::new()));
146			assert!(collision.build().to_urdf(&mut writer, urdf_config).is_ok());
147
148			writer.get_mut().rewind().unwrap();
149
150			assert_eq!(
151				std::io::read_to_string(writer.into_inner()).unwrap(),
152				result
153			)
154		}
155
156		#[test]
157		fn no_name_no_origin() {
158			test_to_urdf_collision(
159				Collision::builder(BoxGeometry::new(1.0, 2.0, 3.0)),
160				String::from(r#"<collision><geometry><box size="1 2 3"/></geometry></collision>"#),
161				&URDFConfig::default(),
162			);
163		}
164
165		#[test]
166		fn name_no_origin() {
167			test_to_urdf_collision(
168				Collision::builder(CylinderGeometry::new(9., 6.258)).named("myLink_col"),
169				String::from(
170					r#"<collision name="myLink_col"><geometry><cylinder radius="9" length="6.258"/></geometry></collision>"#,
171				),
172				&URDFConfig::default(),
173			);
174		}
175
176		#[test]
177		fn no_name_origin() {
178			test_to_urdf_collision(
179				Collision::builder(SphereGeometry::new(3.))
180					.transformed(Transform::new((4., 6.78, 1.), (PI, 2. * PI, 0.))),
181				String::from(
182					r#"<collision><origin xyz="4 6.78 1" rpy="3.1415927 6.2831855 0"/><geometry><sphere radius="3"/></geometry></collision>"#,
183				),
184				&URDFConfig::default(),
185			);
186		}
187
188		#[test]
189		fn name_origin() {
190			test_to_urdf_collision(
191				Collision::builder(CylinderGeometry::new(4.5, 75.35))
192					.named("some_col")
193					.transformed(Transform::new_translation(5.4, 9.1, 7.8)),
194				String::from(
195					r#"<collision name="some_col"><origin xyz="5.4 9.1 7.8"/><geometry><cylinder radius="4.5" length="75.35"/></geometry></collision>"#,
196				),
197				&URDFConfig::default(),
198			);
199		}
200	}
201}
robot_description_builder/link/collision.rs

robot_description_builder/link/
collision.rs
