geop_ops_assembly/urdf/
xml.rs

1//! A [`Robot`] as URDF's XML ([`Robot::to_urdf`]).
2
3use std::fmt::Write;
4
5use geop_lib_sheet::xml_escape;
6
7use crate::urdf::{Inertial, JointType, Origin, Robot, Visual};
8
9/// `value` as short as reads back exactly: plainly, or in exponent
10/// notation, which every URDF reader parses too, where that is shorter —
11/// as for the tiny masses and inertias of small parts in SI units.
12fn number(value: f64) -> String {
13    let (plain, exponent) = (format!("{value}"), format!("{value:e}"));
14    if exponent.len() < plain.len() {
15        exponent
16    } else {
17        plain
18    }
19}
20
21/// Numbers separated by spaces (see [`number`]).
22fn numbers(values: &[f64]) -> String {
23    let values: Vec<String> = values.iter().map(|&v| number(v)).collect();
24    values.join(" ")
25}
26
27fn origin(out: &mut String, indent: &str, origin: &Origin) {
28    let _ = writeln!(
29        out,
30        r#"{indent}<origin xyz="{}" rpy="{}"/>"#,
31        numbers(&origin.xyz),
32        numbers(&origin.rpy)
33    );
34}
35
36fn inertial(out: &mut String, inertial: &Inertial) {
37    let i = &inertial.inertia;
38    out.push_str("    <inertial>\n");
39    origin(
40        out,
41        "      ",
42        &Origin {
43            xyz: inertial.center,
44            rpy: [0.0; 3],
45        },
46    );
47    let _ = writeln!(out, r#"      <mass value="{}"/>"#, number(inertial.mass));
48    let _ = writeln!(
49        out,
50        r#"      <inertia ixx="{}" ixy="{}" ixz="{}" iyy="{}" iyz="{}" izz="{}"/>"#,
51        number(i[0][0]),
52        number(i[0][1]),
53        number(i[0][2]),
54        number(i[1][1]),
55        number(i[1][2]),
56        number(i[2][2])
57    );
58    out.push_str("    </inertial>\n");
59}
60
61/// A `<visual>` or `<collision>` of the mesh `visual`.
62fn shape(out: &mut String, element: &str, visual: &Visual) {
63    let _ = writeln!(
64        out,
65        r#"    <{element} name="{}">"#,
66        xml_escape(&visual.name)
67    );
68    origin(out, "      ", &visual.origin);
69    let _ = writeln!(
70        out,
71        r#"      <geometry><mesh filename="{}" scale="0.001 0.001 0.001"/></geometry>"#,
72        xml_escape(&visual.mesh)
73    );
74    let _ = writeln!(out, "    </{element}>");
75}
76
77impl Robot {
78    /// The robot as a URDF file.
79    pub fn to_urdf(&self) -> String {
80        let mut out = String::new();
81        out.push_str("<?xml version=\"1.0\"?>\n");
82        out.push_str(
83            "<!-- Exported by geop. Lengths in metres, angles in radians, masses in kilograms; \
84             meshes in millimetres, scaled. geop models no actuators: joint effort and \
85             velocity are 0 — set them for the drives. -->\n",
86        );
87        let _ = writeln!(out, r#"<robot name="{}">"#, xml_escape(&self.name));
88        for link in &self.links {
89            let _ = writeln!(out, r#"  <link name="{}">"#, xml_escape(&link.name));
90            if let Some(i) = &link.inertial {
91                inertial(&mut out, i);
92            }
93            for visual in &link.visuals {
94                shape(&mut out, "visual", visual);
95            }
96            for visual in &link.visuals {
97                shape(&mut out, "collision", visual);
98            }
99            out.push_str("  </link>\n");
100        }
101        for joint in &self.joints {
102            let _ = writeln!(
103                out,
104                r#"  <joint name="{}" type="{}">"#,
105                xml_escape(&joint.name),
106                joint.kind.name()
107            );
108            let _ = writeln!(out, r#"    <parent link="{}"/>"#, xml_escape(&joint.parent));
109            let _ = writeln!(out, r#"    <child link="{}"/>"#, xml_escape(&joint.child));
110            origin(&mut out, "    ", &joint.origin);
111            let _ = writeln!(out, r#"    <axis xyz="{}"/>"#, numbers(&joint.axis));
112            match joint.kind {
113                JointType::Revolute { lower, upper } | JointType::Prismatic { lower, upper } => {
114                    let _ = writeln!(
115                        out,
116                        r#"    <limit lower="{}" upper="{}" effort="0" velocity="0"/>"#,
117                        number(lower),
118                        number(upper)
119                    );
120                }
121                JointType::Continuous => {}
122            }
123            if let Some(mimic) = &joint.mimic {
124                let _ = writeln!(
125                    out,
126                    r#"    <mimic joint="{}" multiplier="{}" offset="0"/>"#,
127                    xml_escape(&mimic.joint),
128                    number(mimic.multiplier)
129                );
130            }
131            out.push_str("  </joint>\n");
132        }
133        out.push_str("</robot>\n");
134        out
135    }
136}
137
138#[cfg(test)]
139mod tests {
140    use super::*;
141    use crate::urdf::{Joint, Link, Mimic};
142
143    /// Names are escaped, limits written only where the joint type has
144    /// them, a mimic where a coupling drives the joint.
145    #[test]
146    fn a_small_robot_is_written() {
147        let at = |x: f64| Origin {
148            xyz: [x, 0.0, 0.0],
149            rpy: [0.0; 3],
150        };
151        let robot = Robot {
152            name: "a<b>".into(),
153            links: vec![
154                Link {
155                    name: "base".into(),
156                    parts: Vec::new(),
157                    inertial: None,
158                    visuals: Vec::new(),
159                },
160                Link {
161                    name: "wheel".into(),
162                    parts: vec!["wheel".into()],
163                    inertial: Some(Inertial {
164                        mass: 0.5,
165                        center: [0.0; 3],
166                        inertia: [[1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 3.0]],
167                    }),
168                    visuals: vec![Visual {
169                        name: "wheel".into(),
170                        origin: at(0.0),
171                        mesh: "meshes/wheel.stl".into(),
172                    }],
173                },
174            ],
175            joints: vec![Joint {
176                name: "add_part(wheel,m1)".into(),
177                kind: JointType::Continuous,
178                parent: "base".into(),
179                child: "wheel".into(),
180                origin: at(0.25),
181                axis: [0.0, 0.0, 1.0],
182                mimic: Some(Mimic {
183                    joint: "motor\"1".into(),
184                    multiplier: -0.5,
185                }),
186            }],
187        };
188        let urdf = robot.to_urdf();
189        assert!(urdf.contains(r#"<robot name="a&lt;b&gt;">"#), "{urdf}");
190        assert!(urdf.contains(r#"<joint name="add_part(wheel,m1)" type="continuous">"#));
191        assert!(!urdf.contains("<limit"));
192        assert!(urdf.contains(r#"<origin xyz="0.25 0 0" rpy="0 0 0"/>"#));
193        assert!(urdf.contains(r#"<mimic joint="motor&quot;1" multiplier="-0.5" offset="0"/>"#));
194        assert!(urdf.contains(r#"<inertia ixx="1" ixy="0" ixz="0" iyy="2" iyz="0" izz="3"/>"#));
195        assert_eq!(
196            urdf.matches("<mesh ").count(),
197            2,
198            "a visual and a collision"
199        );
200        assert!(urdf.ends_with("</robot>\n"));
201    }
202}
geop_ops_assembly/urdf/xml.rs

geop_ops_assembly/urdf/
xml.rs
