From 17db109c5d07cad90f4203194882c42ded5e0e9a Mon Sep 17 00:00:00 2001 From: Mateusz Sych <151755914+SychPL@users.noreply.github.com> Date: Wed, 14 Oct 2026 03:52:30 +0200 Subject: [PATCH] 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. --- README.md | 3 +- ROADMAP.md | 2 +- crates/doc/src/assembly.rs | 11 +- crates/doc/src/joints.rs | 3 +- crates/doc/src/lib.rs | 5 +- crates/engine/src/cmd/file.rs | 69 ++- crates/engine/src/cmd/urdf_tests.rs | 647 ++++++++++++++++++++ crates/geom/src/lib.rs | 2 +- crates/geom/src/mesh.rs | 66 ++ crates/geom/src/tests.rs | 96 +++ crates/io/src/lib.rs | 11 +- crates/io/src/urdf.rs | 916 ++++++++++++++++++++++++++++ crates/mcp/src/tests.rs | 18 + crates/mcp/src/tools.rs | 13 +- crates/ui-egui/src/toolbar.rs | 23 +- docs/book/src/guide/files.md | 6 + docs/mcp.md | 2 +- docs/urdf.md | 115 ++++ skills/solidcraft-modeling/SKILL.md | 2 +- 19 files changed, 1993 insertions(+), 17 deletions(-) create mode 100644 crates/engine/src/cmd/urdf_tests.rs create mode 100644 crates/io/src/urdf.rs create mode 100644 docs/urdf.md diff --git a/README.md b/README.md index 937f157e..815cc4d3 100644 --- a/README.md +++ b/README.md @@ -125,7 +125,8 @@ rebuild with no errors, replayed here at 2× speed. provisional picture from warm geometry caches while rebuilding before editing; caches never replace the parametric design. Export to STEP (AP242 with body names, colours and components as assemblies; revolved faces as native planes, cylinders, cones, spheres and tori), - IGES 5.3 (manifold solid B-reps with names and colours), 3MF, STL (binary/ASCII) and OBJ; + IGES 5.3 (manifold solid B-reps with names and colours), 3MF, STL (binary/ASCII) and OBJ, and + assemblies with joints as URDF for robot simulators (docs/urdf.md); 3MF, STL and OBJ import as mesh bodies (they render, measure, move and export; solid features need B-rep bodies). Convert Mesh reconstructs supported closed constant-section parts as parametric solids with planar and circular cylindrical faces; diff --git a/ROADMAP.md b/ROADMAP.md index 30e4aff8..abc28802 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -52,7 +52,7 @@ PCB / Package, Render, Animation, Simulation, Manufacture/CAM, Drawings. | M6 | UI depth: selection feedback and origin (done), timeline editing (done: hover and selection highlight modifier faces, including fillets and chamfers; edit feature, history marker drag, dependency-checked single and multi-item reorder, suppress, rename, delete with dependents, re-resolved references; redefining selections refreshes persistent edge/face references while retaining feature identity and dimension names; optional compact named left feature history with rollback bar, multi-selection and persistent folders above the parts browser), right-docked dialogs (done), live previews for every feature dialog (done), keyboard-first commands with on-canvas value boxes and sketch inline dimensions (done), drag manipulators with automatic join/cut (done: extrude incl. two sides, fillet, shell, hole depth, move, pattern spacing, revolve rotator; taper handle open), dark theme (done), right-click menu with Repeat (done), measure tool (done), section analysis (done), selection filter (done), drag-solve of sketch points (done); appearance controls now preserve body/face/component targeting, named transparent looks, reuse/search and override removal; preferences | in progress | 6 | | M7 | Sweep, loft (incl. to a point), rib and web (to next / depth), emboss (planar faces and exact radial emboss/deboss of bounded orthogonal profiles on complete exterior cylinders/sleeves), thread (cosmetic, and modelled M3–M20 outside and inside: the ISO profile screwed in as a faceted cut), coil (smooth B-spline helical walls), pipe, pattern on path, patterns of bodies and components, replace face (parallel), align, remove, feature copy/paste: done. Unique eligible cylindrical targets are inferred for new Emboss features. Left: general curved-face emboss and exact (unfaceted) modelled threads | in progress | 12 | | M8 | Surface workspace (#772). Done: Patch (sketch regions; edge loops filled G0), Stitch, Thicken (planes and cylinders), Extend (straight edges of planar surfaces), Trim (by a plane or a face), Delete Face (healing), Boundary Fill, zebra/draft/curvature-map analysis. Proposed, in order: surface Extrude/Revolve/Sweep/Loft from open and closed curves (#757), Offset surface (#758), Unstitch and Reverse Normal (#766), Delete Face without healing (#768); Extend curved faces and edges (#760), mutual trim and trim by projected curves (#767), Untrim (#761), Thicken every surface kind (#765); Ruled (#762), Boundary surface and curve networks with G0/G1/G2 (#763), Patch with G1/G2 and general loops (#759), Blend surface (#764), sweep rails, twist and scale (#771, loft rails #201), fillet between surface bodies (#769); continuity analysis, surface curvature combs and isocurves (#770) | deferred | — | -| M9 | Components and assemblies: New Component / Create Components from Bodies, active component, per-component origin/bodies/sketches/timeline entries, occurrences with transforms, Copy/Paste vs Paste New, nested Browser tree with visibility and isolate; Ground, Move/Copy of occurrences, joints (rigid, revolute, slider, cylindrical, pin-slot, planar, ball) by joint origins with limits and Drive Joints, As-built Joint, Rigid Group, Contact Sets (driving and motion studies stop where bodies in a set would pass through each other), Motion Study (joint values keyed along a timeline, playback, export of occurrence positions as JSON/CSV), Exploded views (automatic or by hand, shown without changing placements); interference, per-component physical properties; STEP with assembly structure. Physical properties now measure material-aware component/occurrence placements, descendants and repeated instances, including pending moves and body reassignment offsets; contact geometry and automatic exploded centres respect those offsets. Groundwork done: component tree, occurrences with transforms (instances, Paste New), world placement, active component as authoring frame, ground, Capture/Revert Position, moving bodies and sketches. UI: ASSEMBLE panel dialogs for New Component, Joint (snaps on faces, edges, circles and vertices; animated motion preview; offsets, flip, limits), As-built Joint, Joint Origin, Rigid Group, Drive Joints (live slider), Motion Link and Interference (overlaps shown red); joints in the timeline and the Browser | in progress | 32 | +| M9 | Components and assemblies: New Component / Create Components from Bodies, active component, per-component origin/bodies/sketches/timeline entries, occurrences with transforms, Copy/Paste vs Paste New, nested Browser tree with visibility and isolate; Ground, Move/Copy of occurrences, joints (rigid, revolute, slider, cylindrical, pin-slot, planar, ball) by joint origins with limits and Drive Joints, As-built Joint, Rigid Group, Contact Sets (driving and motion studies stop where bodies in a set would pass through each other), Motion Study (joint values keyed along a timeline, playback, export of occurrence positions as JSON/CSV), Exploded views (automatic or by hand, shown without changing placements); interference, per-component physical properties; STEP with assembly structure; URDF export for robot simulators (links with meshes, mass and inertia; joint chains whose values match Drive Joints; docs/urdf.md). Physical properties now measure material-aware component/occurrence placements, descendants and repeated instances, including pending moves and body reassignment offsets; contact geometry and automatic exploded centres respect those offsets. Groundwork done: component tree, occurrences with transforms (instances, Paste New), world placement, active component as authoring frame, ground, Capture/Revert Position, moving bodies and sketches. UI: ASSEMBLE panel dialogs for New Component, Joint (snaps on faces, edges, circles and vertices; animated motion preview; offsets, flip, limits), As-built Joint, Joint Origin, Rigid Group, Drive Joints (live slider), Motion Link and Interference (overlaps shown red); joints in the timeline and the Browser | in progress | 32 | | M10 | Sheet metal: flange (base, edge, contour), hem, unfold/refold, convert, flat pattern + DXF, rules: done. PLASTIC enclosure subset: Boss, Rib, Web, Lip/Groove, Snap Fit, Rest, Plastic Rules (library, edit, assign; rule draft and clearance as feature defaults): done, tested against hand-computed geometry. UI: SHEET METAL and PLASTIC tabs in Fusion's layout with dialogs (live preview, Edit Feature, pre-selection) for Flange, Hem, Unfold/Refold, Convert, Sheet Metal Rules, Create Flat Pattern (flat view with bend lines, bend table CSV/JSON and DXF export with exact circle/arc/line cutouts), Boss, Lip/Groove, Snap Fit, Rest, Manage and Assign Plastic Rules; Fold (bend along a sketch line); flanges meeting at a corner closed at any angle with the rule's gap; edge flanges with their own angle and height per edge in one feature; corner reliefs (notch, round, rectangular, tear) chosen by the rule; Convert to Sheet Metal from a solid or shelled body by its bend edges (flat faces, sharp bends) | in progress | 36 | | M11 | 2D drawings | deferred | — | | M12 | Release: cross-platform packages, verification and deliberate publication implemented; original current-UI demo refreshed. Trusted distribution signing and broader performance work remain | in progress | 16 | diff --git a/crates/doc/src/assembly.rs b/crates/doc/src/assembly.rs index 7c01ea8a..79bf49e4 100644 --- a/crates/doc/src/assembly.rs +++ b/crates/doc/src/assembly.rs @@ -124,6 +124,10 @@ fn identity() -> Mat { IDENTITY } +/// How deep and how many placements [`Document::placements`] walks. +pub const MAX_PLACEMENT_DEPTH: usize = 64; +pub const MAX_PLACEMENTS: usize = 100_000; + fn map_plane(m: &Mat, p: &Plane) -> Plane { let (o, x, y) = (apply_point(m, p.origin), apply_vector(m, p.x), apply_vector(m, p.y)); Plane::new(o, x, y).unwrap_or(*p) @@ -375,13 +379,18 @@ impl Document { } /// Every placement of each component: (component, occurrence path ids, world transform). + /// A branch stops past [`MAX_PLACEMENT_DEPTH`] levels, the walk past [`MAX_PLACEMENTS`] placements. pub fn placements(&self) -> Vec<(u64, Vec, Mat)> { let mut out = vec![(0, Vec::new(), IDENTITY)]; let mut stack: Vec<(u64, Vec, Mat)> = vec![(0, Vec::new(), IDENTITY)]; while let Some((comp, path, m)) = stack.pop() { - if path.len() > 64 || out.len() > 100_000 { + if out.len() > MAX_PLACEMENTS { break; } + // Only this branch stops past the depth limit; the others are still walked. + if path.len() > MAX_PLACEMENT_DEPTH { + continue; + } for o in self.occurrences.iter().filter(|o| o.parent == comp) { let mut p = path.clone(); p.push(o.id); diff --git a/crates/doc/src/joints.rs b/crates/doc/src/joints.rs index f0f56044..034bcd35 100644 --- a/crates/doc/src/joints.rs +++ b/crates/doc/src/joints.rs @@ -17,7 +17,8 @@ use solidcraft_geom::Vec3; use crate::{DocError, Document, IDENTITY, Mat, ModelState, Result, apply_point, apply_vector, mat_inverse, mat_mul}; -const MAX_JOINTS: usize = 10_000; +/// Joints the solver (and the URDF export) reads at most. +pub const MAX_JOINTS: usize = 10_000; /// Where a joint origin sits on a component's geometry (component coordinates). #[derive(Clone, Debug, PartialEq, Serialize, Deserialize)] diff --git a/crates/doc/src/lib.rs b/crates/doc/src/lib.rs index c188a9b1..016ccfd0 100644 --- a/crates/doc/src/lib.rs +++ b/crates/doc/src/lib.rs @@ -40,7 +40,10 @@ mod project3d; pub use project3d::iso_grid; pub mod sheet; -pub use assembly::{IDENTITY, Mat, Occurrence, apply_point, apply_vector, is_identity, map_point_expr, mat_inverse, mat_mul, rigid}; +pub use assembly::{ + IDENTITY, MAX_PLACEMENT_DEPTH, MAX_PLACEMENTS, Mat, Occurrence, apply_point, apply_vector, is_identity, map_point_expr, mat_inverse, mat_mul, + rigid, +}; pub use document::*; pub use eval::{ EdgeRef, FeatureResult, Model, ModelBody, ModelState, RevolveExtent, SolvedSketch, ThreadInfo, curve_pick_error, edge_names_for, edge_refs, diff --git a/crates/engine/src/cmd/file.rs b/crates/engine/src/cmd/file.rs index e09af4bd..3b77ceda 100644 --- a/crates/engine/src/cmd/file.rs +++ b/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 { 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 { + 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 = 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 { 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 { Ok(json!({"discarded": ids})) } +#[cfg(test)] +#[path = "urdf_tests.rs"] +mod urdf_tests; + #[cfg(test)] mod tests { use super::*; diff --git a/crates/engine/src/cmd/urdf_tests.rs b/crates/engine/src/cmd/urdf_tests.rs new file mode 100644 index 00000000..ff136c53 --- /dev/null +++ b/crates/engine/src/cmd/urdf_tests.rs @@ -0,0 +1,647 @@ +//! URDF export (#719, docs/urdf.md): files, names, limits, mass and forward kinematics against +//! the joint solver. + +use std::path::PathBuf; + +use serde_json::{Value, json}; +use solidcraft_doc::{Mat, joints, mat_mul}; +use solidcraft_io::urdf::{Step, UrdfModel, UrdfOptions, UrdfPackage}; + +use crate::Session; + +fn run(s: &mut Session, id: &str, p: Value) -> Value { + match s.execute(id, &p) { + Ok(v) => v, + Err(e) => panic!("{id} {p}: {e}"), + } +} + +fn occ(s: &Session, component: &str) -> u64 { + let c = s.doc.find_component(component).unwrap_or(0); + s.doc.occurrence_of(c).map(|o| o.id).unwrap_or(0) +} + +fn ground(s: &mut Session, component: &str, grounded: bool) { + let o = occ(s, component); + run(s, "occurrence.ground", json!({"occurrence": o, "grounded": grounded})); +} + +/// A hinge leaf: a 40 x 30 x 3 plate and an r4 knuckle along X at y = 33, z = 4. +fn leaf(s: &mut Session, name: &str) { + run(s, "component.activate", json!({"component": "root"})); + run(s, "component.create", json!({"name": name})); + run(s, "solid.box", json!({"length": 40, "width": 30, "height": 3, "corner": [0, 0, 0]})); + run(s, "solid.cylinder", json!({"base": [0, 33, 4], "axis": [1, 0, 0], "radius": 4, "height": 40, "operation": "new"})); +} + +/// LeafA (grounded) and LeafB on a revolute joint "Hinge" without limits. +fn hinge() -> Session { + let mut s = Session::default(); + leaf(&mut s, "LeafA"); + leaf(&mut s, "LeafB"); + ground(&mut s, "LeafA", true); + run( + &mut s, + "joint.create", + json!({"type": "revolute", "a": {"occurrence": "LeafA", "circle": [1, 37, 4]}, "b": {"occurrence": "LeafB", "circle": [1, 37, 4]}, "flip": true, "name": "Hinge"}), + ); + s +} + +/// 10 mm cubes in components A (grounded, at the origin) and B (at x = 20). +fn two_cubes() -> Session { + let mut s = Session::default(); + for (name, x) in [("A", 0), ("B", 20)] { + run(&mut s, "component.activate", json!({"component": "root"})); + run(&mut s, "component.create", json!({"name": name})); + run(&mut s, "solid.box", json!({"length": 10, "width": 10, "height": 10, "corner": [x, 0, 0]})); + } + ground(&mut s, "A", true); + s +} + +/// B's bottom face on A's top face. +fn joint(s: &mut Session, kind: &str, name: &str, limits: Value) { + run(s, "joint.create", json!({"type": kind, "a": {"face": [5, 5, 10]}, "b": {"face": [25, 5, 0]}, "limits": limits, "name": name})); +} + +fn package(s: &Session) -> UrdfPackage { + match solidcraft_io::urdf(&s.doc, &s.doc.painted(&s.model.state()), "t", &UrdfOptions::default()) { + Ok(p) => p, + Err(e) => panic!("urdf: {e}"), + } +} + +fn close(a: &Mat, b: &Mat) -> bool { + a.iter().flatten().zip(b.iter().flatten()).all(|(x, y)| (x - y).abs() < 1e-9) +} + +/// Every placement's link, composed at the design's current joint values, sits where SolidCraft +/// places it. +fn assert_fk(m: &UrdfModel, s: &Session, what: &str) { + let vals = joints::joint_values(&s.doc.assembly); + let frames = m.link_frames(|j| j.source.and_then(|(id, k)| vals.get(&id)?.get(k).copied()).unwrap_or(0.0)); + let placements = s.doc.placements(); + let mut seen = 0; + for (l, f) in m.links.iter().zip(&frames) { + if l.path.is_empty() { + continue; + } + let Some((_, _, want)) = placements.iter().find(|p| p.1 == l.path) else { panic!("{what}: {} has no placement", l.name) }; + let got = mat_mul(f, &l.mesh_origin); + assert!(close(&got, want), "{what}: {} at {got:?}, SolidCraft has {want:?}", l.name); + seen += 1; + } + assert_eq!(seen, placements.len() - 1, "{what}: a link per placement"); +} + +fn temp(tag: &str) -> PathBuf { + let d = std::env::temp_dir().join(format!("solidcraft-urdf-{tag}-{}", std::process::id())); + let _ = std::fs::remove_dir_all(&d); + let _ = std::fs::create_dir_all(&d); + d +} + +/// Volume (m³) of an STL whose triangles share their corners (`None`: not closed). +fn stl_volume(bytes: &[u8]) -> Option { + let mut index: std::collections::HashMap<[u32; 3], u32> = std::collections::HashMap::new(); + let mut mesh = solidcraft_geom::Mesh::default(); + for t in solidcraft_io::read_stl(bytes).ok()? { + let mut tri = [0u32; 3]; + for (k, p) in t.iter().enumerate() { + let n = mesh.positions.len() as u32; + tri[k] = *index.entry(p.map(f32::to_bits)).or_insert_with(|| { + mesh.positions.push(solidcraft_geom::Vec3::new(p[0] as f64, p[1] as f64, p[2] as f64)); + n + }); + } + mesh.triangles.push(tri); + } + mesh.mass_properties().map(|m| m.volume) +} + +fn names(m: &UrdfModel) -> (Vec<&str>, Vec<&str>) { + (m.links.iter().map(|l| l.name.as_str()).collect(), m.joints.iter().map(|j| j.name.as_str()).collect()) +} + +fn has_warning(p: &UrdfPackage, needle: &str) -> bool { + p.warnings.iter().any(|w| w.contains(needle)) +} + +#[test] +fn urdf_of_a_hinge() { + let mut s = hinge(); + run(&mut s, "joint.drive", json!({"joint": "Hinge", "value": 90})); + let d = temp("hinge"); + let path = d.join("hinge.urdf"); + let r = run(&mut s, "file.export", json!({"path": path.to_string_lossy()})); + assert_eq!(r["format"], "Urdf", "{r}"); + assert_eq!(r["links"], 3, "{r}"); + let mut files: Vec = std::fs::read_dir(&d).unwrap().map(|e| e.unwrap().file_name().to_string_lossy().to_string()).collect(); + files.sort(); + assert_eq!(files, ["hinge-LeafA.stl", "hinge-LeafB.stl", "hinge.urdf"]); + let xml = std::fs::read_to_string(&path).unwrap(); + for want in [ + r#""#, + r#""#, + r#""#, + r#""#, + r#""#, + r#""#, + ] { + assert!(xml.contains(want), "{want} missing from\n{xml}"); + } + // Every mesh is a closed solid in metres. + for f in ["hinge-LeafA.stl", "hinge-LeafB.stl"] { + let v = stl_volume(&std::fs::read(d.join(f)).unwrap()); + assert!(v.is_some_and(|v| v > 0.0 && v < 1e-5), "{f}: {v:?}"); + } + let _ = std::fs::remove_dir_all(&d); +} + +#[test] +fn urdf_forward_kinematics_match_the_assembly() { + let cases = [ + ("revolute", json!([30]), json!([-50])), + ("slider", json!([5]), json!([-3])), + ("cylindrical", json!([30, 4]), json!([-70, -2])), + ("pin_slot", json!([30, 3]), json!([100, -6])), + ("planar", json!([2, 3, 40]), json!([-4, 7, -20])), + ("ball", json!([20, 30, 40]), json!([-60, 10, 75])), + ]; + for (kind, first, second) in cases { + // The tree reaches the child on B, then (B grounded) on A. + for b_grounded in [false, true] { + let mut s = two_cubes(); + if b_grounded { + ground(&mut s, "A", false); + ground(&mut s, "B", true); + } + joint(&mut s, kind, "J", Value::Null); + run(&mut s, "joint.drive", json!({"joint": "J", "values": first})); + let p = package(&s); + let what = format!("{kind} (B grounded: {b_grounded})"); + assert_fk(&p.model, &s, &what); + // The same URDF follows SolidCraft to other values. + run(&mut s, "joint.drive", json!({"joint": "J", "values": second})); + assert_fk(&p.model, &s, &what); + let moving = p.model.joints.iter().filter(|j| j.step != Step::Fixed).count(); + assert_eq!(moving, joints::JointKind::parse(kind).map(|k| k.dofs().len()).unwrap_or(0), "{what}"); + } + } +} + +#[test] +fn urdf_limits_convert_units() { + let mut s = two_cubes(); + joint(&mut s, "slider", "Rail", json!([[10, 0]])); + let x = package(&s).xml; + assert!(x.contains(r#""#), "{x}"); + assert!(x.contains(r#""#), "{x}"); + + let mut s = two_cubes(); + joint(&mut s, "revolute", "Turn", json!([[-30, 45]])); + let p = package(&s); + let Some(j) = p.model.joints.iter().find(|j| j.name == "Turn") else { panic!("{:?}", names(&p.model)) }; + let Some((lo, hi)) = j.limit else { panic!("no limit") }; + assert!((lo + 30f64.to_radians()).abs() < 1e-12 && (hi - 45f64.to_radians()).abs() < 1e-12); + assert!(p.xml.contains(r#"type="revolute">"#), "{}", p.xml); + assert!(has_warning(&p, "effort and velocity"), "{:?}", p.warnings); + + // A prismatic step needs a limit: an unlimited slide gets ±1000 m and a warning. + let mut s = two_cubes(); + joint(&mut s, "slider", "Free", Value::Null); + let p = package(&s); + assert!(p.xml.contains(r#"lower="-1000" upper="1000""#), "{}", p.xml); + assert!(has_warning(&p, "Free"), "{:?}", p.warnings); +} + +/// A limit that is not finite (a hand-edited file) counts as none, never as `0`. +#[test] +fn urdf_treats_infinite_limits_as_none() { + let mut s = two_cubes(); + joint(&mut s, "revolute", "Turn", Value::Null); + let mut doc = (*s.doc).clone(); + for j in &mut doc.assembly.joints { + j.limits = vec![Some((f64::NEG_INFINITY, f64::INFINITY))]; + } + let p = solidcraft_io::urdf(&doc, &s.doc.painted(&s.model.state()), "t", &UrdfOptions::default()).unwrap(); + assert!(p.xml.contains(r#""#), "{}", p.xml); +} + +#[test] +fn urdf_effort_and_velocity_are_written_when_given() { + let mut s = two_cubes(); + joint(&mut s, "revolute", "Turn", Value::Null); + let d = temp("effort"); + let path = d.join("e.urdf"); + let r = run(&mut s, "file.export", json!({"path": path.to_string_lossy(), "effort": 12.5, "velocity": 0.5})); + assert!(r["warnings"].as_array().unwrap().iter().all(|w| !w.as_str().unwrap_or("").contains("effort")), "{r}"); + let xml = std::fs::read_to_string(&path).unwrap(); + assert!(xml.contains(r#""#), "{xml}"); + for bad in [json!(0), json!(-1), json!("x")] { + assert!(s.execute("file.export", &json!({"path": path.to_string_lossy(), "effort": bad})).is_err(), "{bad}"); + } + let _ = std::fs::remove_dir_all(&d); +} + +/// A moved occurrence whose position is not captured yet exports where it is shown. +#[test] +fn urdf_exports_uncaptured_moves() { + let mut s = two_cubes(); + let b = occ(&s, "B"); + run(&mut s, "occurrence.move", json!({"occurrence": b, "translate": [0, 0, 30], "capture": false})); + assert!(!s.pending_moves.is_empty()); + let d = temp("pending"); + let path = d.join("m.urdf"); + run(&mut s, "file.export", json!({"path": path.to_string_lossy()})); + let xml = std::fs::read_to_string(&path).unwrap(); + // B hangs on base_link where it is shown: 30 mm up. + assert!(xml.contains(r#""#), "{xml}"); + let _ = std::fs::remove_dir_all(&d); +} + +/// A jointed occurrence moved without capturing exports where its joint puts it: its joint +/// would put it back as soon as it moves, so the move must not become part of its link. +#[test] +fn urdf_ignores_uncaptured_moves_of_jointed_occurrences() { + let mut s = two_cubes(); + joint(&mut s, "revolute", "Turn", Value::Null); + let d = temp("pending-jointed"); + let path = d.join("j.urdf"); + run(&mut s, "file.export", json!({"path": path.to_string_lossy()})); + let solved = std::fs::read_to_string(&path).unwrap(); + let b = occ(&s, "B"); + run(&mut s, "occurrence.move", json!({"occurrence": b, "translate": [0, 0, 30], "capture": false})); + let r = run(&mut s, "file.export", json!({"path": path.to_string_lossy()})); + assert_eq!(std::fs::read_to_string(&path).unwrap(), solved); + assert!(r["warnings"].as_array().unwrap().iter().any(|w| w.as_str().unwrap_or("").contains("not captured")), "{r}"); + let _ = std::fs::remove_dir_all(&d); +} + +/// A joint whose origins no longer resolve does not place its occurrence, so that occurrence's +/// uncaptured move counts. +#[test] +fn urdf_keeps_uncaptured_moves_when_the_joint_is_broken() { + let mut s = two_cubes(); + joint(&mut s, "revolute", "Turn", Value::Null); + let Some(f) = s.doc.features.iter().rev().find(|f| matches!(f.kind, solidcraft_doc::FeatureKind::Box { .. })).map(|f| f.id) else { + panic!("box") + }; + run(&mut s, "timeline.suppress", json!({"feature": f, "suppressed": true})); + let d = temp("pending-broken"); + let path = d.join("k.urdf"); + run(&mut s, "file.export", json!({"path": path.to_string_lossy()})); + let before = std::fs::read_to_string(&path).unwrap(); + let b = occ(&s, "B"); + run(&mut s, "occurrence.move", json!({"occurrence": b, "translate": [0, 0, 30], "capture": false})); + let r = run(&mut s, "file.export", json!({"path": path.to_string_lossy()})); + assert_ne!(std::fs::read_to_string(&path).unwrap(), before, "B exports where it is shown"); + assert!(r["warnings"].as_array().unwrap().iter().all(|w| !w.as_str().unwrap_or("").contains("not captured")), "{r}"); + let _ = std::fs::remove_dir_all(&d); +} + +/// A group the solver leaves where it is (grounded A elsewhere, C and D only joined to each +/// other) keeps an uncaptured move of C: no joint puts C back. +#[test] +fn urdf_keeps_uncaptured_moves_the_solver_does_not_undo() { + let mut s = two_cubes(); + for (name, x) in [("C", 40), ("D", 60)] { + run(&mut s, "component.activate", json!({"component": "root"})); + run(&mut s, "component.create", json!({"name": name})); + run(&mut s, "solid.box", json!({"length": 10, "width": 10, "height": 10, "corner": [x, 0, 0]})); + } + // A and B are joined, so the solver walks from grounded A and never anchors C. + joint(&mut s, "revolute", "Turn", Value::Null); + run(&mut s, "joint.create", json!({"type": "revolute", "a": {"face": [45, 5, 10]}, "b": {"face": [65, 5, 0]}, "name": "Pair"})); + let d = temp("pending-floating"); + let path = d.join("f.urdf"); + run(&mut s, "file.export", json!({"path": path.to_string_lossy()})); + let before = std::fs::read_to_string(&path).unwrap(); + let c = occ(&s, "C"); + run(&mut s, "occurrence.move", json!({"occurrence": c, "translate": [0, 0, 30], "capture": false})); + let r = run(&mut s, "file.export", json!({"path": path.to_string_lossy()})); + assert_ne!(std::fs::read_to_string(&path).unwrap(), before, "C exports where it is shown"); + assert!(r["warnings"].as_array().unwrap().iter().all(|w| !w.as_str().unwrap_or("").contains("not captured")), "{r}"); + let _ = std::fs::remove_dir_all(&d); +} + +#[test] +fn urdf_mesh_prefix_is_written_and_checked() { + let mut s = hinge(); + let d = temp("prefix"); + let path = d.join("p.urdf"); + run(&mut s, "file.export", json!({"path": path.to_string_lossy(), "mesh_prefix": "package://robot/meshes/"})); + let xml = std::fs::read_to_string(&path).unwrap(); + assert!(xml.contains(r#""#), "{xml}"); + // XML cannot hold a control character, even escaped. + let e = s.execute("file.export", &json!({"path": d.join("q.urdf").to_string_lossy(), "mesh_prefix": "a\u{1}b"})); + assert!(e.is_err_and(|e| e.to_string().contains("mesh_prefix"))); + assert!(!d.join("q.urdf").exists()); + let _ = std::fs::remove_dir_all(&d); +} + +/// The visual's colour keeps the body's opacity, and a too long prefix says so. +#[test] +fn urdf_colour_keeps_opacity() { + let mut s = Session::default(); + run(&mut s, "component.create", json!({"name": "Glass"})); + run(&mut s, "solid.box", json!({"length": 10, "width": 10, "height": 10})); + let body = s.model.state().bodies.first().map(|b| b.name.clone()).unwrap_or_default(); + run(&mut s, "appearance.assign", json!({"bodies": [body], "color": [255, 0, 0], "opacity": 0.5})); + let p = package(&s); + assert!(p.xml.contains(r#""#), "{}", p.xml); + let d = temp("long-prefix"); + let e = s.execute("file.export", &json!({"path": d.join("l.urdf").to_string_lossy(), "mesh_prefix": "a".repeat(5000)})); + assert!(e.is_err_and(|e| e.to_string().contains("longer than"))); + let _ = std::fs::remove_dir_all(&d); +} + +#[test] +fn urdf_inertial_is_mass_weighted() { + let mut s = Session::default(); + run(&mut s, "component.create", json!({"name": "Cube"})); + run(&mut s, "solid.box", json!({"length": 10, "width": 10, "height": 10, "corner": [100, 0, 0]})); + let body = s.model.state().bodies.first().map(|b| b.name.clone()).unwrap_or_default(); + run(&mut s, "material.assign", json!({"bodies": [body], "material": "Aluminum"})); + let p = package(&s); + let Some(l) = p.model.links.iter().find(|l| l.name == "Cube") else { panic!("{:?}", names(&p.model)) }; + let Some(i) = l.inertial else { panic!("no inertial") }; + let m = 0.0027; + assert!((i.mass - m).abs() < 1e-12, "{}", i.mass); + let ixx = m * (0.01f64.powi(2) * 2.0) / 12.0; + for k in 0..3 { + assert!((i.inertia[k][k] - ixx).abs() < 1e-12 * ixx.max(1.0) + 1e-15, "{:?}", i.inertia); + } + assert!(i.inertia[0][1].abs() < 1e-15); + // The centre of mass is the cube's centre, in the link frame. + assert!(i.com.dist(solidcraft_doc::apply_point(&l.mesh_origin, solidcraft_geom::Vec3::new(105.0, 5.0, 5.0))) < 1e-9, "{:?}", i.com); +} + +#[test] +fn urdf_names_are_unique_across_links_joints_and_files() { + let mut s = Session::default(); + for name in ["Arm", "base", "Arm_link1"] { + run(&mut s, "component.activate", json!({"component": "root"})); + run(&mut s, "component.create", json!({"name": name})); + run(&mut s, "solid.box", json!({"length": 10, "width": 10, "height": 10, "corner": [0, 0, 0]})); + } + run(&mut s, "solid.box", json!({"length": 5, "width": 5, "height": 5, "corner": [50, 50, 0]})); + ground(&mut s, "base", true); + run( + &mut s, + "joint.create", + json!({"type": "cylindrical", "a": {"occurrence": "base", "face": [5, 5, 10]}, "b": {"occurrence": "Arm", "face": [5, 5, 0]}, "name": "Arm"}), + ); + let p = package(&s); + let (links, joints) = names(&p.model); + let mut all: Vec<&str> = links.iter().chain(&joints).copied().collect(); + all.extend(p.meshes.iter().map(|(f, _)| f.as_str())); + let n = all.len(); + all.sort(); + all.dedup(); + assert_eq!(all.len(), n, "{links:?} {joints:?}"); + assert!(joints.contains(&"Arm"), "{joints:?}"); +} + +#[test] +fn urdf_collision_skips_open_bodies() { + let mut s = Session::default(); + run(&mut s, "component.create", json!({"name": "Part"})); + run(&mut s, "solid.box", json!({"length": 10, "width": 10, "height": 10, "corner": [0, 0, 0]})); + run(&mut s, "sketch.create", json!({"plane": "XY", "name": "S"})); + run(&mut s, "sketch.line", json!({"points": [[30, 0], [40, 0]], "ids": ["l1"]})); + run(&mut s, "sketch.finish", json!({})); + run(&mut s, "surface.extrude", json!({"sketch": "S", "curves": ["l1"], "distance": 10, "body_name": "Wall"})); + let p = package(&s); + let Some(l) = p.model.links.iter().find(|l| l.name == "Part") else { panic!("{:?}", names(&p.model)) }; + let (Some(v), Some(c)) = (&l.visual, &l.collision) else { panic!("{l:?}") }; + assert_ne!(v, c, "a separate collision mesh"); + let bytes = &p.meshes.iter().find(|(f, _)| f == c).unwrap().1; + assert!(stl_volume(bytes).is_some(), "the collision mesh is closed"); + assert!(has_warning(&p, "not a closed solid"), "{:?}", p.warnings); + // Mass from the cube alone (default density). + let rho = s.doc.density(&s.model.state().bodies.first().map(|b| b.name.clone()).unwrap_or_default()); + assert!(l.inertial.is_some_and(|i| (i.mass - 1000.0 * rho * 1e-6).abs() < 1e-12)); +} + +#[test] +fn urdf_open_only_component_is_drawn() { + let mut s = Session::default(); + run(&mut s, "component.create", json!({"name": "Sheet"})); + run(&mut s, "sketch.create", json!({"plane": "XY", "name": "S"})); + run(&mut s, "sketch.line", json!({"points": [[30, 0], [40, 0]], "ids": ["l1"]})); + run(&mut s, "sketch.finish", json!({})); + run(&mut s, "surface.extrude", json!({"sketch": "S", "curves": ["l1"], "distance": 10, "body_name": "Wall"})); + let p = package(&s); + let Some(l) = p.model.links.iter().find(|l| l.name == "Sheet") else { panic!("{:?}", names(&p.model)) }; + assert!(l.visual.is_some() && l.collision.is_none(), "{l:?}"); + assert!(l.inertial.is_some_and(|i| i.mass == 1e-6), "{l:?}"); +} + +#[test] +fn urdf_skips_a_joint_that_closes_a_loop() { + let mut s = two_cubes(); + run(&mut s, "component.activate", json!({"component": "root"})); + run(&mut s, "component.create", json!({"name": "C"})); + run(&mut s, "solid.box", json!({"length": 10, "width": 10, "height": 10, "corner": [40, 0, 0]})); + run(&mut s, "joint.rigid_group", json!({"occurrences": ["A", "B", "C"]})); + run(&mut s, "joint.as_built", json!({"type": "rigid", "a": "B", "b": "C"})); + let p = package(&s); + assert!(has_warning(&p, "closes a loop"), "{:?}", p.warnings); + assert_tree(&p.model); + assert_fk(&p.model, &s, "loop"); +} + +/// Two grounded occurrences each reach C by a joint: the solver walks them in id order, so the +/// URDF keeps the joint from the one with the lower id, whatever order the file lists them in. +#[test] +fn urdf_walks_grounded_occurrences_in_id_order() { + let mut s = two_cubes(); + run(&mut s, "component.activate", json!({"component": "root"})); + run(&mut s, "component.create", json!({"name": "C"})); + run(&mut s, "solid.box", json!({"length": 10, "width": 10, "height": 10, "corner": [40, 0, 0]})); + ground(&mut s, "B", true); + run(&mut s, "joint.create", json!({"type": "rigid", "a": {"face": [25, 5, 10]}, "b": {"face": [45, 5, 0]}, "name": "FromB"})); + run(&mut s, "joint.create", json!({"type": "rigid", "a": {"face": [5, 5, 10]}, "b": {"occurrence": "C", "face": [25, 5, 20]}, "name": "FromA"})); + let mut doc = (*s.doc).clone(); + doc.occurrences.reverse(); + let p = solidcraft_io::urdf(&doc, &s.doc.painted(&s.model.state()), "t", &UrdfOptions::default()).unwrap(); + assert!(p.model.joints.iter().any(|j| j.name == "FromA"), "{:?}", names(&p.model)); + assert!(has_warning(&p, "joint `FromB` closes a loop"), "{:?}", p.warnings); +} + +/// Every link but `base_link` has exactly one parent joint. +fn assert_tree(m: &UrdfModel) { + for (i, l) in m.links.iter().enumerate().skip(1) { + assert_eq!(m.joints.iter().filter(|j| j.child == i).count(), 1, "{}", l.name); + } + assert_eq!(m.joints.iter().filter(|j| j.child == 0).count(), 0); +} + +#[test] +fn urdf_fixes_an_unjointed_occurrence_to_base() { + let s = two_cubes(); + let p = package(&s); + assert!(has_warning(&p, "`B` has no joint"), "{:?}", p.warnings); + assert_tree(&p.model); + assert_fk(&p.model, &s, "loose"); +} + +#[test] +fn urdf_of_an_ungrounded_assembly_keeps_its_joints() { + let mut s = two_cubes(); + ground(&mut s, "A", false); + joint(&mut s, "revolute", "Turn", Value::Null); + run(&mut s, "joint.drive", json!({"joint": "Turn", "value": 20})); + let p = package(&s); + assert!(has_warning(&p, "`A` is not grounded"), "{:?}", p.warnings); + assert!(p.model.joints.iter().any(|j| j.name == "Turn" && j.step == Step::Revolute)); + assert_fk(&p.model, &s, "ungrounded"); + run(&mut s, "joint.drive", json!({"joint": "Turn", "value": -70})); + assert_fk(&p.model, &s, "ungrounded, driven"); +} + +#[test] +fn urdf_skips_a_joint_with_an_error() { + let mut s = two_cubes(); + joint(&mut s, "revolute", "Turn", Value::Null); + // Snap the joint to a face that then goes away. + let Some(f) = s.doc.features.iter().rev().find(|f| matches!(f.kind, solidcraft_doc::FeatureKind::Box { .. })).map(|f| f.id) else { + panic!("box") + }; + run(&mut s, "timeline.suppress", json!({"feature": f, "suppressed": true})); + let p = package(&s); + assert!(has_warning(&p, "joint `Turn` is skipped"), "{:?}", p.warnings); + assert_tree(&p.model); +} + +#[test] +fn urdf_nested_occurrence_moves_with_its_parent() { + let mut s = hinge(); + let b = s.doc.find_component("LeafB").unwrap_or(0); + run(&mut s, "component.activate", json!({"component": "LeafB"})); + run(&mut s, "component.create", json!({"name": "Pin", "parent": b})); + run(&mut s, "solid.box", json!({"length": 4, "width": 4, "height": 4, "corner": [10, 10, 3]})); + run(&mut s, "joint.drive", json!({"joint": "Hinge", "value": 30})); + let p = package(&s); + assert!(!has_warning(&p, "Pin"), "{:?}", p.warnings); + let pin = p.model.links.iter().position(|l| l.name == "Pin").unwrap_or(0); + let leaf_b = p.model.links.iter().position(|l| l.name == "LeafB").unwrap_or(0); + assert!(p.model.joints.iter().any(|j| j.child == pin && j.parent == leaf_b), "Pin hangs on LeafB"); + assert_fk(&p.model, &s, "nested"); + run(&mut s, "joint.drive", json!({"joint": "Hinge", "value": 120})); + assert_fk(&p.model, &s, "nested, driven"); +} + +#[test] +fn urdf_copies_nested_occurrences_of_each_placement() { + let mut s = Session::default(); + run(&mut s, "component.create", json!({"name": "Leg"})); + run(&mut s, "solid.box", json!({"length": 10, "width": 10, "height": 30, "corner": [0, 0, 0]})); + let leg = s.doc.find_component("Leg").unwrap_or(0); + run(&mut s, "component.create", json!({"name": "Foot", "parent": leg})); + run(&mut s, "solid.box", json!({"length": 12, "width": 12, "height": 2, "corner": [-1, -1, -2]})); + run(&mut s, "component.activate", json!({"component": "root"})); + run(&mut s, "occurrence.copy", json!({"component": "Leg", "translate": [50, 0, 0]})); + assert_eq!(s.doc.placements().len(), 5, "root, two legs, two feet"); + let p = package(&s); + assert_fk(&p.model, &s, "copies"); + assert_tree(&p.model); +} + +/// Deeper than `placements()` walks (a file can hold it; commands stop at 64 levels): the +/// export says copies past the limit are left out. +#[test] +fn urdf_warns_when_placements_are_cut_off() { + let s = Session::default(); + let mut doc = (*s.doc).clone(); + for k in 1..=66u64 { + doc.components.push(solidcraft_doc::Component { id: 1000 + k, name: format!("C{k}"), parent: if k == 1 { 0 } else { 999 + k } }); + doc.occurrences.push(solidcraft_doc::Occurrence { + id: 2000 + k, + component: 1000 + k, + parent: if k == 1 { 0 } else { 999 + k }, + name: format!("C{k}:1"), + transform: solidcraft_doc::IDENTITY, + grounded: false, + }); + } + // A component placed twice with a part nested in it, next to the deep chain: its copy is + // still there, since only the branch past the limit stops. + doc.components.push(solidcraft_doc::Component { id: 3001, name: "Leg".into(), parent: 0 }); + doc.components.push(solidcraft_doc::Component { id: 3002, name: "Foot".into(), parent: 3001 }); + // Listed first, so the walk (a stack) goes down the chain before reaching them. + for (id, component, parent, name) in [(4003, 3002, 3001, "Foot:1"), (4002, 3001, 0, "Leg:2"), (4001, 3001, 0, "Leg:1")] { + let o = solidcraft_doc::Occurrence { id, component, parent, name: name.into(), transform: solidcraft_doc::IDENTITY, grounded: false }; + doc.occurrences.insert(0, o); + } + let p = solidcraft_io::urdf(&doc, &s.model.state(), "t", &UrdfOptions::default()).unwrap(); + assert!(has_warning(&p, "left out"), "{:?}", p.warnings); + assert!(p.model.links.iter().any(|l| l.path == [4002, 4003]), "the copy of Foot under Leg:2"); + assert_tree(&p.model); +} + +#[test] +fn urdf_robot_name_is_xml_safe() { + let s = hinge(); + for stem in ["a--b", "x\u{1}y", "<&>"] { + let p = match solidcraft_io::urdf(&s.doc, &s.doc.painted(&s.model.state()), stem, &UrdfOptions::default()) { + Ok(p) => p, + Err(e) => panic!("{e}"), + }; + assert!(well_formed(&p.xml).is_ok(), "{stem}: {:?}\n{}", well_formed(&p.xml), p.xml); + assert!(p.xml.chars().all(|c| c == '\n' || !c.is_control()), "{stem}"); + } +} + +/// The XML is well formed: every element closes, in order. +fn well_formed(xml: &str) -> Result<(), String> { + let mut stack: Vec = Vec::new(); + let mut rest = xml; + while let Some(i) = rest.find('<') { + rest = &rest[i + 1..]; + let Some(end) = rest.find('>') else { return Err("unclosed tag".into()) }; + let tag = &rest[..end]; + rest = &rest[end + 1..]; + if tag.starts_with('?') || tag.starts_with('!') || tag.ends_with('/') { + continue; + } + if let Some(name) = tag.strip_prefix('/') { + if stack.pop().as_deref() != Some(name) { + return Err(format!(" out of order")); + } + } else { + stack.push(tag.split_whitespace().next().unwrap_or("").to_string()); + } + } + if stack.is_empty() { Ok(()) } else { Err(format!("unclosed {stack:?}")) } +} + +#[test] +fn urdf_needs_the_whole_assembly() { + let mut s = hinge(); + let d = temp("bodies"); + let e = s.execute("file.export", &json!({"path": d.join("x.urdf").to_string_lossy(), "bodies": ["Body1"]})); + assert!(e.is_err_and(|e| e.to_string().contains("whole assembly"))); + assert_eq!(std::fs::read_dir(&d).map(|r| r.count()).unwrap_or(0), 0); + let _ = std::fs::remove_dir_all(&d); +} + +#[test] +fn urdf_files_stay_inside_allow_paths() { + let mut s = hinge(); + let inside = temp("scope-in"); + let outside = temp("scope-out"); + let refused = solidcraft_io::vfs::scoped(Some(solidcraft_io::vfs::PathScope::new([inside.clone()])), || { + s.execute("file.export", &json!({"path": outside.join("r.urdf").to_string_lossy()})) + }); + assert!(refused.is_err()); + assert_eq!(std::fs::read_dir(&outside).map(|r| r.count()).unwrap_or(0), 0, "nothing written"); + let ok = solidcraft_io::vfs::scoped(Some(solidcraft_io::vfs::PathScope::new([inside.clone()])), || { + s.execute("file.export", &json!({"path": inside.join("r.urdf").to_string_lossy()})) + }); + assert!(ok.is_ok(), "{ok:?}"); + let _ = std::fs::remove_dir_all(&inside); + let _ = std::fs::remove_dir_all(&outside); +} diff --git a/crates/geom/src/lib.rs b/crates/geom/src/lib.rs index fa6c9432..8a6e3f13 100644 --- a/crates/geom/src/lib.rs +++ b/crates/geom/src/lib.rs @@ -21,7 +21,7 @@ pub mod tspline; mod vec; pub use arcfit::MAX_FIT_PIECES; -pub use mesh::{Aabb3, BoxedTriangle, InsideIndex, Mesh, MeshMeasure, find_close_pair}; +pub use mesh::{Aabb3, BoxedTriangle, InsideIndex, MassProperties, Mesh, MeshMeasure, find_close_pair}; pub use plane::Plane; pub use profile::{Loop2, MAX_BEZIER_DEGREE, Region2, Seg2, bezier, regions_apart}; pub use vec::{Vec2, Vec3}; diff --git a/crates/geom/src/mesh.rs b/crates/geom/src/mesh.rs index 03aa7cc8..8a9045d4 100644 --- a/crates/geom/src/mesh.rs +++ b/crates/geom/src/mesh.rs @@ -252,6 +252,72 @@ pub struct MeshMeasure { pub centroid: Vec3, } +/// Volume, centroid and inertia tensor of a closed mesh at unit density: the inertia is about +/// the centroid, in the mesh's axes (times a density, mass and inertia in that density's units). +#[derive(Clone, Copy, Debug, Default, PartialEq)] +pub struct MassProperties { + pub volume: f64, + pub centroid: Vec3, + pub inertia: [[f64; 3]; 3], +} + +impl Mesh { + /// [`MassProperties`] of a closed mesh, by the divergence theorem over the tetrahedra its + /// triangles make with the origin. `None` unless every edge (by vertex index: weld first) is + /// shared by exactly two triangles that cross it in opposite directions (closed, consistently + /// oriented), or for a mesh without volume or with non-finite values. + pub fn mass_properties(&self) -> Option { + // Closed and consistently oriented: each edge is crossed once in each direction. + let mut edges: std::collections::HashMap<(u32, u32), u32> = std::collections::HashMap::new(); + for t in &self.triangles { + for (a, b) in [(t[0], t[1]), (t[1], t[2]), (t[2], t[0])] { + *edges.entry((a, b)).or_insert(0) += 1; + } + } + if edges.is_empty() || edges.iter().any(|(&(a, b), n)| *n != 1 || edges.get(&(b, a)) != Some(&1)) { + return None; + } + let xyz = |v: Vec3| [v.x, v.y, v.z]; + let (mut v6, mut first, mut second) = (0.0, Vec3::ZERO, [[0.0f64; 3]; 3]); + for t in &self.triangles { + let [a, b, c] = self.tri(t)?; + let det = a.dot(b.cross(c)); + v6 += det; + first += (a + b + c) * det; + // ∫ x xᵀ over the tetrahedron (0, a, b, c) = det/120 · (Σ p pᵀ + s sᵀ), s = a + b + c. + let (p, s) = ([xyz(a), xyz(b), xyz(c)], xyz(a + b + c)); + for (i, row) in second.iter_mut().enumerate() { + for (j, cell) in row.iter_mut().enumerate() { + *cell += det / 120.0 * (p.iter().map(|q| q[i] * q[j]).sum::() + s[i] * s[j]); + } + } + } + if !(v6.abs() / 6.0 > 1e-12) { + return None; + } + // Inward-facing: every sum changes sign together. + let sign = v6.signum(); + let volume = v6.abs() / 6.0; + let centroid = first / (4.0 * v6); + let c = xyz(centroid); + let mut about = [[0.0f64; 3]; 3]; + for i in 0..3 { + for j in 0..3 { + about[i][j] = sign * second[i][j] - volume * c[i] * c[j]; + } + } + let trace = about[0][0] + about[1][1] + about[2][2]; + let mut inertia = [[0.0f64; 3]; 3]; + for i in 0..3 { + for j in 0..3 { + inertia[i][j] = if i == j { trace } else { 0.0 } - about[i][j]; + } + } + let finite = volume.is_finite() && c.iter().all(|x| x.is_finite()) && inertia.iter().flatten().all(|x| x.is_finite()); + finite.then_some(MassProperties { volume, centroid, inertia }) + } +} + impl Mesh { /// Visibility in shaded edge presentation, independent of selection/topology. pub fn shaded_edge_visible(&self, index: usize) -> bool { diff --git a/crates/geom/src/tests.rs b/crates/geom/src/tests.rs index 3929e5ac..3e607f32 100644 --- a/crates/geom/src/tests.rs +++ b/crates/geom/src/tests.rs @@ -320,6 +320,102 @@ fn meshes_cross_only_where_their_surfaces_pass_through_each_other() { assert!(!a.crosses(&cube(9.85, 3.0, 1), f64::NAN)); } +/// A closed box mesh `[x0, x0+a] x [y0, y0+b] x [z0, z0+c]`, outward-facing. +fn offset_box(o: Vec3, a: f64, b: f64, c: f64) -> Mesh { + let p = |x: f64, y: f64, z: f64| o + Vec3::new(x * a, y * b, z * c); + let positions = vec![p(0., 0., 0.), p(1., 0., 0.), p(1., 1., 0.), p(0., 1., 0.), p(0., 0., 1.), p(1., 0., 1.), p(1., 1., 1.), p(0., 1., 1.)]; + let triangles = + vec![[0, 2, 1], [0, 3, 2], [4, 5, 6], [4, 6, 7], [0, 1, 5], [0, 5, 4], [1, 2, 6], [1, 6, 5], [2, 3, 7], [2, 7, 6], [3, 0, 4], [3, 4, 7]]; + Mesh { positions, triangles, ..Default::default() } +} + +fn close(a: f64, b: f64, rel: f64) -> bool { + (a - b).abs() <= rel * b.abs().max(1e-12) +} + +#[test] +fn mass_properties_of_a_box_match_the_closed_form() { + let (a, b, c) = (20.0, 30.0, 40.0); + let m = offset_box(Vec3::ZERO, a, b, c).mass_properties().expect("a closed box has mass properties"); + let v = a * b * c; + assert!(close(m.volume, v, 1e-12), "{m:?}"); + assert!((m.centroid - Vec3::new(10.0, 15.0, 20.0)).len() < 1e-9, "{m:?}"); + let i = m.inertia; + assert!(close(i[0][0], v * (b * b + c * c) / 12.0, 1e-9), "{i:?}"); + assert!(close(i[1][1], v * (a * a + c * c) / 12.0, 1e-9), "{i:?}"); + assert!(close(i[2][2], v * (a * a + b * b) / 12.0, 1e-9), "{i:?}"); + for (r, cc) in [(0, 1), (0, 2), (1, 2)] { + assert!(i[r][cc].abs() < 1e-6 && i[cc][r].abs() < 1e-6, "{i:?}"); + } +} + +#[test] +fn mass_properties_of_a_cylinder_are_close() { + // A cylinder of radius 10 and height 30 along z, 256 sides, capped by fans. + let (r, h, n) = (10.0, 30.0, 256usize); + let mut positions = vec![Vec3::new(0.0, 0.0, 0.0), Vec3::new(0.0, 0.0, h)]; + for k in 0..n { + let t = std::f64::consts::TAU * k as f64 / n as f64; + positions.push(Vec3::new(r * t.cos(), r * t.sin(), 0.0)); + positions.push(Vec3::new(r * t.cos(), r * t.sin(), h)); + } + let mut triangles = Vec::new(); + for k in 0..n as u32 { + let (b0, t0) = (2 + 2 * k, 3 + 2 * k); + let (b1, t1) = (2 + 2 * ((k + 1) % n as u32), 3 + 2 * ((k + 1) % n as u32)); + triangles.push([0, b1, b0]); + triangles.push([1, t0, t1]); + triangles.push([b0, b1, t1]); + triangles.push([b0, t1, t0]); + } + let m = Mesh { positions, triangles, ..Default::default() }.mass_properties().expect("closed"); + let v = std::f64::consts::PI * r * r * h; + assert!(close(m.volume, v, 1e-3), "{m:?}"); + assert!(close(m.inertia[2][2], m.volume * r * r / 2.0, 1e-3), "{m:?}"); + assert!(close(m.inertia[0][0], m.volume * (3.0 * r * r + h * h) / 12.0, 1e-3), "{m:?}"); +} + +#[test] +fn mass_properties_are_translation_invariant_about_the_centroid() { + let at_origin = offset_box(Vec3::ZERO, 20.0, 30.0, 40.0).mass_properties().unwrap(); + let moved = offset_box(Vec3::new(100.0, -50.0, 7.0), 20.0, 30.0, 40.0).mass_properties().unwrap(); + assert!((moved.centroid - Vec3::new(110.0, -35.0, 27.0)).len() < 1e-9, "{moved:?}"); + for r in 0..3 { + for c in 0..3 { + assert!((moved.inertia[r][c] - at_origin.inertia[r][c]).abs() <= 1e-9 * at_origin.inertia[0][0], "{r}{c}"); + } + } +} + +#[test] +fn mass_properties_of_an_inward_facing_box_are_positive() { + let mut m = offset_box(Vec3::ZERO, 2.0, 3.0, 4.0); + for t in &mut m.triangles { + t.swap(1, 2); + } + let p = m.mass_properties().unwrap(); + assert!(close(p.volume, 24.0, 1e-12) && p.inertia[0][0] > 0.0, "{p:?}"); +} + +#[test] +fn mass_properties_of_an_open_or_empty_mesh_are_none() { + let mut open = offset_box(Vec3::ZERO, 20.0, 30.0, 40.0); + open.triangles.truncate(10); + assert!(open.mass_properties().is_none(), "a box missing a face has no mass"); + assert!(Mesh::default().mass_properties().is_none()); +} + +#[test] +fn mass_properties_need_consistently_oriented_faces() { + // Every edge still has two triangles, but one face of the box points inwards: its signed + // volume would cancel part of the rest. + let mut m = offset_box(Vec3::ZERO, 20.0, 30.0, 40.0); + for t in m.triangles.iter_mut().take(2) { + t.swap(1, 2); + } + assert!(m.mass_properties().is_none()); +} + mod close_pairs { use super::*; diff --git a/crates/io/src/lib.rs b/crates/io/src/lib.rs index d5f7b831..3a380344 100644 --- a/crates/io/src/lib.rs +++ b/crates/io/src/lib.rs @@ -1,6 +1,6 @@ //! File formats. Designs use JSON or versioned containers (`.solidcraft`); the authoritative //! document rebuilds on load and optional display caches remain provisional. Bodies export -//! as STL (binary or ASCII), OBJ, STEP and 3MF; STEP +//! as STL (binary or ASCII), OBJ, STEP and 3MF, and assemblies as URDF ([`urdf()`]); STEP //! files import as a base feature (see [`step_import_feature`]), 3MF and STL files as mesh bodies //! (see [`mesh_import_feature`]). #![deny(clippy::unwrap_used, clippy::expect_used, clippy::panic, clippy::unimplemented, clippy::todo, clippy::unreachable)] @@ -20,6 +20,7 @@ pub mod safe_write; mod sketch2d; mod svg; mod threemf; +pub mod urdf; pub mod vfs; pub use dxf::{read_dxf, write_dxf}; @@ -30,6 +31,7 @@ pub use svg::read_svg; mod obj; pub use obj::read_obj; pub use threemf::{MAX_3MF_BYTES, MeshObject, model_xml, read_3mf, weld, write_3mf}; +pub use urdf::{UrdfOptions, UrdfPackage, urdf}; use solidcraft_doc::{Document, ModelState}; use solidcraft_geom::{Mesh, Vec3}; @@ -54,7 +56,7 @@ pub const MAX_DESIGN_BYTES: usize = 256 << 20; #[derive(Debug, thiserror::Error)] pub enum IoError { - #[error("unsupported format `{0}` (use stl, obj, step, 3mf or solidcraft)")] + #[error("unsupported format `{0}` (use stl, obj, step, 3mf, urdf or solidcraft)")] Format(String), #[error("nothing to export: the design has no bodies")] Empty, @@ -202,6 +204,9 @@ pub enum Format { /// `.3mf` path alone means [`Format::ThreeMf`]. ThreeMfTagged, Design, + /// A robot description of the assembly with its mesh files ([`urdf()`]); written by the + /// engine, since it is several files. + Urdf, } impl Format { @@ -216,6 +221,7 @@ impl Format { "igs" | "iges" => Format::Iges, "3mf" => Format::ThreeMf, "3mf-tagged" => Format::ThreeMfTagged, + "urdf" => Format::Urdf, "solidcraft" | "solvecraft" | "json" => Format::Design, _ => return Err(IoError::Format(ext)), }) @@ -336,6 +342,7 @@ pub fn export(state: &ModelState, bodies: &[String], format: Format, name: &str) write_3mf(&objs)? } Format::Design => return Err(IoError::Format("solidcraft (save the document instead)".into())), + Format::Urdf => return Err(IoError::Format("urdf (an assembly package; use file.export)".into())), }) } diff --git a/crates/io/src/urdf.rs b/crates/io/src/urdf.rs new file mode 100644 index 00000000..ea5dd9ee --- /dev/null +++ b/crates/io/src/urdf.rs @@ -0,0 +1,916 @@ +//! URDF robot description of a design's components and joints (#719, docs/urdf.md): a link per +//! placement of a component (plus `base_link` for the root and what is grounded), a joint chain +//! per SolidCraft joint, meshes in metres next to the `.urdf`, and each link's mass, centre of +//! mass and inertia from its bodies and their materials. +//! +//! Frames follow the joint kinematics `T_B · F_b = T_A · F_a · M(q) · Flip` (`doc::joints`). +//! Every constant is taken from world transforms at the current state, so `Flip` never appears: +//! with the child on B its link frame is `L = T_A · F_a · M(q)`; with the child on A it is +//! `L = T_A · F_a` and the parent-side joint frame `L · M(q)`, the chain inverted (reversed, axes +//! negated, so each URDF value still equals SolidCraft's). + +use std::collections::{BTreeMap, BTreeSet, HashSet, VecDeque}; + +use solidcraft_doc::joints::{self, Joint, JointKind}; +use solidcraft_doc::{Document, IDENTITY, MAX_PLACEMENT_DEPTH, MAX_PLACEMENTS, Mat, ModelState, Occurrence, apply_point, mat_inverse, mat_mul}; +use solidcraft_geom::{Mesh, Vec3}; + +use crate::{IoError, Result, export_mesh, stl_binary, weld, weld_eps}; + +/// Mass (kg) and inertia (kg m²) of a link with no massive body: Gazebo drops massless links. +const TOKEN_MASS: f64 = 1e-6; +const TOKEN_INERTIA: f64 = 1e-12; +/// Travel written for a prismatic step without a limit (mm): URDF requires one. +const UNLIMITED_TRAVEL_MM: f64 = 1e6; +const DEFAULT_EFFORT: f64 = 1000.0; +const DEFAULT_VELOCITY: f64 = 10.0; + +/// Choices for [`urdf`] (`file.export` parameters). +#[derive(Clone, Debug, Default)] +pub struct UrdfOptions { + /// Put before each mesh file name (`package://robot/meshes/` for RViz; empty: relative). + pub mesh_prefix: String, + /// `` (N or N m) and `velocity` (m/s or rad/s); defaults with a warning. + pub effort: Option, + pub velocity: Option, +} + +/// The URDF, its mesh files (name next to the `.urdf`, bytes) and what was approximated. +#[derive(Clone, Debug)] +pub struct UrdfPackage { + pub xml: String, + pub meshes: Vec<(String, Vec)>, + pub model: UrdfModel, + pub warnings: Vec, +} + +/// The robot before serialisation (lengths in mm): tests compose it to check the kinematics. +#[derive(Clone, Debug, Default)] +pub struct UrdfModel { + pub links: Vec, + pub joints: Vec, +} + +#[derive(Clone, Debug)] +pub struct Link { + pub name: String, + /// The placement it is (occurrence ids from the root, as `Document::placements`); empty for + /// `base_link` and a chain's virtual links. + pub path: Vec, + /// Mesh coordinates (the component's frame) to the link frame. + pub mesh_origin: Mat, + pub visual: Option, + pub collision: Option, + /// sRGB and opacity, 0..1. + pub color: Option<[f32; 4]>, + pub inertial: Option, +} + +/// Mass (kg), centre of mass (mm, link frame) and inertia about it (kg m², link axes). +#[derive(Clone, Copy, Debug)] +pub struct Inertial { + pub mass: f64, + pub com: Vec3, + pub inertia: [[f64; 3]; 3], +} + +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub enum Step { + Fixed, + Revolute, + Prismatic, +} + +#[derive(Clone, Debug)] +pub struct UrdfJoint { + pub name: String, + pub parent: usize, + pub child: usize, + /// Parent link frame to this joint's frame (mm). + pub origin: Mat, + pub step: Step, + pub axis: Vec3, + /// (lower, upper): radians or mm; `None` for a revolute without limit (`continuous`). + pub limit: Option<(f64, f64)>, + /// The SolidCraft joint and the index of its value this step follows. + pub source: Option<(u64, usize)>, +} + +impl UrdfModel { + /// Every link's world frame (mm) for the given value of each step. + pub fn link_frames(&self, value: impl Fn(&UrdfJoint) -> f64) -> Vec { + let mut frames = vec![IDENTITY; self.links.len()]; + let mut done = vec![false; self.links.len()]; + if let Some(d) = done.get_mut(0) { + *d = true; + } + // Joints are emitted parents first. + for j in &self.joints { + let (Some(parent), true) = (frames.get(j.parent).copied(), done.get(j.parent).copied().unwrap_or(false)) else { continue }; + let q = value(j); + let m = match j.step { + Step::Fixed => IDENTITY, + Step::Revolute => rotation(j.axis, q), + Step::Prismatic => translation(j.axis * q), + }; + if let (Some(f), Some(d)) = (frames.get_mut(j.child), done.get_mut(j.child)) { + *f = mat_mul(&mat_mul(&parent, &j.origin), &m); + *d = true; + } + } + frames + } +} + +fn translation(v: Vec3) -> Mat { + let mut m = IDENTITY; + m[3] = [v.x, v.y, v.z, 1.0]; + m +} + +/// Rotation by `a` about the unit axis `u` (Rodrigues), column-major like `Mat`. +fn rotation(u: Vec3, a: f64) -> Mat { + let (s, c) = a.sin_cos(); + let t = 1.0 - c; + let r = [ + [t * u.x * u.x + c, t * u.x * u.y - s * u.z, t * u.x * u.z + s * u.y], + [t * u.x * u.y + s * u.z, t * u.y * u.y + c, t * u.y * u.z - s * u.x], + [t * u.x * u.z - s * u.y, t * u.y * u.z + s * u.x, t * u.z * u.z + c], + ]; + // r[row][col] -> m[col][row] + let mut m = IDENTITY; + for (row, cols) in r.iter().enumerate() { + for (col, v) in cols.iter().enumerate() { + m[col][row] = *v; + } + } + m +} + +/// The joint's motion as URDF steps, in order from A's frame: (kind, axis, index of the value). +fn steps(kind: JointKind) -> Vec<(Step, Vec3, usize)> { + use Step::{Prismatic as P, Revolute as R}; + match kind { + JointKind::Rigid => vec![], + JointKind::Revolute => vec![(R, Vec3::Z, 0)], + JointKind::Slider => vec![(P, Vec3::Z, 0)], + JointKind::Cylindrical => vec![(R, Vec3::Z, 0), (P, Vec3::Z, 1)], + JointKind::PinSlot => vec![(P, Vec3::X, 1), (R, Vec3::Z, 0)], + JointKind::Planar => vec![(P, Vec3::X, 0), (P, Vec3::Y, 1), (R, Vec3::Z, 2)], + JointKind::Ball => vec![(R, Vec3::Z, 0), (R, Vec3::Y, 1), (R, Vec3::Z, 2)], + } +} + +/// `raw` with every character but `[A-Za-z0-9_]` (and `-` in file names) replaced by `_`, so it +/// is a valid name in URDF, SDFormat and XML. +fn sanitise(raw: &str, file: bool) -> String { + let s: String = raw.chars().map(|c| if c.is_ascii_alphanumeric() || c == '_' || (file && c == '-') { c } else { '_' }).collect(); + if s.is_empty() { "part".into() } else { s } +} + +/// Unique names over links, joints and files (SDFormat refuses a joint named like a link). +#[derive(Default)] +struct Names(HashSet); + +impl Names { + fn take(&mut self, raw: &str) -> String { + self.unique(sanitise(raw, false)) + } + /// A mesh file name, unique without regard to case: Windows and macOS file systems ignore + /// it, so `t-base` and `t-Base` would be one file. + fn file(&mut self, raw: &str) -> String { + let base = sanitise(raw, true); + let mut name = base.clone(); + let mut k = 2; + while !self.0.insert(name.to_ascii_lowercase()) { + name = format!("{base}_{k}"); + k += 1; + } + format!("{name}.stl") + } + fn unique(&mut self, base: String) -> String { + let mut name = base.clone(); + let mut k = 2; + while !self.0.insert(name.clone()) { + name = format!("{base}_{k}"); + k += 1; + } + name + } +} + +/// One body's mass properties in its component's frame: (mass kg, centre mm, inertia kg mm²). +type BodyMass = (f64, Vec3, [[f64; 3]; 3]); + +/// A component's meshes and masses. +struct Part { + visual: Option, + collision: Option, + color: Option<[f32; 4]>, + masses: Vec, +} + +/// Mesh files and masses per component (0: the root's bodies, named `base`). +fn parts(doc: &Document, st: &ModelState, stem: &str, names: &mut Names, out: &mut UrdfPackage) -> Result> { + let mut used: BTreeSet = doc.occurrences.iter().map(|o| o.component).collect(); + used.insert(0); + let mut parts = BTreeMap::new(); + for c in used { + let label = if c == 0 { "base".to_string() } else { doc.components.iter().find(|x| x.id == c).map(|x| x.name.clone()).unwrap_or_default() }; + let (mut visual, mut collision) = (Mesh::default(), Mesh::default()); + let (mut masses, mut color, mut open) = (Vec::new(), None, false); + for b in st.bodies.iter().filter(|b| doc.model_body_component(b) == c) { + let mut m = export_mesh(&b.body)?; + if let Some(o) = doc.model_body_offset(b) { + for p in &mut m.positions { + *p = apply_point(o, *p); + } + } + let (positions, triangles) = weld(&m.positions, &m.triangles, weld_eps(&b.body)); + let welded = Mesh { positions, triangles, ..Mesh::default() }; + let opacity = b.body.paint().map(|p| p.opacity).filter(|o| o.is_finite()).map_or(1.0, |o| o.clamp(0.0, 1.0)); + color = color.or(b.body.color().map(|[r, g, b]| [r, g, b, opacity])); + append(&mut visual, &welded); + match welded.mass_properties() { + Some(mp) => { + // g/cm³ to kg/mm³. + let rho = doc.density(&b.name) * 1e-6; + masses.push((rho * mp.volume, mp.centroid, mp.inertia.map(|r| r.map(|x| x * rho)))); + append(&mut collision, &welded); + } + None => { + open = true; + out.warnings.push(format!("body `{}` is not a closed solid: it is drawn but adds no mass or collision", b.name)); + } + } + } + let mut part = Part { visual: None, collision: None, color, masses }; + if !visual.triangles.is_empty() { + let file = names.file(&format!("{stem}-{label}")); + out.meshes.push((file.clone(), stl_binary(&[metres(&visual)], &label))); + part.visual = Some(file.clone()); + if !open { + part.collision = Some(file); + } else if !collision.triangles.is_empty() { + let file = names.file(&format!("{stem}-{label}-collision")); + out.meshes.push((file.clone(), stl_binary(&[metres(&collision)], &label))); + part.collision = Some(file); + } + } + parts.insert(c, part); + } + Ok(parts) +} + +/// The placement `occurrence_world` resolves for an occurrence: through the first occurrence of +/// each parent component. +fn canonical(doc: &Document, occ: u64) -> Vec { + let mut path = Vec::new(); + let mut cur = doc.occurrences.iter().find(|o| o.id == occ); + for _ in 0..1000 { + let Some(o) = cur else { break }; + path.push(o.id); + if o.parent == 0 { + break; + } + cur = doc.occurrence_of(o.parent); + } + path.reverse(); + path +} + +/// The tree as it grows. +struct Tree<'a> { + doc: &'a Document, + parts: BTreeMap, + names: Names, + model: UrdfModel, + warnings: Vec, + /// Occurrence to its link and the link's world frame (0: `base_link`). + placed: BTreeMap, + /// Each usable joint's name, taken before any link's so it keeps the name the user gave. + joint_names: BTreeMap, +} + +impl<'a> Tree<'a> { + fn world(&self, occ: u64) -> Mat { + if occ == 0 { IDENTITY } else { joints::occurrence_world(self.doc, occ) } + } + + fn occurrence(&self, occ: u64) -> Option<&'a Occurrence> { + let doc = self.doc; + doc.occurrences.iter().find(|o| o.id == occ) + } + + /// A link for placement `path` of `o`: its frame is `frame`, its component's meshes sit at + /// `at` (both world). Named after the component when it is placed once, else the occurrence. + fn link(&mut self, o: &Occurrence, path: Vec, frame: &Mat, at: &Mat) -> Option { + let mesh_origin = mat_mul(&mat_inverse(frame)?, at); + let doc = self.doc; + let single = doc.occurrences.iter().filter(|x| x.component == o.component).count() == 1; + let raw = match doc.components.iter().find(|c| c.id == o.component) { + Some(c) if single => c.name.as_str(), + _ => o.name.as_str(), + }; + let part = self.parts.get(&o.component); + let link = Link { + name: self.names.take(raw), + path, + mesh_origin, + visual: part.and_then(|p| p.visual.clone()), + collision: part.and_then(|p| p.collision.clone()), + color: part.and_then(|p| p.color), + inertial: part.and_then(|p| inertial(&p.masses, &mesh_origin)).or(Some(token())), + }; + self.model.links.push(link); + Some(self.model.links.len() - 1) + } + + /// `child` hangs on `parent` where it is (`origin`: parent link frame to child link frame). + fn fixed(&mut self, parent: usize, child: usize, origin: Mat) { + let n = self.model.links.get(child).map(|l| format!("{}_fixed", l.name)).unwrap_or_default(); + let name = self.names.take(&n); + self.model.joints.push(UrdfJoint { name, parent, child, origin, step: Step::Fixed, axis: Vec3::Z, limit: None, source: None }); + } + + /// The link an occurrence fixed in place hangs on, and its frame: `base_link` for the top + /// level (or `to_base`), else its parent occurrence's link once that is in the tree. + fn parent_link(&self, o: &Occurrence, to_base: bool) -> Option<(usize, Mat)> { + if to_base || o.parent == 0 { + return Some((0, IDENTITY)); + } + let p = self.doc.occurrence_of(o.parent)?; + self.placed.get(&p.id).copied() + } + + /// Fix an occurrence where it is on its parent link. + fn fix(&mut self, occ: u64, to_base: bool) -> Option { + let o = self.occurrence(occ)?; + let (parent, parent_frame) = self.parent_link(o, to_base)?; + let w = self.world(occ); + let origin = mat_mul(&mat_inverse(&parent_frame)?, &w); + let i = self.link(o, canonical(self.doc, occ), &w, &w)?; + self.fixed(parent, i, origin); + self.placed.insert(occ, (i, w)); + Some(i) + } + + /// Breadth-first over the usable joints from the occurrences in `queue`. + fn traverse(&mut self, queue: &mut VecDeque, usable: &[(&Joint, Mat)], taken: &mut HashSet) { + let values = joints::joint_values(&self.doc.assembly); + while let Some(p) = queue.pop_front() { + for (j, fa) in usable { + if taken.contains(&j.id) || (j.a.occurrence != p && j.b.occurrence != p) { + continue; + } + taken.insert(j.id); + let child = if j.a.occurrence == p { j.b.occurrence } else { j.a.occurrence }; + if self.placed.contains_key(&child) { + self.warnings.push(format!("joint `{}` closes a loop: skipped (URDF is a tree)", j.name)); + continue; + } + if self.attach(j, fa, p, child, values.get(&j.id).map(Vec::as_slice).unwrap_or(&[])).is_some() { + queue.push_back(child); + } + } + } + } + + /// `child` on joint `j` from `p` (already in the tree): the joint's chain and the child's link. + fn attach(&mut self, j: &Joint, fa: &Mat, p: u64, child: u64, q: &[f64]) -> Option<()> { + let m = joints::motion(j.kind, q); + let child_on_b = j.a.occurrence == p; + // (parent-side joint frame, child link frame) in the world. + let (jf, lf) = if child_on_b { + let jf = mat_mul(&self.world(p), fa); + (jf, mat_mul(&jf, &m)) + } else { + let lf = mat_mul(&self.world(child), fa); + (mat_mul(&lf, &m), lf) + }; + let (parent_index, parent_frame) = self.placed.get(&p).copied()?; + let origin = mat_mul(&mat_inverse(&parent_frame)?, &jf); + let mut chain = steps(j.kind); + if !child_on_b { + chain.reverse(); + for s in &mut chain { + s.1 = -s.1; + } + } + let o = self.occurrence(child)?; + let child_index = self.link(o, canonical(self.doc, child), &lf, &self.world(child))?; + self.placed.insert(child, (child_index, lf)); + let joint_name = self.joint_names.get(&j.id).cloned().unwrap_or_else(|| j.name.clone()); + if chain.is_empty() { + let name = joint_name; + self.model.joints.push(UrdfJoint { + name, + parent: parent_index, + child: child_index, + origin, + step: Step::Fixed, + axis: Vec3::Z, + limit: None, + source: None, + }); + return Some(()); + } + let mut from = parent_index; + let last = chain.len() - 1; + for (k, (step, axis, dof)) in chain.into_iter().enumerate() { + let to = if k == last { + child_index + } else { + let name = self.names.take(&format!("{}_link{}", j.name, k + 1)); + self.model.links.push(Link { + name, + path: Vec::new(), + mesh_origin: IDENTITY, + visual: None, + collision: None, + color: None, + inertial: Some(token()), + }); + self.model.links.len() - 1 + }; + // A limit that is not finite (a hand-edited file) is no limit. + let limit = + j.limits.get(dof).copied().flatten().filter(|(lo, hi)| lo.is_finite() && hi.is_finite()).map(|(lo, hi)| (lo.min(hi), lo.max(hi))); + let limit = match (step, limit) { + (Step::Prismatic, None) => { + self.warnings.push(format!("joint `{}` slides without a limit: written as ±1000 m (URDF needs one)", j.name)); + Some((-UNLIMITED_TRAVEL_MM, UNLIMITED_TRAVEL_MM)) + } + (_, l) => l, + }; + let name = if k == last { joint_name.clone() } else { self.names.take(&format!("{}_dof{}", j.name, k + 1)) }; + self.model.joints.push(UrdfJoint { + name, + parent: from, + child: to, + origin: if k == 0 { origin } else { IDENTITY }, + step, + axis, + limit, + source: Some((j.id, dof)), + }); + from = to; + } + Some(()) + } + + /// Every other placement of a nested occurrence (its parent component is placed several + /// times): a copy fixed to its parent placement's link. + fn copies(&mut self, jointed: &HashSet) { + let mut copies: BTreeMap, (usize, Mat)> = BTreeMap::new(); + let placements = self.doc.placements(); + if placements.len() > MAX_PLACEMENTS || placements.iter().any(|p| p.1.len() > MAX_PLACEMENT_DEPTH) { + self.warnings.push(format!( + "the assembly is deeper than {MAX_PLACEMENT_DEPTH} levels or has more than {MAX_PLACEMENTS} placements: copies past that are left out" + )); + } + for (_, path, w) in placements { + let Some((&occ, parent_path)) = path.split_last() else { continue }; + if path == canonical(self.doc, occ) { + continue; + } + let parent = match parent_path.last() { + None => Some((0, IDENTITY)), + Some(&p) if parent_path == canonical(self.doc, p).as_slice() => self.placed.get(&p).copied(), + Some(_) => copies.get(parent_path).copied(), + }; + let (Some((pi, pf)), Some(o)) = (parent, self.occurrence(occ)) else { continue }; + let Some(origin) = mat_inverse(&pf).map(|inv| mat_mul(&inv, &w)) else { continue }; + let Some(i) = self.link(o, path.clone(), &w, &w) else { continue }; + self.fixed(pi, i, origin); + copies.insert(path, (i, w)); + if jointed.contains(&occ) + && let Some(l) = self.model.links.get(i) + { + self.warnings.push(format!( + "`{}` is a copy of a jointed occurrence: fixed where it is (the URDF moves only the first placement; copies would need )", + l.name + )); + } + } + } +} + +/// Build the URDF of a design: `doc` with its occurrences placed (pending moves applied), `st` +/// with the bodies in their components' own frames, painted. `stem` names the robot and +/// prefixes the mesh files. +pub fn urdf(doc: &Document, st: &ModelState, stem: &str, options: &UrdfOptions) -> Result { + // XML 1.0 has no way to write these, escaped or not. + let unwritable = |c: char| (c.is_control() && !matches!(c, '\t' | '\n' | '\r')) || matches!(c, '\u{FFFE}' | '\u{FFFF}'); + if options.mesh_prefix.len() > 4096 { + return Err(IoError::Invalid("`mesh_prefix` is longer than 4096 bytes".into())); + } + if options.mesh_prefix.chars().any(unwritable) { + return Err(IoError::Invalid("`mesh_prefix` has a character XML cannot hold".into())); + } + let mut names = Names::default(); + let mut out = UrdfPackage { xml: String::new(), meshes: Vec::new(), model: UrdfModel::default(), warnings: Vec::new() }; + let base = names.take("base_link"); + let robot = sanitise(stem, false); + let parts = parts(doc, st, stem, &mut names, &mut out)?; + let mut tree = Tree { + doc, + parts, + names, + model: UrdfModel::default(), + warnings: std::mem::take(&mut out.warnings), + placed: BTreeMap::from([(0, (0, IDENTITY))]), + joint_names: BTreeMap::new(), + }; + let root = tree.parts.get(&0); + let base_link = Link { + name: base, + path: Vec::new(), + mesh_origin: IDENTITY, + visual: root.and_then(|p| p.visual.clone()), + collision: root.and_then(|p| p.collision.clone()), + color: root.and_then(|p| p.color), + inertial: root.and_then(|p| inertial(&p.masses, &IDENTITY)), + }; + tree.model.links.push(base_link); + + // Usable joints: unsuppressed (as many as the solver reads), both ends placed and distinct, + // both origins resolving. + let exists = |occ: u64| occ == 0 || doc.occurrences.iter().any(|o| o.id == occ); + let mut usable: Vec<(&Joint, Mat)> = Vec::new(); + for j in doc.assembly.joints.iter().filter(|j| !j.suppressed).take(joints::MAX_JOINTS) { + let fa = joints::resolve_origin(doc, st, &j.a).ok().filter(|_| joints::resolve_origin(doc, st, &j.b).is_ok()); + match fa { + Some(fa) if exists(j.a.occurrence) && exists(j.b.occurrence) && j.a.occurrence != j.b.occurrence => usable.push((j, fa)), + _ => tree.warnings.push(format!("joint `{}` is skipped: its origins don't resolve or it joins an occurrence to itself", j.name)), + } + } + let jointed: HashSet = usable.iter().flat_map(|(j, _)| [j.a.occurrence, j.b.occurrence]).collect(); + for (j, _) in &usable { + let name = tree.names.take(&j.name); + tree.joint_names.insert(j.id, name); + } + + // Grounded occurrences stay where they are in the world, nested or not. + let mut queue = VecDeque::from([0]); + // In id order, as the solver's queue, so both keep the same joint where two could reach a part. + let mut grounded: Vec = doc.occurrences.iter().filter(|o| o.grounded).map(|o| o.id).collect(); + grounded.sort_unstable(); + for id in grounded { + if tree.fix(id, true).is_some() { + queue.push_back(id); + } + } + let mut taken = HashSet::new(); + let mut left_out: HashSet = HashSet::new(); + loop { + tree.traverse(&mut queue, &usable, &mut taken); + let done = |t: &Tree, occ: u64| t.placed.contains_key(&occ) || left_out.contains(&occ); + let ready = |t: &Tree, occ: u64| t.occurrence(occ).is_some_and(|o| t.parent_link(o, false).is_some()); + // A floating group: anchored at its first joint's A side, as the solver anchors one + // when nothing is grounded; then what no joint reaches, parents first. + let next = usable + .iter() + .find(|(j, _)| !done(&tree, j.a.occurrence) && !done(&tree, j.b.occurrence) && ready(&tree, j.a.occurrence)) + .map(|(j, _)| (j.a.occurrence, false)) + .or_else(|| doc.occurrences.iter().find(|o| !done(&tree, o.id) && ready(&tree, o.id)).map(|o| (o.id, false))) + .or_else(|| doc.occurrences.iter().find(|o| !done(&tree, o.id)).map(|o| (o.id, true))); + let Some((occ, to_base)) = next else { break }; + let Some(i) = tree.fix(occ, to_base) else { + tree.warnings.push(format!("occurrence {occ} has a degenerate placement: left out")); + left_out.insert(occ); + continue; + }; + queue.push_back(occ); + let nested = tree.occurrence(occ).is_some_and(|o| o.parent != 0); + let name = tree.model.links.get(i).map(|l| l.name.clone()).unwrap_or_default(); + if jointed.contains(&occ) { + tree.warnings.push(format!("`{name}` is not grounded: fixed where it is")); + } else if !nested || to_base { + tree.warnings.push(format!("`{name}` has no joint: fixed where it is")); + } + } + tree.copies(&jointed); + + let moving = tree.model.joints.iter().any(|j| j.step != Step::Fixed); + if moving && (options.effort.is_none() || options.velocity.is_none()) { + tree.warnings.push(format!( + "joint effort and velocity limits are not given: written as {DEFAULT_EFFORT} and {DEFAULT_VELOCITY} (pass effort and velocity)" + )); + } + out.xml = write_xml(&tree.model, &robot, options); + out.model = tree.model; + out.warnings = tree.warnings; + Ok(out) +} + +fn token() -> Inertial { + Inertial { mass: TOKEN_MASS, com: Vec3::ZERO, inertia: [[TOKEN_INERTIA, 0.0, 0.0], [0.0, TOKEN_INERTIA, 0.0], [0.0, 0.0, TOKEN_INERTIA]] } +} + +/// Bodies' masses (component frame) combined in the link frame (`to_link`: component to link). +fn inertial(masses: &[BodyMass], to_link: &Mat) -> Option { + let mass: f64 = masses.iter().map(|m| m.0).sum(); + if !(mass > 0.0) { + return None; + } + let com = masses.iter().fold(Vec3::ZERO, |acc, m| acc + apply_point(to_link, m.1) * m.0) * (1.0 / mass); + let r = |i: usize, j: usize| to_link.get(j).and_then(|c| c.get(i)).copied().unwrap_or(0.0); + let mut total = [[0.0f64; 3]; 3]; + for (m, c, inertia) in masses { + // R I Rᵀ, then the parallel-axis term about the link's centre of mass. + let d = apply_point(to_link, *c) - com; + let dv = [d.x, d.y, d.z]; + let d2 = d.dot(d); + for (i, (row_out, di)) in total.iter_mut().zip(dv).enumerate() { + for (j, (cell, dj)) in row_out.iter_mut().zip(dv).enumerate() { + let mut v = 0.0; + for (a, row) in inertia.iter().enumerate() { + for (b, x) in row.iter().enumerate() { + v += r(i, a) * x * r(j, b); + } + } + *cell += v + m * (if i == j { d2 } else { 0.0 } - di * dj); + } + } + } + // kg mm² to kg m². + let inertia = total.map(|row| row.map(|x| x * 1e-6)); + let finite = mass.is_finite() && [com.x, com.y, com.z].iter().all(|x| x.is_finite()) && inertia.iter().flatten().all(|x| x.is_finite()); + finite.then_some(Inertial { mass, com, inertia }) +} + +fn append(into: &mut Mesh, m: &Mesh) { + let base = u32::try_from(into.positions.len()).unwrap_or(u32::MAX); + into.positions.extend_from_slice(&m.positions); + into.triangles.extend(m.triangles.iter().map(|t| t.map(|i| i.saturating_add(base)))); +} + +fn metres(m: &Mesh) -> Mesh { + Mesh { positions: m.positions.iter().map(|p| *p * 1e-3).collect(), triangles: m.triangles.clone(), ..Mesh::default() } +} + +/// `xyz` (m) and `rpy` (rad, fixed axes: R = Rz(yaw) Ry(pitch) Rx(roll)) of a frame. +fn xyz_rpy(m: &Mat) -> (Vec3, Vec3) { + let r = |row: usize, col: usize| m.get(col).and_then(|c| c.get(row)).copied().unwrap_or(0.0); + // atan2 over hypot keeps pitch precise next to +-90 degrees, where asin would not. + let cos_pitch = r(0, 0).hypot(r(1, 0)); + let pitch = (-r(2, 0)).atan2(cos_pitch); + let (roll, yaw) = if cos_pitch > 1e-12 { + (r(2, 1).atan2(r(2, 2)), r(1, 0).atan2(r(0, 0))) + } else { + // Gimbal lock: yaw carries the whole turn about z. + (0.0, (-r(0, 1)).atan2(r(1, 1))) + }; + let t = apply_point(m, Vec3::ZERO) * 1e-3; + (t, Vec3::new(roll, pitch, yaw)) +} + +fn attr(s: &str) -> String { + quick_xml::escape::escape(s).into_owned() +} + +fn num(x: f64) -> String { + // No "-0" and no non-finite values in the XML. Shortest round-trip digits either way; very + // small or large values with an exponent rather than a run of zeros. + if !x.is_finite() || x == 0.0 { + "0".into() + } else if x.abs() < 1e-3 || x.abs() >= 1e7 { + format!("{x:e}") + } else { + format!("{x}") + } +} + +fn triple(v: Vec3) -> String { + format!("{} {} {}", num(v.x), num(v.y), num(v.z)) +} + +fn origin_xml(m: &Mat) -> String { + let (xyz, rpy) = xyz_rpy(m); + format!("", triple(xyz), triple(rpy)) +} + +fn write_xml(model: &UrdfModel, name: &str, options: &UrdfOptions) -> String { + let effort = options.effort.filter(|x| x.is_finite() && *x > 0.0).unwrap_or(DEFAULT_EFFORT); + let velocity = options.velocity.filter(|x| x.is_finite() && *x > 0.0).unwrap_or(DEFAULT_VELOCITY); + let prefix = attr(&options.mesh_prefix); + let mut s = String::from("\n\n"); + s += &format!("\n", attr(name)); + for l in &model.links { + s += &format!(" \n", attr(&l.name)); + if let Some(i) = &l.inertial { + let c = i.com * 1e-3; + let t = &i.inertia; + s += &format!( + " \n \n \n \n \n", + triple(c), + num(i.mass), + num(t[0][0]), + num(t[0][1]), + num(t[0][2]), + num(t[1][1]), + num(t[1][2]), + num(t[2][2]) + ); + } + if let Some(file) = &l.visual { + s += &format!( + " \n {}\n \n", + origin_xml(&l.mesh_origin), + attr(file) + ); + if let Some([r, g, b, a]) = l.color { + s += &format!( + " \n", + attr(&l.name), + num(r as f64), + num(g as f64), + num(b as f64), + num(a as f64) + ); + } + s += " \n"; + } + if let Some(file) = &l.collision { + s += &format!( + " \n {}\n \n \n", + origin_xml(&l.mesh_origin), + attr(file) + ); + } + s += " \n"; + } + for j in &model.joints { + let (Some(parent), Some(child)) = (model.links.get(j.parent), model.links.get(j.child)) else { continue }; + let kind = match (j.step, j.limit) { + (Step::Fixed, _) => "fixed", + (Step::Revolute, None) => "continuous", + (Step::Revolute, Some(_)) => "revolute", + (Step::Prismatic, _) => "prismatic", + }; + s += &format!(" \n", attr(&j.name)); + s += &format!(" \n \n {}\n", attr(&parent.name), attr(&child.name), origin_xml(&j.origin)); + if j.step != Step::Fixed { + s += &format!(" \n", triple(j.axis)); + let scale = if j.step == Step::Prismatic { 1e-3 } else { 1.0 }; + match j.limit { + Some((lo, hi)) => { + s += &format!( + " \n", + num(lo * scale), + num(hi * scale), + num(effort), + num(velocity) + ); + } + None => s += &format!(" \n", num(effort), num(velocity)), + } + } + s += " \n"; + } + s += "\n"; + s +} + +#[cfg(test)] +mod tests { + use super::*; + + use std::f64::consts::FRAC_PI_2; + + use quick_xml::events::Event; + + /// Rotations that stress the decomposition: general ones, gimbal lock (pitch ±90°, which a + /// hinge whose axis lies along x gives) and next to it. + fn hard_rotations() -> Vec { + let r = |axis: Vec3, a: f64| rotation(axis.normalized().unwrap_or(Vec3::Z), a); + let mut out = vec![r(Vec3::X, 0.3), r(Vec3::Y, -1.1), r(Vec3::Z, 2.5), r(Vec3::new(1.0, 2.0, 3.0), 0.7)]; + for pitch in [FRAC_PI_2, -FRAC_PI_2, FRAC_PI_2 - 1e-7, -FRAC_PI_2 + 1e-9] { + for (yaw, roll) in [(0.0, 0.0), (0.7, 0.3), (-2.0, 1.4)] { + out.push(mat_mul(&mat_mul(&r(Vec3::Z, yaw), &r(Vec3::Y, pitch)), &r(Vec3::X, roll))); + } + } + out + } + + /// The frame an `` stands for (xyz in m, the frame in mm). + fn compose(xyz: Vec3, rpy: Vec3) -> Mat { + mat_mul(&mat_mul(&mat_mul(&translation(xyz * 1e3), &rotation(Vec3::Z, rpy.z)), &rotation(Vec3::Y, rpy.y)), &rotation(Vec3::X, rpy.x)) + } + + fn close(a: &Mat, b: &Mat) -> bool { + a.iter().flatten().zip(b.iter().flatten()).all(|(x, y)| (x - y).abs() < 1e-9) + } + + #[test] + fn rpy_round_trips_a_rotation() { + for (k, mut m) in hard_rotations().into_iter().enumerate() { + m[3] = [10.0, -20.0, 30.0, 1.0]; + let (xyz, rpy) = xyz_rpy(&m); + assert!(close(&compose(xyz, rpy), &m), "{k}: {rpy:?}"); + } + } + + /// Every `` in the file parses back to the model's frame, so the forward kinematics + /// the engine tests check on [`UrdfModel`] are the ones a URDF reader gets. + #[test] + fn written_origins_parse_back_to_the_model() { + let link = |name: String, mesh_origin: Mat| Link { + name, + path: Vec::new(), + mesh_origin, + visual: Some("m.stl".into()), + collision: Some("m.stl".into()), + color: None, + inertial: None, + }; + let mut model = UrdfModel { links: vec![link("base_link".into(), IDENTITY)], joints: Vec::new() }; + for (k, mut m) in hard_rotations().into_iter().enumerate() { + m[3] = [k as f64, 2.0, -3.5, 1.0]; + model.links.push(link(format!("l{k}"), m)); + let step = if k % 2 == 0 { Step::Revolute } else { Step::Prismatic }; + model.joints.push(UrdfJoint { + name: format!("j{k}"), + parent: 0, + child: k + 1, + origin: m, + step, + axis: -Vec3::Y, + limit: Some((-1.0, 2.0)), + source: None, + }); + } + let xml = write_xml(&model, "r", &UrdfOptions::default()); + let mut reader = quick_xml::Reader::from_str(&xml); + let mut origins = Vec::new(); + loop { + match reader.read_event() { + Ok(Event::Empty(e)) if e.local_name().as_ref() == b"origin" => { + let triple = |key: &[u8]| -> Vec3 { + let v: Vec = e + .attributes() + .flatten() + .find(|a| a.key.local_name().as_ref() == key) + .map(|a| String::from_utf8_lossy(&a.value).split_whitespace().filter_map(|x| x.parse().ok()).collect()) + .unwrap_or_default(); + Vec3::new( + v.first().copied().unwrap_or(f64::NAN), + v.get(1).copied().unwrap_or(f64::NAN), + v.get(2).copied().unwrap_or(f64::NAN), + ) + }; + origins.push(compose(triple(b"xyz"), triple(b"rpy"))); + } + Ok(Event::Eof) => break, + Err(e) => panic!("{e}"), + _ => {} + } + } + // Each link writes its visual's and collision's origin, then each joint its own. + let want: Vec = model.links.iter().flat_map(|l| [l.mesh_origin, l.mesh_origin]).chain(model.joints.iter().map(|j| j.origin)).collect(); + assert_eq!(origins.len(), want.len()); + for (k, (got, want)) in origins.iter().zip(&want).enumerate() { + assert!(close(got, want), "origin {k}: {got:?} vs {want:?}"); + } + } + + /// XML cannot hold a control character, escaped or not: the prefix is refused here, for + /// every caller. + #[test] + fn a_mesh_prefix_xml_cannot_hold_is_refused() { + let doc = Document::new("t"); + let st = ModelState::default(); + for (prefix, why) in [("a\u{1}b".to_string(), "character"), ("a".repeat(5000), "longer than")] { + let options = UrdfOptions { mesh_prefix: prefix, ..UrdfOptions::default() }; + let e = urdf(&doc, &st, "t", &options).err().map(|e| e.to_string()).unwrap_or_default(); + assert!(e.contains("mesh_prefix") && e.contains(why), "{e}"); + } + assert!(urdf(&doc, &st, "t", &UrdfOptions { mesh_prefix: "package://r/m/".into(), ..UrdfOptions::default() }).is_ok()); + } + + #[test] + fn numbers_are_short_and_exact() { + assert_eq!(num(0.01), "0.01"); + assert_eq!(num(-1000.0), "-1000"); + assert_eq!(num(2.1500000000000004e-8), "2.1500000000000004e-8"); + assert_eq!(num(-8.881784197001253e-19), "-8.881784197001253e-19"); + assert_eq!(num(-0.0), "0"); + assert_eq!(num(f64::NAN), "0"); + for x in [0.01, 1e-3, 9.99e-4, 123456.789, 3.0e12, -2.5e-7] { + assert_eq!(num(x).parse::().ok(), Some(x), "{x}"); + } + } + + #[test] + fn names_are_unique_and_sanitised() { + let mut n = Names::default(); + assert_eq!(n.take("base_link"), "base_link"); + assert_eq!(n.take("Leaf A:1"), "Leaf_A_1"); + assert_eq!(n.take("Leaf_A_1"), "Leaf_A_1_2"); + assert_eq!(n.take(""), "part"); + // File systems that ignore case would make these one file. + assert_eq!(n.file("t-base"), "t-base.stl"); + assert_eq!(n.file("t-Base"), "t-Base_2.stl"); + } +} diff --git a/crates/mcp/src/tests.rs b/crates/mcp/src/tests.rs index 24aa1e76..23ee10ab 100644 --- a/crates/mcp/src/tests.rs +++ b/crates/mcp/src/tests.rs @@ -103,6 +103,24 @@ fn handshake_and_tools_list() { assert_eq!(call(&mut s, 8, "nope/nope", json!({}))["error"]["code"], -32601); } +/// A URDF goes through `export` with its own options (#719). +#[test] +fn export_tool_writes_a_urdf_package() { + let mut s = server(); + tool(&mut s, "execute", json!({"command": "solid.box", "params": {"length": 10, "width": 10, "height": 10}})); + let dir = std::env::temp_dir().join(format!("solidcraft-mcp-urdf-{}", std::process::id())); + std::fs::create_dir_all(&dir).unwrap(); + let urdf = dir.join("robot.urdf"); + let r = tool(&mut s, "export", json!({"path": urdf.to_string_lossy(), "mesh_prefix": "package://robot/meshes/"})); + assert_eq!(r["format"], "Urdf", "{r}"); + assert!(std::fs::read_to_string(&urdf).unwrap().contains(r#"filename="package://robot/meshes/robot-base.stl""#)); + assert!(dir.join("robot-base.stl").exists()); + // effort and velocity reach file.export, which refuses a bad one. + let r = raw_tool(&mut s, "export", json!({"path": urdf.to_string_lossy(), "effort": -1})); + assert_eq!(r["result"]["isError"], json!(true), "{r}"); + let _ = std::fs::remove_dir_all(&dir); +} + /// Box → fillet → hole in one batch, then verify with measure / inspect_design and look at it. #[test] fn agent_builds_box_fillet_hole() { diff --git a/crates/mcp/src/tools.rs b/crates/mcp/src/tools.rs index 92466d88..99cce369 100644 --- a/crates/mcp/src/tools.rs +++ b/crates/mcp/src/tools.rs @@ -190,12 +190,17 @@ pub fn tool_definitions() -> Vec { "export", "Export", "Export bodies to STEP (.step/.stp), 3MF (.3mf), binary STL (.stl), ASCII STL (format stla) or OBJ. The format \ - comes from the extension unless given.", + comes from the extension unless given. URDF (.urdf) exports the whole assembly for robot simulators: a link per \ + component placement with mass and inertia, joints from the assembly's joints, and STL meshes in metres written \ + next to it (docs/urdf.md); RViz needs mesh_prefix such as package://robot/meshes/.", obj( json!({ "path": string("Output file"), - "format": {"type": "string", "enum": ["step", "3mf", "3mf-tagged", "stl", "stla", "obj"]}, - "bodies": {"type": "array", "items": {"type": "string"}, "description": "Body names (default: all)"}, + "format": {"type": "string", "enum": ["step", "3mf", "3mf-tagged", "stl", "stla", "obj", "urdf"]}, + "bodies": {"type": "array", "items": {"type": "string"}, "description": "Body names (default: all; not with urdf)"}, + "mesh_prefix": string("urdf: put before each mesh file name (default: none, so relative to the .urdf)"), + "effort": {"type": "number", "description": "urdf: joint effort limit, N or N m (default 1000)"}, + "velocity": {"type": "number", "description": "urdf: joint velocity limit, m/s or rad/s (default 10)"}, }), &["path"], ), @@ -387,7 +392,7 @@ fn dispatch(b: &mut dyn Backend, name: &str, a: &Map) -> Result exec(b, "file.export", pick(a, &["path", "format", "bodies"])), + "export" => exec(b, "file.export", pick(a, &["path", "format", "bodies", "mesh_prefix", "effort", "velocity"])), "crash_reports" => match a.get("id") { Some(_) => exec(b, "diagnostics.crash_report", pick(a, &["id"])), None => exec(b, "diagnostics.crash_reports", pick(a, &["limit"])), diff --git a/crates/ui-egui/src/toolbar.rs b/crates/ui-egui/src/toolbar.rs index df727350..889a90c0 100644 --- a/crates/ui-egui/src/toolbar.rs +++ b/crates/ui-egui/src/toolbar.rs @@ -436,6 +436,10 @@ fn file_menu(app: &mut SolveApp, ctx: &egui::Context) { export(app, "obj"); close = true; } + if item_if(ui, "Export URDF…", "", shown) { + export(app, "urdf"); + close = true; + } if item(ui, "Export Design as JSON…", "") { export(app, "json"); close = true; @@ -1452,7 +1456,8 @@ mod home_tests { let name = if cmd { format!("Ctrl+{key}") } else { key.to_string() }; actions.push((name, Box::new(move |h| drop(h.call("ui.key", json!({"key": key, "cmd": cmd})))))); } - for item in ["Insert STEP…", "Insert Mesh…", "Save", "Save As…", "Export STL…", "Export STEP…", "Export 3MF…", "Export OBJ…"] { + for item in ["Insert STEP…", "Insert Mesh…", "Save", "Save As…", "Export STL…", "Export STEP…", "Export 3MF…", "Export OBJ…", "Export URDF…"] + { actions.push(( format!("File › {item}"), Box::new(move |h| { @@ -1489,6 +1494,22 @@ mod home_tests { assert!(h.app.session.path.is_some() && !h.app.session.is_dirty(), "the design saves once Home is closed"); } + /// File › Export URDF… writes the robot description and its meshes next to the picked file. + #[test] + fn file_menu_exports_a_urdf() { + let mut h = crate::scenario::Harness::new(); + h.app.home.open = false; + h.app.run("solid.box", json!({"length": 10, "width": 10, "height": 10})).unwrap(); + h.frames(2); + h.ctx.data_mut(|d| d.insert_temp(egui::Id::new("sc_file_menu"), false)); + let at = crate::scenario::handle_at("appbar:folder").unwrap(); + h.call("ui.click", json!({"x": at.x, "y": at.y})); + let _ = h.step(&json!({"widget": "Export URDF…"})); + h.settle(); + let files: Vec = std::fs::read_dir(&h.dir).unwrap().map(|e| e.unwrap().file_name().to_string_lossy().into_owned()).collect(); + assert!(files.iter().any(|f| f.ends_with(".urdf")) && files.iter().any(|f| f.ends_with("-base.stl")), "{files:?}"); + } + /// The disabled buttons' tooltip is translated in every catalog (not English fallback). #[test] fn needs_design_tooltip_is_translated() { diff --git a/docs/book/src/guide/files.md b/docs/book/src/guide/files.md index 71d78905..b0f628dc 100644 --- a/docs/book/src/guide/files.md +++ b/docs/book/src/guide/files.md @@ -8,6 +8,7 @@ | IGES 5.3 | manifold solids, or trimmed surfaces sewn into solids | manifold solids with names and colours | | 3MF, STL, OBJ | mesh bodies | yes | | 3MF with face tags | as 3MF | each triangle tagged with its B-rep face, exact coordinates | +| URDF | | the assembly as a robot: links with meshes, mass and inertia, joints from the assembly's joints | | DXF, SVG | into a sketch | sketches and flat patterns as DXF | | PNG, JPEG | canvases and decals | | @@ -35,6 +36,11 @@ is the tessellation's own double-precision value. Meshing and analysis tools can keep a model's faces (for loads, boundary conditions or feature edges); other 3MF readers ignore them. The format is specified in [`docs/formats/3mf-face-tags.md`](https://github.com/bherbruck/solidcraft/blob/main/docs/formats/3mf-face-tags.md). +**File → Export URDF…** (`file.export` with a `.urdf` path) writes the assembly for robot +simulators and ROS: a link per component placement with its mesh, mass and inertia, and the +assembly's joints, so the parts move in the simulator as they do in SolidCraft. The meshes are +written as STL files next to the `.urdf`. See [`docs/urdf.md`](https://github.com/bherbruck/solidcraft/blob/main/docs/urdf.md). + Saves are atomic: a crash at any moment leaves either the old file or the new one, and the replaced version is kept as `.bak`. A save keeps the file's permissions, and a design opened through a symbolic link (a synced folder linked into a project) is saved into the file the diff --git a/docs/mcp.md b/docs/mcp.md index 317eb642..94c1188c 100644 --- a/docs/mcp.md +++ b/docs/mcp.md @@ -124,7 +124,7 @@ They cover the workflow, the command shapes, how to verify with `measure`, and t | `body_topology` | `body` | Edges (index, midpoint, length, ends) and faces (index, area, centroid, normal). An edge midpoint is the `[x,y,z]` that picks that edge for fillet and chamfer. | | `set_parameter` | `name`, `value` (number or expression), `unit?`, `comment?` | Create or change a user parameter and recompute; reports `recomputed` (features rebuilt), `restored` (features whose earlier result for the same inputs was reused, e.g. a value set back) and features that now fail. | | `screenshot` | `view?` (iso, top, front, back, bottom, left, right, home, fit, or a named view saved with `view.save`), `camera?` (`{target, yaw, pitch, distance, fov?}`, as `view.apply`), `path?`, `width?`, `height?`, `source?` (window, model) | A PNG image content block. Headless: CPU render fitted to the model, or from the named view or camera. Bridged: the app window (default) or a model render with the app's camera; a named view or camera moves the app's viewport there first (`execute` `view.apply` does the same without a screenshot). | -| `export` | `path`, `format?` (step, 3mf, 3mf-tagged, stl, stla, obj), `bodies?` | STEP, 3MF or mesh export. | +| `export` | `path`, `format?` (step, 3mf, 3mf-tagged, stl, stla, obj, urdf), `bodies?`, `mesh_prefix?`, `effort?`, `velocity?` | STEP, 3MF or mesh export; URDF writes the whole assembly with its meshes next to it (`docs/urdf.md`). | | `undo`, `redo` | — | Undo / redo one change. | | `crash_reports` | `id?`, `limit?` | The crash reports on this computer, newest first; with `id`, the whole report (backtrace, command, recent commands, `design_path`/`replay_path`). See [crash-reports.md](crash-reports.md). | | `new_design` | `name?` | Start an empty design. | diff --git a/docs/urdf.md b/docs/urdf.md new file mode 100644 index 00000000..9f855712 --- /dev/null +++ b/docs/urdf.md @@ -0,0 +1,115 @@ +# URDF export + +A design with components and joints exports as a robot description in URDF (Unified Robot +Description Format), the format ROS, Gazebo, Isaac Sim, PyBullet and MuJoCo read. Each part +becomes a link with its mesh, mass and inertia. Each joint becomes a URDF joint, or a short +chain of them, that moves the parts the way SolidCraft does. + +- **Desktop:** **File → Export URDF…** +- **Commands:** `file.export {"path": "robot.urdf"}` (or `"format": "urdf"`) +- **MCP:** the `export` tool with a `.urdf` path or `format: "urdf"` +- **CLI:** `solidcraft-cli run script.json --out robot.urdf` + +URDF always exports the whole assembly, so `bodies` is refused. + +## Files + +Next to `robot.urdf` the export writes one binary STL per component, in metres: + +- `robot-.stl` holds all of the component's bodies. The root component's bodies go + in `robot-base.stl`. +- `robot--collision.stl` appears only when a component mixes closed solids with + open surfaces: it holds the closed ones, which are the collision geometry. + +Every path is checked before anything is written, including against the folders an agent may +use (`allow_paths`). The meshes are written first and the `.urdf` last. If a write fails part +way, the error names the files already written and the one that failed. + +## Parameters + +| Parameter | Default | Meaning | +|---|---|---| +| `mesh_prefix` | none | Put before each mesh file name. Leave it empty for loaders that resolve against the URDF's folder (Isaac Sim, PyBullet, MuJoCo, yourdfpy). RViz and other ROS tools need `package:///meshes/` or an absolute folder. | +| `effort` | 1000 | Joint effort limit, in N (sliding) or N m (turning). | +| `velocity` | 10 | Joint velocity limit, in m/s or rad/s. | + +URDF requires `effort` and `velocity` on every moving joint. When they are not given, the +defaults are written and a warning says so. A value that is not a positive number is refused. + +## Links + +- **`base_link`** holds the root component's bodies and stays fixed in the world. Every + grounded occurrence hangs on it by a `fixed` joint at its current place. +- **One link per placement of a component.** A component placed once gives its name to the + link (`Gripper`). One placed several times gives the occurrence names (`Finger:1` becomes + `Finger_1`). +- **Names** use only `A-Z a-z 0-9 _` (mesh files also keep `-`). Links, joints and files share + one set of names, because Gazebo refuses a link and a joint with the same name. A name + already taken gets `_2`, `_3` and so on. Joint names are taken first, so a joint keeps the + name you gave it. Mesh file names are compared without case, since Windows and macOS ignore + it. +- **Mass and inertia** come from each body's mesh and material density: the mass in kg, the + centre of mass and the inertia tensor about it in the link frame, in kg m². Bodies of the + same link combine with the parallel-axis theorem. +- **Open surfaces** are drawn but add no mass or collision, with a warning naming them. +- **A link with no mass** (only surfaces, no bodies, or a chain's in-between link) gets a token + mass of 1e-6 kg and inertia of 1e-12 kg m², because Gazebo drops massless links and breaks + the chain there. +- **Colour:** the first body's colour, as the visual's material. + +## Joints + +| SolidCraft joint | URDF | +|---|---| +| Rigid | `fixed` | +| Revolute | `revolute` about z with limits, or `continuous` without | +| Slider | `prismatic` along z | +| Cylindrical | `revolute` z, then `prismatic` z | +| Pin-slot | `prismatic` x, then `revolute` z | +| Planar | `prismatic` x, `prismatic` y, then `revolute` z | +| Ball | `revolute` z, `revolute` y, then `revolute` z | + +- **Chains:** a joint with several values becomes one URDF joint per value. Between them sit + links named `_link1`, `_link2`. The steps before the last are named + `_dof1`, `_dof2`, and the last one keeps the joint's name. +- **Values:** each URDF joint value equals the SolidCraft value it comes from: the same angle + (in radians) or distance (in metres). This holds whichever side of the joint is nearer the + base. When the tree reaches a joint from its B side, the chain runs backwards with its axes + reversed, so the values still match. +- **Limits** are written as `lower <= upper` in radians or metres. A sliding step without a + limit gets ±1000 m, since URDF requires one, with a warning. +- **Motion links** are not written as ``. Each joint's own value is free in the URDF. + +The URDF is written at the design's current joint values, with every part where it is shown, +including moves not captured yet. The exception is a part the joints place: it exports where its +joints put it, since they put it back as soon as one moves, and a warning says so. Moving a joint in a simulator puts +the parts the joint solver places where SolidCraft's **Drive Joints** would. + +## The tree + +URDF is a tree hanging from `base_link`. The export builds it the way the joint solver walks +the assembly. + +1. From `base_link` and the grounded occurrences, it follows the joints outwards. +2. A group of parts joined to each other but to nothing grounded is fixed where it is by the + A side of its first joint, as the solver does when nothing is grounded. A warning says the + part is not grounded. +3. A part no joint reaches is fixed where it is. A nested one hangs on its parent part, so it + moves with it, as in SolidCraft. A top-level one hangs on `base_link`, with a warning. +4. A joint that would close a loop is left out, with a warning, since a tree cannot hold it. +5. A joint whose snap points no longer resolve (for example, the face it was on is gone) is + left out, with a warning naming it. + +When a component is placed several times, what is nested in it appears in each placement. In +the URDF the first placement is the one its joints move; the others are copies fixed in their +parent at their current pose, with a warning when the copied part has a joint. (In SolidCraft +the copies move together, which a URDF could only express with ``.) The export stops copying past +64 levels or 100 000 placements, with a warning. + +## Not covered + +- `` for motion links. +- Simplified collision shapes: the collision mesh is the visual mesh. +- SDF and MJCF export, and URDF import. +- Joint motion inside the copies of a component placed several times. +- Drive dynamics: `effort` and `velocity` are limits you choose. diff --git a/skills/solidcraft-modeling/SKILL.md b/skills/solidcraft-modeling/SKILL.md index fcf2b0ea..13cae196 100644 --- a/skills/solidcraft-modeling/SKILL.md +++ b/skills/solidcraft-modeling/SKILL.md @@ -56,7 +56,7 @@ Work the part out as numbers first. Then build it in a few large `batch` calls, - When the server is bridged, `source` defaults to the whole app window and `view` also turns the user's camera, so pass `"source":"model"` and leave out `view` unless the user wants the camera moved. ## Finish -`export {path}` writes `.step` for CAD or `.3mf`/`.stl` for printing. `save {path}` writes `.solidcraft`. +`export {path}` writes `.step` for CAD, `.3mf`/`.stl` for printing, or `.urdf` (an assembly with joints, for robot simulators). `save {path}` writes `.solidcraft`. ## Red flags