URDF export for robot simulators (#719) (#942)

A design with components and joints exports as a URDF robot description (file.export to .urdf, the MCP export tool, solidcraft-cli run --out, File > Export URDF...). It writes one binary STL per component in metres, links with mass, centre of mass and inertia from material density, and maps rigid, revolute, slider, cylindrical, pin-slot, planar and ball joints to URDF joints or short chains. Loops, broken joints and loose parts are reported as warnings. Options: mesh_prefix, effort, velocity. Docs in docs/urdf.md.

crates/engine/src/cmd/file.rs

@@ -8,7 +8,7 @@ use solidcraft_doc::Document;
 use solidcraft_io::Format;
 
 use super::CommandSpec;
-use crate::params::{bad, bool_, str_, string_list};
+use crate::params::{bad, bool_, num, str_, string_list};
 use crate::{EngineError, Result, Session};
 
 pub static COMMANDS: &[CommandSpec] = &[
@@ -45,7 +45,7 @@ pub static COMMANDS: &[CommandSpec] = &[
     CommandSpec::new("file.export", "Export", export)
         .icon("export")
         .noundo()
-        .params("path; format?: stl|stla|obj|step|3mf|3mf-tagged|json (default from extension; 3mf-tagged: 3MF whose triangles carry their B-rep face, at full precision, for meshing and analysis tools; docs/formats/3mf-face-tags.md); bodies?: [names] (JSON exports the complete design)"),
+        .params("path; format?: stl|stla|obj|step|3mf|3mf-tagged|urdf|json (default from extension; 3mf-tagged: 3MF whose triangles carry their B-rep face, at full precision, for meshing and analysis tools; docs/formats/3mf-face-tags.md; urdf: the whole assembly as a robot, with STL meshes written next to it; docs/urdf.md); bodies?: [names] (JSON and URDF export the complete design); urdf only: mesh_prefix? (before each mesh file name, e.g. package://robot/meshes/), effort? (default 1000), velocity? (default 10)"),
     CommandSpec::new("file.save_mesh", "Save As Mesh", save_stl).icon("export").noundo().params("path; bodies?: [names]; ascii?: bool"),
 ];
 
@@ -328,9 +328,70 @@ fn export_to(s: &Session, path: &str, format: Format, bodies: &[String]) -> Resu
 fn export(s: &mut Session, p: &Value) -> Result<Value> {
     let path = path_arg(p, "file.export")?;
     let format = Format::from_name(str_(p, "format").unwrap_or(path))?;
+    if format == Format::Urdf {
+        return export_urdf(s, path, p);
+    }
     export_to(s, path, format, &string_list(p, "bodies"))
 }
 
+/// The assembly as a URDF and its meshes next to it (docs/urdf.md). Every path is checked before
+/// anything is written; the meshes go first and the `.urdf` last.
+fn export_urdf(s: &Session, path: &str, p: &Value) -> Result<Value> {
+    let cmd = "file.export";
+    if !string_list(p, "bodies").is_empty() {
+        return Err(bad(cmd, "urdf exports the whole assembly; omit bodies"));
+    }
+    let positive = |k: &str| match p.get(k) {
+        None | Some(Value::Null) => Ok(None),
+        Some(_) => num(p, k).filter(|x| *x > 0.0).map(Some).ok_or_else(|| bad(cmd, format!("`{k}` must be a positive number"))),
+    };
+    // `io::urdf` refuses a prefix XML cannot hold.
+    let mesh_prefix = str_(p, "mesh_prefix").unwrap_or("").to_string();
+    let options = solidcraft_io::UrdfOptions { mesh_prefix, effort: positive("effort")?, velocity: positive("velocity")? };
+    let file = std::path::Path::new(path);
+    let stem = file.file_stem().map(|x| x.to_string_lossy().to_string()).unwrap_or_else(|| s.doc.name.clone());
+    // Occurrences where they are shown, uncaptured moves included, except those the joint solver
+    // places: it puts them back as soon as a joint moves, so the move must not become part of
+    // their links.
+    let st = s.model.state();
+    let mut held = Vec::new();
+    let doc = if s.pending_moves.is_empty() {
+        std::borrow::Cow::Borrowed(&*s.doc)
+    } else {
+        let placed = solidcraft_doc::joints::solve(&s.doc, &st).world;
+        let mut doc = (*s.doc).clone();
+        for o in &mut doc.occurrences {
+            match s.pending_moves.get(&o.id) {
+                Some(_) if placed.contains_key(&o.id) => held.push(o.name.clone()),
+                Some(t) => o.transform = *t,
+                None => {}
+            }
+        }
+        std::borrow::Cow::Owned(doc)
+    };
+    let mut pkg = solidcraft_io::urdf(&doc, &doc.painted(&st), &stem, &options)?;
+    for name in held {
+        pkg.warnings.push(format!("`{name}` was moved but the move is not captured: exported where its joints place it"));
+    }
+    let dir = file.parent().unwrap_or(std::path::Path::new(""));
+    let mut targets: Vec<(String, &[u8])> = pkg.meshes.iter().map(|(n, b)| (dir.join(n).to_string_lossy().to_string(), b.as_slice())).collect();
+    targets.push((path.to_string(), pkg.xml.as_bytes()));
+    for (t, _) in &targets {
+        solidcraft_io::vfs::check(t).map_err(|e| EngineError::Other(format!("{t}: {e}")))?;
+    }
+    let mut written: Vec<String> = Vec::new();
+    for (t, bytes) in &targets {
+        if let Err(e) = solidcraft_io::vfs::write(t, bytes) {
+            let before = if written.is_empty() { "none".to_string() } else { written.join(", ") };
+            return Err(EngineError::Other(format!("{t}: {e} (this file may be incomplete; written before it: {before})")));
+        }
+        written.push(t.clone());
+    }
+    Ok(
+        json!({"path": path, "format": "Urdf", "files": written, "links": pkg.model.links.len(), "joints": pkg.model.joints.len(), "warnings": pkg.warnings}),
+    )
+}
+
 fn save_stl(s: &mut Session, p: &Value) -> Result<Value> {
     let path = path_arg(p, "file.save_mesh")?;
     let f = if bool_(p, "ascii").unwrap_or(false) { Format::StlAscii } else { Format::StlBinary };
@@ -384,6 +445,10 @@ fn recovery_discard(_s: &mut Session, p: &Value) -> Result<Value> {
     Ok(json!({"discarded": ids}))
 }
 
+#[cfg(test)]
+#[path = "urdf_tests.rs"]
+mod urdf_tests;
+
 #[cfg(test)]
 mod tests {
     use super::*;