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.
This commit is contained in:
Mateusz Sych authored and GitHub committed 2026-10-13 21:52:30 -04:00
1 parent d2e409f40c
commit 17db109c5d
19 files changed
+1993 -17

No files matched your search

+2 -1
View File
@@ -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 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 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), 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 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 need B-rep bodies). Convert Mesh reconstructs supported closed constant-section parts
as parametric solids with planar and circular cylindrical faces; as parametric solids with planar and circular cylindrical faces;
+1 -1
View File
@@ -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 | | 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 | | 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 | — | | 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 | | 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 | — | | 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 | | 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 |
+10 -1
View File
@@ -124,6 +124,10 @@ fn identity() -> Mat {
IDENTITY 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 { 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)); 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) 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). /// 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<u64>, Mat)> { pub fn placements(&self) -> Vec<(u64, Vec<u64>, Mat)> {
let mut out = vec![(0, Vec::new(), IDENTITY)]; let mut out = vec![(0, Vec::new(), IDENTITY)];
let mut stack: Vec<(u64, Vec<u64>, Mat)> = vec![(0, Vec::new(), IDENTITY)]; let mut stack: Vec<(u64, Vec<u64>, Mat)> = vec![(0, Vec::new(), IDENTITY)];
while let Some((comp, path, m)) = stack.pop() { while let Some((comp, path, m)) = stack.pop() {
if path.len() > 64 || out.len() > 100_000 { if out.len() > MAX_PLACEMENTS {
break; 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) { for o in self.occurrences.iter().filter(|o| o.parent == comp) {
let mut p = path.clone(); let mut p = path.clone();
p.push(o.id); p.push(o.id);
+2 -1
View File
@@ -17,7 +17,8 @@ use solidcraft_geom::Vec3;
use crate::{DocError, Document, IDENTITY, Mat, ModelState, Result, apply_point, apply_vector, mat_inverse, mat_mul}; 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). /// Where a joint origin sits on a component's geometry (component coordinates).
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)] #[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
+4 -1
View File
@@ -40,7 +40,10 @@ mod project3d;
pub use project3d::iso_grid; pub use project3d::iso_grid;
pub mod sheet; 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 document::*;
pub use eval::{ pub use eval::{
EdgeRef, FeatureResult, Model, ModelBody, ModelState, RevolveExtent, SolvedSketch, ThreadInfo, curve_pick_error, edge_names_for, edge_refs, EdgeRef, FeatureResult, Model, ModelBody, ModelState, RevolveExtent, SolvedSketch, ThreadInfo, curve_pick_error, edge_names_for, edge_refs,
+67 -2
View File
@@ -8,7 +8,7 @@ use solidcraft_doc::Document;
use solidcraft_io::Format; use solidcraft_io::Format;
use super::CommandSpec; 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}; use crate::{EngineError, Result, Session};
pub static COMMANDS: &[CommandSpec] = &[ pub static COMMANDS: &[CommandSpec] = &[
@@ -45,7 +45,7 @@ pub static COMMANDS: &[CommandSpec] = &[
CommandSpec::new("file.export", "Export", export) CommandSpec::new("file.export", "Export", export)
.icon("export") .icon("export")
.noundo() .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"), CommandSpec::new("file.save_mesh", "Save As Mesh", save_stl).icon("export").noundo().params("path; bodies?: [names]; ascii?: bool"),
]; ];
@@ -328,9 +328,70 @@ fn export_to(s: &Session, path: &str, format: Format, bodies: &[String]) -> Resu
fn export(s: &mut Session, p: &Value) -> Result<Value> { fn export(s: &mut Session, p: &Value) -> Result<Value> {
let path = path_arg(p, "file.export")?; let path = path_arg(p, "file.export")?;
let format = Format::from_name(str_(p, "format").unwrap_or(path))?; 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")) export_to(s, path, format, &string_list(p, "bodies"))
} }
/// The assembly as a URDF and its meshes next to it (docs/urdf.md). Every path is checked before
/// anything is written; the meshes go first and the `.urdf` last.
fn export_urdf(s: &Session, path: &str, p: &Value) -> Result<Value> {
let cmd = "file.export";
if !string_list(p, "bodies").is_empty() {
return Err(bad(cmd, "urdf exports the whole assembly; omit bodies"));
}
let positive = |k: &str| match p.get(k) {
None | Some(Value::Null) => Ok(None),
Some(_) => num(p, k).filter(|x| *x > 0.0).map(Some).ok_or_else(|| bad(cmd, format!("`{k}` must be a positive number"))),
};
// `io::urdf` refuses a prefix XML cannot hold.
let mesh_prefix = str_(p, "mesh_prefix").unwrap_or("").to_string();
let options = solidcraft_io::UrdfOptions { mesh_prefix, effort: positive("effort")?, velocity: positive("velocity")? };
let file = std::path::Path::new(path);
let stem = file.file_stem().map(|x| x.to_string_lossy().to_string()).unwrap_or_else(|| s.doc.name.clone());
// Occurrences where they are shown, uncaptured moves included, except those the joint solver
// places: it puts them back as soon as a joint moves, so the move must not become part of
// their links.
let st = s.model.state();
let mut held = Vec::new();
let doc = if s.pending_moves.is_empty() {
std::borrow::Cow::Borrowed(&*s.doc)
} else {
let placed = solidcraft_doc::joints::solve(&s.doc, &st).world;
let mut doc = (*s.doc).clone();
for o in &mut doc.occurrences {
match s.pending_moves.get(&o.id) {
Some(_) if placed.contains_key(&o.id) => held.push(o.name.clone()),
Some(t) => o.transform = *t,
None => {}
}
}
std::borrow::Cow::Owned(doc)
};
let mut pkg = solidcraft_io::urdf(&doc, &doc.painted(&st), &stem, &options)?;
for name in held {
pkg.warnings.push(format!("`{name}` was moved but the move is not captured: exported where its joints place it"));
}
let dir = file.parent().unwrap_or(std::path::Path::new(""));
let mut targets: Vec<(String, &[u8])> = pkg.meshes.iter().map(|(n, b)| (dir.join(n).to_string_lossy().to_string(), b.as_slice())).collect();
targets.push((path.to_string(), pkg.xml.as_bytes()));
for (t, _) in &targets {
solidcraft_io::vfs::check(t).map_err(|e| EngineError::Other(format!("{t}: {e}")))?;
}
let mut written: Vec<String> = Vec::new();
for (t, bytes) in &targets {
if let Err(e) = solidcraft_io::vfs::write(t, bytes) {
let before = if written.is_empty() { "none".to_string() } else { written.join(", ") };
return Err(EngineError::Other(format!("{t}: {e} (this file may be incomplete; written before it: {before})")));
}
written.push(t.clone());
}
Ok(
json!({"path": path, "format": "Urdf", "files": written, "links": pkg.model.links.len(), "joints": pkg.model.joints.len(), "warnings": pkg.warnings}),
)
}
fn save_stl(s: &mut Session, p: &Value) -> Result<Value> { fn save_stl(s: &mut Session, p: &Value) -> Result<Value> {
let path = path_arg(p, "file.save_mesh")?; let path = path_arg(p, "file.save_mesh")?;
let f = if bool_(p, "ascii").unwrap_or(false) { Format::StlAscii } else { Format::StlBinary }; let f = if bool_(p, "ascii").unwrap_or(false) { Format::StlAscii } else { Format::StlBinary };
@@ -384,6 +445,10 @@ fn recovery_discard(_s: &mut Session, p: &Value) -> Result<Value> {
Ok(json!({"discarded": ids})) Ok(json!({"discarded": ids}))
} }
#[cfg(test)]
#[path = "urdf_tests.rs"]
mod urdf_tests;
#[cfg(test)] #[cfg(test)]
mod tests { mod tests {
use super::*; use super::*;
+647
View File
@@ -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<f64> {
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<String> = 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#"<link name="base_link">"#,
r#"<link name="LeafA">"#,
r#"<link name="LeafB">"#,
r#"<joint name="LeafA_fixed" type="fixed">"#,
r#"<joint name="Hinge" type="continuous">"#,
r#"<mesh filename="hinge-LeafB.stl"/>"#,
] {
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#"<joint name="Rail" type="prismatic">"#), "{x}");
assert!(x.contains(r#"<limit lower="0" upper="0.01" effort="1000" velocity="10"/>"#), "{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#"<joint name="Turn" type="continuous">"#), "{}", 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#"<limit effort="12.5" velocity="0.5"/>"#), "{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#"<origin xyz="0 0 0.03" rpy="0 0 0"/>"#), "{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#"<mesh filename="package://robot/meshes/p-LeafA.stl"/>"#), "{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#"<color rgba="1 0 0 0.5"/>"#), "{}", 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<String> = 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!("</{name}> 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);
}
+1 -1
View File
@@ -21,7 +21,7 @@ pub mod tspline;
mod vec; mod vec;
pub use arcfit::MAX_FIT_PIECES; 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 plane::Plane;
pub use profile::{Loop2, MAX_BEZIER_DEGREE, Region2, Seg2, bezier, regions_apart}; pub use profile::{Loop2, MAX_BEZIER_DEGREE, Region2, Seg2, bezier, regions_apart};
pub use vec::{Vec2, Vec3}; pub use vec::{Vec2, Vec3};
+66
View File
@@ -252,6 +252,72 @@ pub struct MeshMeasure {
pub centroid: Vec3, 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<MassProperties> {
// 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::<f64>() + 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 { impl Mesh {
/// Visibility in shaded edge presentation, independent of selection/topology. /// Visibility in shaded edge presentation, independent of selection/topology.
pub fn shaded_edge_visible(&self, index: usize) -> bool { pub fn shaded_edge_visible(&self, index: usize) -> bool {
+96
View File
@@ -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)); 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 { mod close_pairs {
use super::*; use super::*;
+9 -2
View File
@@ -1,6 +1,6 @@
//! File formats. Designs use JSON or versioned containers (`.solidcraft`); the authoritative //! File formats. Designs use JSON or versioned containers (`.solidcraft`); the authoritative
//! document rebuilds on load and optional display caches remain provisional. Bodies export //! 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 //! files import as a base feature (see [`step_import_feature`]), 3MF and STL files as mesh bodies
//! (see [`mesh_import_feature`]). //! (see [`mesh_import_feature`]).
#![deny(clippy::unwrap_used, clippy::expect_used, clippy::panic, clippy::unimplemented, clippy::todo, clippy::unreachable)] #![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 sketch2d;
mod svg; mod svg;
mod threemf; mod threemf;
pub mod urdf;
pub mod vfs; pub mod vfs;
pub use dxf::{read_dxf, write_dxf}; pub use dxf::{read_dxf, write_dxf};
@@ -30,6 +31,7 @@ pub use svg::read_svg;
mod obj; mod obj;
pub use obj::read_obj; pub use obj::read_obj;
pub use threemf::{MAX_3MF_BYTES, MeshObject, model_xml, read_3mf, weld, write_3mf}; 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_doc::{Document, ModelState};
use solidcraft_geom::{Mesh, Vec3}; use solidcraft_geom::{Mesh, Vec3};
@@ -54,7 +56,7 @@ pub const MAX_DESIGN_BYTES: usize = 256 << 20;
#[derive(Debug, thiserror::Error)] #[derive(Debug, thiserror::Error)]
pub enum IoError { 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), Format(String),
#[error("nothing to export: the design has no bodies")] #[error("nothing to export: the design has no bodies")]
Empty, Empty,
@@ -202,6 +204,9 @@ pub enum Format {
/// `.3mf` path alone means [`Format::ThreeMf`]. /// `.3mf` path alone means [`Format::ThreeMf`].
ThreeMfTagged, ThreeMfTagged,
Design, Design,
/// A robot description of the assembly with its mesh files ([`urdf()`]); written by the
/// engine, since it is several files.
Urdf,
} }
impl Format { impl Format {
@@ -216,6 +221,7 @@ impl Format {
"igs" | "iges" => Format::Iges, "igs" | "iges" => Format::Iges,
"3mf" => Format::ThreeMf, "3mf" => Format::ThreeMf,
"3mf-tagged" => Format::ThreeMfTagged, "3mf-tagged" => Format::ThreeMfTagged,
"urdf" => Format::Urdf,
"solidcraft" | "solvecraft" | "json" => Format::Design, "solidcraft" | "solvecraft" | "json" => Format::Design,
_ => return Err(IoError::Format(ext)), _ => return Err(IoError::Format(ext)),
}) })
@@ -336,6 +342,7 @@ pub fn export(state: &ModelState, bodies: &[String], format: Format, name: &str)
write_3mf(&objs)? write_3mf(&objs)?
} }
Format::Design => return Err(IoError::Format("solidcraft (save the document instead)".into())), 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())),
}) })
} }
+916
View File
@@ -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,
/// `<limit effort>` (N or N m) and `velocity` (m/s or rad/s); defaults with a warning.
pub effort: Option<f64>,
pub velocity: Option<f64>,
}
/// 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<u8>)>,
pub model: UrdfModel,
pub warnings: Vec<String>,
}
/// The robot before serialisation (lengths in mm): tests compose it to check the kinematics.
#[derive(Clone, Debug, Default)]
pub struct UrdfModel {
pub links: Vec<Link>,
pub joints: Vec<UrdfJoint>,
}
#[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<u64>,
/// Mesh coordinates (the component's frame) to the link frame.
pub mesh_origin: Mat,
pub visual: Option<String>,
pub collision: Option<String>,
/// sRGB and opacity, 0..1.
pub color: Option<[f32; 4]>,
pub inertial: Option<Inertial>,
}
/// 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<Mat> {
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<String>);
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<String>,
collision: Option<String>,
color: Option<[f32; 4]>,
masses: Vec<BodyMass>,
}
/// 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<BTreeMap<u64, Part>> {
let mut used: BTreeSet<u64> = 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<u64> {
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<u64, Part>,
names: Names,
model: UrdfModel,
warnings: Vec<String>,
/// Occurrence to its link and the link's world frame (0: `base_link`).
placed: BTreeMap<u64, (usize, Mat)>,
/// Each usable joint's name, taken before any link's so it keeps the name the user gave.
joint_names: BTreeMap<u64, String>,
}
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<u64>, frame: &Mat, at: &Mat) -> Option<usize> {
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<usize> {
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<u64>, usable: &[(&Joint, Mat)], taken: &mut HashSet<u64>) {
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<u64>) {
let mut copies: BTreeMap<Vec<u64>, (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 <mimic>)",
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<UrdfPackage> {
// 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<u64> = 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<u64> = 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<u64> = 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<Inertial> {
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!("<origin xyz=\"{}\" rpy=\"{}\"/>", 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("<?xml version=\"1.0\"?>\n<!-- Exported by SolidCraft; units: m, kg, rad -->\n");
s += &format!("<robot name=\"{}\">\n", attr(name));
for l in &model.links {
s += &format!(" <link name=\"{}\">\n", attr(&l.name));
if let Some(i) = &l.inertial {
let c = i.com * 1e-3;
let t = &i.inertia;
s += &format!(
" <inertial>\n <origin xyz=\"{}\" rpy=\"0 0 0\"/>\n <mass value=\"{}\"/>\n <inertia ixx=\"{}\" ixy=\"{}\" ixz=\"{}\" iyy=\"{}\" iyz=\"{}\" izz=\"{}\"/>\n </inertial>\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!(
" <visual>\n {}\n <geometry><mesh filename=\"{prefix}{}\"/></geometry>\n",
origin_xml(&l.mesh_origin),
attr(file)
);
if let Some([r, g, b, a]) = l.color {
s += &format!(
" <material name=\"{}_color\"><color rgba=\"{} {} {} {}\"/></material>\n",
attr(&l.name),
num(r as f64),
num(g as f64),
num(b as f64),
num(a as f64)
);
}
s += " </visual>\n";
}
if let Some(file) = &l.collision {
s += &format!(
" <collision>\n {}\n <geometry><mesh filename=\"{prefix}{}\"/></geometry>\n </collision>\n",
origin_xml(&l.mesh_origin),
attr(file)
);
}
s += " </link>\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!(" <joint name=\"{}\" type=\"{kind}\">\n", attr(&j.name));
s += &format!(" <parent link=\"{}\"/>\n <child link=\"{}\"/>\n {}\n", attr(&parent.name), attr(&child.name), origin_xml(&j.origin));
if j.step != Step::Fixed {
s += &format!(" <axis xyz=\"{}\"/>\n", triple(j.axis));
let scale = if j.step == Step::Prismatic { 1e-3 } else { 1.0 };
match j.limit {
Some((lo, hi)) => {
s += &format!(
" <limit lower=\"{}\" upper=\"{}\" effort=\"{}\" velocity=\"{}\"/>\n",
num(lo * scale),
num(hi * scale),
num(effort),
num(velocity)
);
}
None => s += &format!(" <limit effort=\"{}\" velocity=\"{}\"/>\n", num(effort), num(velocity)),
}
}
s += " </joint>\n";
}
s += "</robot>\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<Mat> {
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 `<origin xyz rpy>` 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 `<origin>` 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<f64> = 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<Mat> = 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::<f64>().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");
}
}
+18
View File
@@ -103,6 +103,24 @@ fn handshake_and_tools_list() {
assert_eq!(call(&mut s, 8, "nope/nope", json!({}))["error"]["code"], -32601); 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. /// Box → fillet → hole in one batch, then verify with measure / inspect_design and look at it.
#[test] #[test]
fn agent_builds_box_fillet_hole() { fn agent_builds_box_fillet_hole() {
+9 -4
View File
@@ -190,12 +190,17 @@ pub fn tool_definitions() -> Vec<Value> {
"export", "export",
"Export", "Export",
"Export bodies to STEP (.step/.stp), 3MF (.3mf), binary STL (.stl), ASCII STL (format stla) or OBJ. The format \ "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( obj(
json!({ json!({
"path": string("Output file"), "path": string("Output file"),
"format": {"type": "string", "enum": ["step", "3mf", "3mf-tagged", "stl", "stla", "obj"]}, "format": {"type": "string", "enum": ["step", "3mf", "3mf-tagged", "stl", "stla", "obj", "urdf"]},
"bodies": {"type": "array", "items": {"type": "string"}, "description": "Body names (default: all)"}, "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"], &["path"],
), ),
@@ -387,7 +392,7 @@ fn dispatch(b: &mut dyn Backend, name: &str, a: &Map<String, Value>) -> Result<V
p["expression"] = a.get("value").cloned().unwrap_or(Value::Null); p["expression"] = a.get("value").cloned().unwrap_or(Value::Null);
exec(b, "parameters.change", p) exec(b, "parameters.change", p)
} }
"export" => 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") { "crash_reports" => match a.get("id") {
Some(_) => exec(b, "diagnostics.crash_report", pick(a, &["id"])), Some(_) => exec(b, "diagnostics.crash_report", pick(a, &["id"])),
None => exec(b, "diagnostics.crash_reports", pick(a, &["limit"])), None => exec(b, "diagnostics.crash_reports", pick(a, &["limit"])),
+22 -1
View File
@@ -436,6 +436,10 @@ fn file_menu(app: &mut SolveApp, ctx: &egui::Context) {
export(app, "obj"); export(app, "obj");
close = true; close = true;
} }
if item_if(ui, "Export URDF…", "", shown) {
export(app, "urdf");
close = true;
}
if item(ui, "Export Design as JSON…", "") { if item(ui, "Export Design as JSON…", "") {
export(app, "json"); export(app, "json");
close = true; close = true;
@@ -1452,7 +1456,8 @@ mod home_tests {
let name = if cmd { format!("Ctrl+{key}") } else { key.to_string() }; 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})))))); 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(( actions.push((
format!("File › {item}"), format!("File › {item}"),
Box::new(move |h| { 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"); 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<String> = 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). /// The disabled buttons' tooltip is translated in every catalog (not English fallback).
#[test] #[test]
fn needs_design_tooltip_is_translated() { fn needs_design_tooltip_is_translated() {
+6
View File
@@ -8,6 +8,7 @@
| IGES 5.3 | manifold solids, or trimmed surfaces sewn into solids | manifold solids with names and colours | | IGES 5.3 | manifold solids, or trimmed surfaces sewn into solids | manifold solids with names and colours |
| 3MF, STL, OBJ | mesh bodies | yes | | 3MF, STL, OBJ | mesh bodies | yes |
| 3MF with face tags | as 3MF | each triangle tagged with its B-rep face, exact coordinates | | 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 | | DXF, SVG | into a sketch | sketches and flat patterns as DXF |
| PNG, JPEG | canvases and decals | | | 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 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). 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 Saves are atomic: a crash at any moment leaves either the old file or the new one, and the
replaced version is kept as `<file>.bak`. A save keeps the file's permissions, and a design replaced version is kept as `<file>.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 opened through a symbolic link (a synced folder linked into a project) is saved into the file the
+1 -1
View File
@@ -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. | | `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. | | `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). | | `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. | | `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). | | `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. | | `new_design` | `name?` | Start an empty design. |
+115
View File
@@ -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-<component>.stl` holds all of the component's bodies. The root component's bodies go
in `robot-base.stl`.
- `robot-<component>-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://<pkg>/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 `<joint>_link1`, `<joint>_link2`. The steps before the last are named
`<joint>_dof1`, `<joint>_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 `<mimic>`. 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 `<mimic>`.) The export stops copying past
64 levels or 100 000 placements, with a warning.
## Not covered
- `<mimic>` 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.
+1 -1
View File
@@ -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. - 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 ## 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 ## Red flags