urdf_rs/
funcs.rs

Help
1use crate::deserialize::*;
2use crate::errors::*;
3use serde::Serialize;
4
5use std::mem;
6use std::path::Path;
7
8/// sort <link> and <joint> to avoid the [issue](https://github.com/RReverser/serde-xml-rs/issues/5)
9fn sort_link_joint(string: &str) -> Result<String> {
10    let mut e: xml::Element = string.parse().map_err(UrdfError::new)?;
11    let mut links = Vec::new();
12    let mut joints = Vec::new();
13    let mut materials = Vec::new();
14    for c in mem::take(&mut e.children) {
15        if let xml::Xml::ElementNode(xml_elm) = c {
16            if xml_elm.name == "link" {
17                links.push(sort_visual_collision(xml_elm));
18            } else if xml_elm.name == "joint" {
19                joints.push(xml::Xml::ElementNode(xml_elm));
20            } else if xml_elm.name == "material" {
21                materials.push(xml::Xml::ElementNode(xml_elm));
22            }
23        }
24    }
25    let mut new_elm = e;
26    links.extend(joints);
27    links.extend(materials);
28    new_elm.children = links;
29    Ok(format!("{new_elm}"))
30}
31
32fn sort_visual_collision(mut elm: xml::Element) -> xml::Xml {
33    let mut visuals = Vec::new();
34    let mut collisions = Vec::new();
35    for c in mem::take(&mut elm.children) {
36        if let xml::Xml::ElementNode(xml_elm) = c {
37            if xml_elm.name == "visual" || xml_elm.name == "inertial" {
38                visuals.push(xml::Xml::ElementNode(xml_elm));
39            } else if xml_elm.name == "collision" {
40                collisions.push(xml::Xml::ElementNode(xml_elm));
41            }
42        }
43    }
44    let mut new_elm = elm;
45    visuals.extend(collisions);
46    new_elm.children = visuals;
47    xml::Xml::ElementNode(new_elm)
48}
49
50/// Read urdf file and create Robot instance
51///
52/// # Examples
53///
54/// ```
55/// let urdf_robot = urdf_rs::read_file("sample.urdf").unwrap();
56/// let links = urdf_robot.links;
57/// println!("{:?}", links[0].visual[0].origin.xyz);
58/// ```
59pub fn read_file<P: AsRef<Path>>(path: P) -> Result<Robot> {
60    read_from_string(&std::fs::read_to_string(path)?)
61}
62
63/// Read from string instead of file.
64///
65///
66/// # Examples
67///
68/// ```
69/// let s = r#"
70///     <robot name="robot">
71///         <link name="shoulder1">
72///             <inertial>
73///                 <origin xyz="0 0 0.5" rpy="0 0 0"/>
74///                 <mass value="1"/>
75///                 <inertia ixx="100"  ixy="0"  ixz="0" iyy="100" iyz="0" izz="100" />
76///             </inertial>
77///             <visual>
78///                 <origin xyz="0.1 0.2 0.3" rpy="-0.1 -0.2  -0.3" />
79///                 <geometry>
80///                     <box size="1.0 2.0 3.0" />
81///                 </geometry>
82///                 <material name="Cyan">
83///                     <color rgba="0 1.0 1.0 1.0"/>
84///                 </material>
85///             </visual>
86///             <collision>
87///                 <origin xyz="0 0 0" rpy="0 0 0"/>
88///                 <geometry>
89///                     <cylinder radius="1" length="0.5"/>
90///                 </geometry>
91///             </collision>
92///         </link>
93///         <link name="elbow1" />
94///         <link name="wrist1" />
95///         <joint name="shoulder_pitch" type="revolute">
96///             <origin xyz="0.0 0.0 0.1" />
97///             <parent link="shoulder1" />
98///             <child link="elbow1" />
99///             <axis xyz="0 1 -1" />
100///             <limit lower="-1" upper="1.0" effort="0" velocity="1.0"/>
101///         </joint>
102///         <joint name="shoulder_pitch" type="revolute">
103///             <origin xyz="0.0 0.0 0.0" />
104///             <parent link="elbow1" />
105///             <child link="wrist1" />
106///             <axis xyz="0 1 0" />
107///             <limit lower="-2" upper="1.0" effort="0" velocity="1.0"/>
108///         </joint>
109///     </robot>
110///    "#;
111/// let urdf_robot = urdf_rs::read_from_string(s).unwrap();
112/// println!("{:?}", urdf_robot.links[0].visual[0].origin.xyz);
113/// ```
114
115pub fn read_from_string(string: &str) -> Result<Robot> {
116    let sorted_string = sort_link_joint(string)?;
117    serde_xml_rs::from_str(&sorted_string).map_err(UrdfError::new)
118}
119
120pub fn write_to_string(robot: &Robot) -> Result<String> {
121    let mut buffer = String::new();
122    let mut s = quick_xml::se::Serializer::new(&mut buffer);
123    s.indent(' ', 2);
124    robot.serialize(s).map_err(UrdfError::new)?;
125    Ok(buffer)
126}
127
128#[cfg(test)]
129mod tests {
130    use crate::{read_from_string, write_to_string};
131    use crate::{Geometry, JointType, Robot};
132    use assert_approx_eq::assert_approx_eq;
133
134    fn check_robot(robot: &Robot) {
135        assert_eq!(robot.name, "robot");
136
137        // <link>
138        assert_eq!(robot.links.len(), 3);
139        let link = &robot.links[0];
140        assert_eq!(link.name, "shoulder1");
141        let xyz = link.inertial.origin.xyz;
142        assert_approx_eq!(xyz[0], 0.0);
143        assert_approx_eq!(xyz[1], 0.0);
144        assert_approx_eq!(xyz[2], 0.5);
145        let rpy = link.inertial.origin.rpy;
146        assert_approx_eq!(rpy[0], 0.0);
147        assert_approx_eq!(rpy[1], 0.0);
148        assert_approx_eq!(rpy[2], 0.0);
149        assert_approx_eq!(link.inertial.mass.value, 1.0);
150        assert_approx_eq!(link.inertial.inertia.ixx, 100.0);
151        assert_approx_eq!(link.inertial.inertia.ixy, 0.0);
152        assert_approx_eq!(link.inertial.inertia.ixz, 0.0);
153        assert_approx_eq!(link.inertial.inertia.iyy, 100.0);
154        assert_approx_eq!(link.inertial.inertia.ixz, 0.0);
155        assert_approx_eq!(link.inertial.inertia.izz, 100.0);
156
157        assert_eq!(link.visual.len(), 3);
158        let xyz = &link.visual[0].origin.xyz;
159        assert_approx_eq!(xyz[0], 0.1);
160        assert_approx_eq!(xyz[1], 0.2);
161        assert_approx_eq!(xyz[2], 0.3);
162        let rpy = &link.visual[0].origin.rpy;
163        assert_approx_eq!(rpy[0], -0.1);
164        assert_approx_eq!(rpy[1], -0.2);
165        assert_approx_eq!(rpy[2], -0.3);
166
167        // https://github.com/openrr/urdf-rs/issues/94
168        let xyz = &link.visual[1].origin.xyz;
169        assert_approx_eq!(xyz[0], 0.1);
170        assert_approx_eq!(xyz[1], 0.2);
171        assert_approx_eq!(xyz[2], 0.3);
172        let rpy = &link.visual[1].origin.rpy;
173        assert_approx_eq!(rpy[0], -0.1);
174        assert_approx_eq!(rpy[1], -0.2);
175        assert_approx_eq!(rpy[2], -0.3);
176
177        let xyz = &link.visual[2].origin.xyz;
178        assert_approx_eq!(xyz[0], 0.1);
179        assert_approx_eq!(xyz[1], 0.2);
180        assert_approx_eq!(xyz[2], 0.3);
181        let rpy = &link.visual[2].origin.rpy;
182        assert_approx_eq!(rpy[0], -0.1);
183        assert_approx_eq!(rpy[1], -0.2);
184        assert_approx_eq!(rpy[2], -0.3);
185
186        // https://github.com/openrr/urdf-rs/issues/95
187        assert!(link.visual[0].material.is_some());
188        let mat = link.visual[0].material.as_ref().unwrap();
189        assert_eq!(mat.name, "Cyan");
190        let rgba = mat.color.clone().unwrap().rgba;
191        assert_approx_eq!(rgba[0], 0.0);
192        assert_approx_eq!(rgba[1], 1.0);
193        assert_approx_eq!(rgba[2], 1.0);
194        assert_approx_eq!(rgba[3], 1.0);
195
196        match &link.visual[0].geometry {
197            Geometry::Box { size } => {
198                assert_approx_eq!(size[0], 1.0f64);
199                assert_approx_eq!(size[1], 2.0f64);
200                assert_approx_eq!(size[2], 3.0f64);
201            }
202            _ => panic!("geometry error"),
203        }
204        match &link.visual[1].geometry {
205            Geometry::Mesh {
206                ref filename,
207                scale,
208            } => {
209                assert_eq!(filename, "aa.dae");
210                assert!(scale.is_none());
211            }
212            _ => panic!("geometry error"),
213        }
214        match &link.visual[2].geometry {
215            Geometry::Mesh {
216                ref filename,
217                scale,
218            } => {
219                assert_eq!(filename, "bbb.dae");
220                let scale = scale.as_ref().unwrap();
221                assert_approx_eq!(scale[0], 2.0);
222                assert_approx_eq!(scale[1], 3.0);
223                assert_approx_eq!(scale[2], 4.0);
224            }
225            _ => panic!("geometry error"),
226        }
227
228        assert_eq!(link.collision.len(), 1);
229        let xyz = &link.collision[0].origin.xyz;
230        assert_approx_eq!(xyz[0], 0.0);
231        assert_approx_eq!(xyz[1], 0.0);
232        assert_approx_eq!(xyz[2], 0.0);
233        let rpy = &link.collision[0].origin.rpy;
234        assert_approx_eq!(rpy[0], 0.0);
235        assert_approx_eq!(rpy[1], 0.0);
236        assert_approx_eq!(rpy[2], 0.0);
237        match &link.collision[0].geometry {
238            Geometry::Cylinder { radius, length } => {
239                assert_approx_eq!(radius, 1.0);
240                assert_approx_eq!(length, 0.5);
241            }
242            _ => panic!("geometry error"),
243        }
244
245        assert_eq!(robot.links[1].name, "elbow1");
246        assert_eq!(robot.links[2].name, "wrist1");
247
248        // <material>
249        assert_eq!(robot.materials.len(), 1);
250        let mat = &robot.materials[0];
251        assert_eq!(mat.name, "blue");
252        assert!(mat.color.is_some());
253        let rgba = mat.color.clone().unwrap().rgba;
254        assert_approx_eq!(rgba[0], 0.0);
255        assert_approx_eq!(rgba[1], 0.0);
256        assert_approx_eq!(rgba[2], 0.8);
257        assert_approx_eq!(rgba[3], 1.0);
258
259        // <joint>
260        assert_eq!(robot.joints.len(), 2);
261        let joint = &robot.joints[0];
262        assert_eq!(joint.name, "shoulder_pitch");
263        assert_eq!(joint.parent.link, "shoulder1");
264        assert_eq!(joint.child.link, "elbow1");
265        assert_eq!(joint.joint_type, JointType::Revolute);
266        assert_approx_eq!(joint.limit.upper, 1.0);
267        assert_approx_eq!(joint.limit.lower, -1.0);
268        assert_approx_eq!(joint.limit.effort, 0.0);
269        assert_approx_eq!(joint.limit.velocity, 1.0);
270        assert_eq!(joint.calibration.as_ref().unwrap().rising, None);
271        assert_eq!(joint.calibration.as_ref().unwrap().falling, None);
272        assert_approx_eq!(joint.dynamics.as_ref().unwrap().damping, 0.0);
273        assert_approx_eq!(joint.dynamics.as_ref().unwrap().friction, 0.0);
274        assert_eq!(joint.mimic.as_ref().unwrap().joint, "elbow1");
275        assert_approx_eq!(joint.safety_controller.as_ref().unwrap().k_velocity, 10.0);
276        assert!(joint.mimic.as_ref().unwrap().multiplier.is_none());
277        assert!(joint.mimic.as_ref().unwrap().offset.is_none());
278        let xyz = &joint.axis.xyz;
279        assert_approx_eq!(xyz[0], 0.0);
280        assert_approx_eq!(xyz[1], 1.0);
281        assert_approx_eq!(xyz[2], -1.0);
282
283        let joint = &robot.joints[1];
284        assert_eq!(joint.name, "shoulder_pitch");
285        assert_eq!(joint.parent.link, "elbow1");
286        assert_eq!(joint.child.link, "wrist1");
287        assert_eq!(joint.joint_type, JointType::Revolute);
288        assert_approx_eq!(joint.limit.upper, 1.0);
289        assert_approx_eq!(joint.limit.lower, -2.0);
290        assert_approx_eq!(joint.limit.effort, 0.0);
291        assert_approx_eq!(joint.limit.velocity, 1.0);
292        assert_approx_eq!(joint.dynamics.as_ref().unwrap().damping, 10.0);
293        assert_approx_eq!(joint.dynamics.as_ref().unwrap().friction, 1.0);
294        assert_eq!(joint.mimic.as_ref().unwrap().joint, "shoulder1");
295        assert_approx_eq!(joint.mimic.as_ref().unwrap().multiplier.unwrap(), 5.0);
296        assert_approx_eq!(joint.mimic.as_ref().unwrap().offset.unwrap(), 1.0);
297        assert_approx_eq!(joint.safety_controller.as_ref().unwrap().k_position, 10.0);
298        assert_approx_eq!(joint.safety_controller.as_ref().unwrap().k_velocity, 1.0);
299        assert_approx_eq!(
300            joint.safety_controller.as_ref().unwrap().soft_lower_limit,
301            -0.5
302        );
303        assert_approx_eq!(
304            joint.safety_controller.as_ref().unwrap().soft_upper_limit,
305            1.0
306        );
307        let xyz = &joint.axis.xyz;
308        assert_approx_eq!(xyz[0], 0.0);
309        assert_approx_eq!(xyz[1], 1.0);
310        assert_approx_eq!(xyz[2], 0.0);
311    }
312
313    #[test]
314    fn deserialization() {
315        let s = r#"
316            <robot name="robot" xmlns="http://www.ros.org">
317                <material name="blue">
318                  <color rgba="0.0 0.0 0.8 1.0"/>
319                </material>
320
321                <link name="shoulder1">
322                    <inertial>
323                        <origin xyz="0 0 0.5" rpy="0 0 0"/>
324                        <mass value="1"/>
325                        <inertia ixx="100"  ixy="0"  ixz="0" iyy="100" iyz="0" izz="100" />
326                    </inertial>
327                    <visual>
328                        <origin xyz="0.1 0.2 0.3" rpy="-0.1 -0.2  -0.3" />
329                        <geometry>
330                            <box size="1.0 2.0 3.0" />
331                        </geometry>
332                        <material name="Cyan">
333                            <color rgba="0 1.0 1.0 1.0"/>
334                        </material>
335                    </visual>
336                    <visual>
337                        <geometry>
338                            <mesh filename="aa.dae" />
339                        </geometry>
340                        <origin xyz="0.1 0.2 0.3" rpy="-0.1 -0.2  -0.3" />
341                    </visual>
342                    <collision>
343                        <origin xyz="0 0 0" rpy="0 0 0"/>
344                        <geometry>
345                            <cylinder radius="1" length="0.5"/>
346                        </geometry>
347                    </collision>
348                    <visual>
349                        <origin xyz="0.1 0.2 0.3" rpy="-0.1 -0.2  -0.3" />
350                        <geometry>
351                            <mesh filename="bbb.dae" scale="2.0 3.0 4.0" />
352                        </geometry>
353                    </visual>
354                </link>
355                <joint name="shoulder_pitch" type="revolute">
356                    <origin xyz="0.0 0.0 0.1" />
357                    <parent link="shoulder1" />
358                    <child link="elbow1" />
359                    <axis xyz="0 1 -1" />
360                    <calibration />
361                    <dynamics />
362                    <mimic joint="elbow1" />
363                    <safety_controller k_velocity="10" />
364                    <limit lower="-1" upper="1.0" effort="0" velocity="1.0"/>
365                </joint>
366                <link name="elbow1" />
367                <link name="wrist1" />
368                <joint name="shoulder_pitch" type="revolute">
369                    <origin xyz="0.0 0.0 0.0" />
370                    <parent link="elbow1" />
371                    <child link="wrist1" />
372                    <axis xyz=" 0 1 0 " />
373                    <calibration falling="1" rising="1" />
374                    <dynamics damping="10.0" friction="1" />
375                    <mimic joint="shoulder1" offset="1" multiplier="5" />
376                    <safety_controller k_position="10" k_velocity="1" soft_lower_limit="-0.5" soft_upper_limit="1" />
377                    <limit lower=" -2" upper="1.0 " effort="0 " velocity=" 1.0 "/>
378                </joint>
379            </robot>
380        "#;
381        let robot = read_from_string(s).unwrap();
382        check_robot(&robot);
383
384        // Loopback test
385        let s = write_to_string(&robot).unwrap();
386        assert!(!s.contains("Robot"), "{s}"); // https://github.com/openrr/urdf-rs/issues/80
387        let robot = read_from_string(&s).unwrap();
388        check_robot(&robot);
389    }
390}
urdf_rs/funcs.rs

urdf_rs/
funcs.rs
