robot_description_builder/link/
inertial.rs

1use nalgebra::Matrix3;
2
3use crate::transform::{Mirror, Transform};
4
5#[cfg(feature = "urdf")]
6use crate::to_rdf::to_urdf::ToURDF;
7#[cfg(feature = "xml")]
8use quick_xml::{events::attributes::Attribute, name::QName};
9
10// TODO: Maybe rename to Inertial?
11#[derive(Debug, PartialEq, Clone, Copy, Default)]
12pub struct Inertial {
13	/// The transform from the parent [`Link`](super::Link)'s frame to the frame of the `InertialData`.
14	///
15	/// This is the reference for the placement of the center of mass and the moments of inertia.
16	///
17	/// In URDF this field is refered to as `<origin>`.
18	pub transform: Option<Transform>,
19	/// The mass of the current [`Link`](super::Link).
20	pub mass: f32,
21	/// The Moments of ineria around the x axis.
22	pub ixx: f32,
23	/// Product of inertia element xy.
24	pub ixy: f32,
25	/// Product of inertia element xz.
26	pub ixz: f32,
27	/// The Moments of ineria around the y axis.
28	pub iyy: f32,
29	/// Product of inertia element yz.
30	pub iyz: f32,
31	/// The Moments of ineria around the z axis.
32	pub izz: f32,
33}
34
35impl Mirror for Inertial {
36	fn mirrored(&self, mirror_matrix: &Matrix3<f32>) -> Self {
37		Self {
38			transform: self
39				.transform
40				.as_ref()
41				.map(|transform| transform.mirrored(mirror_matrix)),
42			..*self
43		}
44	}
45}
46
47#[cfg(feature = "urdf")]
48impl ToURDF for Inertial {
49	fn to_urdf(
50		&self,
51		writer: &mut quick_xml::Writer<std::io::Cursor<Vec<u8>>>,
52		urdf_config: &crate::to_rdf::to_urdf::URDFConfig,
53	) -> Result<(), quick_xml::Error> {
54		let element = writer.create_element("inertial");
55		element.write_inner_content(|writer| -> quick_xml::Result<()> {
56			if let Some(transform) = &self.transform {
57				transform.to_urdf(writer, urdf_config)?;
58			}
59
60			writer
61				.create_element("mass")
62				.with_attribute(Attribute {
63					key: QName(b"value"),
64					value: format!("{}", self.mass).as_bytes().into(),
65				})
66				.write_empty()?;
67
68			writer
69				.create_element("inertia")
70				.with_attributes([
71					("ixx", self.ixx.to_string().as_str()),
72					("ixy", self.ixy.to_string().as_str()),
73					("ixz", self.ixz.to_string().as_str()),
74					("iyy", self.iyy.to_string().as_str()),
75					("iyz", self.iyz.to_string().as_str()),
76					("izz", self.izz.to_string().as_str()),
77				])
78				.write_empty()?;
79
80			Ok(())
81		})?;
82
83		Ok(())
84	}
85}
86
87#[cfg(test)]
88mod tests {
89	use super::Inertial;
90	use test_log::test;
91
92	#[cfg(feature = "urdf")]
93	mod to_urdf {
94		use super::{test, *};
95
96		use crate::{
97			to_rdf::to_urdf::{ToURDF, URDFConfig},
98			transform::Transform,
99		};
100
101		use std::io::Seek;
102
103		fn test_to_urdf_inertial(
104			inertial_data: Inertial,
105			result: String,
106			urdf_config: &URDFConfig,
107		) {
108			let mut writer = quick_xml::Writer::new(std::io::Cursor::new(Vec::new()));
109			assert!(inertial_data.to_urdf(&mut writer, urdf_config).is_ok());
110
111			writer.get_mut().rewind().unwrap();
112
113			assert_eq!(
114				std::io::read_to_string(writer.into_inner()).unwrap(),
115				result
116			)
117		}
118
119		#[test]
120		fn no_transform() {
121			test_to_urdf_inertial(
122				Inertial {
123					mass: 0.12,
124					ixx: 1.23,
125					ixy: 2.34,
126					ixz: 3.45,
127					iyy: 4.56,
128					iyz: 5.67,
129					izz: 6.78,
130					..Default::default()
131				},
132				String::from(
133					r#"<inertial><mass value="0.12"/><inertia ixx="1.23" ixy="2.34" ixz="3.45" iyy="4.56" iyz="5.67" izz="6.78"/></inertial>"#,
134				),
135				&URDFConfig::default(),
136			);
137		}
138
139		#[test]
140		fn with_transform() {
141			test_to_urdf_inertial(
142				Inertial {
143					transform: Some(Transform {
144						translation: Some((10.1, 20.2, 30.3)),
145						..Default::default()
146					}),
147					mass: 100.,
148					ixx: 123.,
149					iyy: 456.,
150					izz: 789.,
151					..Default::default()
152				},
153				String::from(
154					r#"<inertial><origin xyz="10.1 20.2 30.3"/><mass value="100"/><inertia ixx="123" ixy="0" ixz="0" iyy="456" iyz="0" izz="789"/></inertial>"#,
155				),
156				&URDFConfig::default(),
157			);
158		}
159	}
160}
robot_description_builder/link/inertial.rs

robot_description_builder/link/
inertial.rs
