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

+10 -1
View File
@@ -124,6 +124,10 @@ fn identity() -> Mat {
IDENTITY
}
/// How deep and how many placements [`Document::placements`] walks.
pub const MAX_PLACEMENT_DEPTH: usize = 64;
pub const MAX_PLACEMENTS: usize = 100_000;
fn map_plane(m: &Mat, p: &Plane) -> Plane {
let (o, x, y) = (apply_point(m, p.origin), apply_vector(m, p.x), apply_vector(m, p.y));
Plane::new(o, x, y).unwrap_or(*p)
@@ -375,13 +379,18 @@ impl Document {
}
/// Every placement of each component: (component, occurrence path ids, world transform).
/// A branch stops past [`MAX_PLACEMENT_DEPTH`] levels, the walk past [`MAX_PLACEMENTS`] placements.
pub fn placements(&self) -> Vec<(u64, Vec<u64>, Mat)> {
let mut out = 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() {
if path.len() > 64 || out.len() > 100_000 {
if out.len() > MAX_PLACEMENTS {
break;
}
// Only this branch stops past the depth limit; the others are still walked.
if path.len() > MAX_PLACEMENT_DEPTH {
continue;
}
for o in self.occurrences.iter().filter(|o| o.parent == comp) {
let mut p = path.clone();
p.push(o.id);
+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};
const MAX_JOINTS: usize = 10_000;
/// Joints the solver (and the URDF export) reads at most.
pub const MAX_JOINTS: usize = 10_000;
/// Where a joint origin sits on a component's geometry (component coordinates).
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
+4 -1
View File
@@ -40,7 +40,10 @@ mod project3d;
pub use project3d::iso_grid;
pub mod sheet;
pub use assembly::{IDENTITY, Mat, Occurrence, apply_point, apply_vector, is_identity, map_point_expr, mat_inverse, mat_mul, rigid};
pub use assembly::{
IDENTITY, MAX_PLACEMENT_DEPTH, MAX_PLACEMENTS, Mat, Occurrence, apply_point, apply_vector, is_identity, map_point_expr, mat_inverse, mat_mul,
rigid,
};
pub use document::*;
pub use eval::{
EdgeRef, FeatureResult, Model, ModelBody, ModelState, RevolveExtent, SolvedSketch, ThreadInfo, curve_pick_error, edge_names_for, edge_refs,
+67 -2
View File
@@ -8,7 +8,7 @@ use solidcraft_doc::Document;
use solidcraft_io::Format;
use super::CommandSpec;
use crate::params::{bad, bool_, str_, string_list};
use crate::params::{bad, bool_, num, str_, string_list};
use crate::{EngineError, Result, Session};
pub static COMMANDS: &[CommandSpec] = &[
@@ -45,7 +45,7 @@ pub static COMMANDS: &[CommandSpec] = &[
CommandSpec::new("file.export", "Export", export)
.icon("export")
.noundo()
.params("path; format?: stl|stla|obj|step|3mf|3mf-tagged|json (default from extension; 3mf-tagged: 3MF whose triangles carry their B-rep face, at full precision, for meshing and analysis tools; docs/formats/3mf-face-tags.md); bodies?: [names] (JSON exports the complete design)"),
.params("path; format?: stl|stla|obj|step|3mf|3mf-tagged|urdf|json (default from extension; 3mf-tagged: 3MF whose triangles carry their B-rep face, at full precision, for meshing and analysis tools; docs/formats/3mf-face-tags.md; urdf: the whole assembly as a robot, with STL meshes written next to it; docs/urdf.md); bodies?: [names] (JSON and URDF export the complete design); urdf only: mesh_prefix? (before each mesh file name, e.g. package://robot/meshes/), effort? (default 1000), velocity? (default 10)"),
CommandSpec::new("file.save_mesh", "Save As Mesh", save_stl).icon("export").noundo().params("path; bodies?: [names]; ascii?: bool"),
];
@@ -328,9 +328,70 @@ fn export_to(s: &Session, path: &str, format: Format, bodies: &[String]) -> Resu
fn export(s: &mut Session, p: &Value) -> Result<Value> {
let path = path_arg(p, "file.export")?;
let format = Format::from_name(str_(p, "format").unwrap_or(path))?;
if format == Format::Urdf {
return export_urdf(s, path, p);
}
export_to(s, path, format, &string_list(p, "bodies"))
}
/// The assembly as a URDF and its meshes next to it (docs/urdf.md). Every path is checked before
/// anything is written; the meshes go first and the `.urdf` last.
fn export_urdf(s: &Session, path: &str, p: &Value) -> Result<Value> {
let cmd = "file.export";
if !string_list(p, "bodies").is_empty() {
return Err(bad(cmd, "urdf exports the whole assembly; omit bodies"));
}
let positive = |k: &str| match p.get(k) {
None | Some(Value::Null) => Ok(None),
Some(_) => num(p, k).filter(|x| *x > 0.0).map(Some).ok_or_else(|| bad(cmd, format!("`{k}` must be a positive number"))),
};
// `io::urdf` refuses a prefix XML cannot hold.
let mesh_prefix = str_(p, "mesh_prefix").unwrap_or("").to_string();
let options = solidcraft_io::UrdfOptions { mesh_prefix, effort: positive("effort")?, velocity: positive("velocity")? };
let file = std::path::Path::new(path);
let stem = file.file_stem().map(|x| x.to_string_lossy().to_string()).unwrap_or_else(|| s.doc.name.clone());
// Occurrences where they are shown, uncaptured moves included, except those the joint solver
// places: it puts them back as soon as a joint moves, so the move must not become part of
// their links.
let st = s.model.state();
let mut held = Vec::new();
let doc = if s.pending_moves.is_empty() {
std::borrow::Cow::Borrowed(&*s.doc)
} else {
let placed = solidcraft_doc::joints::solve(&s.doc, &st).world;
let mut doc = (*s.doc).clone();
for o in &mut doc.occurrences {
match s.pending_moves.get(&o.id) {
Some(_) if placed.contains_key(&o.id) => held.push(o.name.clone()),
Some(t) => o.transform = *t,
None => {}
}
}
std::borrow::Cow::Owned(doc)
};
let mut pkg = solidcraft_io::urdf(&doc, &doc.painted(&st), &stem, &options)?;
for name in held {
pkg.warnings.push(format!("`{name}` was moved but the move is not captured: exported where its joints place it"));
}
let dir = file.parent().unwrap_or(std::path::Path::new(""));
let mut targets: Vec<(String, &[u8])> = pkg.meshes.iter().map(|(n, b)| (dir.join(n).to_string_lossy().to_string(), b.as_slice())).collect();
targets.push((path.to_string(), pkg.xml.as_bytes()));
for (t, _) in &targets {
solidcraft_io::vfs::check(t).map_err(|e| EngineError::Other(format!("{t}: {e}")))?;
}
let mut written: Vec<String> = Vec::new();
for (t, bytes) in &targets {
if let Err(e) = solidcraft_io::vfs::write(t, bytes) {
let before = if written.is_empty() { "none".to_string() } else { written.join(", ") };
return Err(EngineError::Other(format!("{t}: {e} (this file may be incomplete; written before it: {before})")));
}
written.push(t.clone());
}
Ok(
json!({"path": path, "format": "Urdf", "files": written, "links": pkg.model.links.len(), "joints": pkg.model.joints.len(), "warnings": pkg.warnings}),
)
}
fn save_stl(s: &mut Session, p: &Value) -> Result<Value> {
let path = path_arg(p, "file.save_mesh")?;
let f = if bool_(p, "ascii").unwrap_or(false) { Format::StlAscii } else { Format::StlBinary };
@@ -384,6 +445,10 @@ fn recovery_discard(_s: &mut Session, p: &Value) -> Result<Value> {
Ok(json!({"discarded": ids}))
}
#[cfg(test)]
#[path = "urdf_tests.rs"]
mod urdf_tests;
#[cfg(test)]
mod tests {
use super::*;
+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;
pub use arcfit::MAX_FIT_PIECES;
pub use mesh::{Aabb3, BoxedTriangle, InsideIndex, Mesh, MeshMeasure, find_close_pair};
pub use mesh::{Aabb3, BoxedTriangle, InsideIndex, MassProperties, Mesh, MeshMeasure, find_close_pair};
pub use plane::Plane;
pub use profile::{Loop2, MAX_BEZIER_DEGREE, Region2, Seg2, bezier, regions_apart};
pub use vec::{Vec2, Vec3};
+66
View File
@@ -252,6 +252,72 @@ pub struct MeshMeasure {
pub centroid: Vec3,
}
/// Volume, centroid and inertia tensor of a closed mesh at unit density: the inertia is about
/// the centroid, in the mesh's axes (times a density, mass and inertia in that density's units).
#[derive(Clone, Copy, Debug, Default, PartialEq)]
pub struct MassProperties {
pub volume: f64,
pub centroid: Vec3,
pub inertia: [[f64; 3]; 3],
}
impl Mesh {
/// [`MassProperties`] of a closed mesh, by the divergence theorem over the tetrahedra its
/// triangles make with the origin. `None` unless every edge (by vertex index: weld first) is
/// shared by exactly two triangles that cross it in opposite directions (closed, consistently
/// oriented), or for a mesh without volume or with non-finite values.
pub fn mass_properties(&self) -> Option<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 {
/// Visibility in shaded edge presentation, independent of selection/topology.
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));
}
/// A closed box mesh `[x0, x0+a] x [y0, y0+b] x [z0, z0+c]`, outward-facing.
fn offset_box(o: Vec3, a: f64, b: f64, c: f64) -> Mesh {
let p = |x: f64, y: f64, z: f64| o + Vec3::new(x * a, y * b, z * c);
let positions = vec![p(0., 0., 0.), p(1., 0., 0.), p(1., 1., 0.), p(0., 1., 0.), p(0., 0., 1.), p(1., 0., 1.), p(1., 1., 1.), p(0., 1., 1.)];
let triangles =
vec![[0, 2, 1], [0, 3, 2], [4, 5, 6], [4, 6, 7], [0, 1, 5], [0, 5, 4], [1, 2, 6], [1, 6, 5], [2, 3, 7], [2, 7, 6], [3, 0, 4], [3, 4, 7]];
Mesh { positions, triangles, ..Default::default() }
}
fn close(a: f64, b: f64, rel: f64) -> bool {
(a - b).abs() <= rel * b.abs().max(1e-12)
}
#[test]
fn mass_properties_of_a_box_match_the_closed_form() {
let (a, b, c) = (20.0, 30.0, 40.0);
let m = offset_box(Vec3::ZERO, a, b, c).mass_properties().expect("a closed box has mass properties");
let v = a * b * c;
assert!(close(m.volume, v, 1e-12), "{m:?}");
assert!((m.centroid - Vec3::new(10.0, 15.0, 20.0)).len() < 1e-9, "{m:?}");
let i = m.inertia;
assert!(close(i[0][0], v * (b * b + c * c) / 12.0, 1e-9), "{i:?}");
assert!(close(i[1][1], v * (a * a + c * c) / 12.0, 1e-9), "{i:?}");
assert!(close(i[2][2], v * (a * a + b * b) / 12.0, 1e-9), "{i:?}");
for (r, cc) in [(0, 1), (0, 2), (1, 2)] {
assert!(i[r][cc].abs() < 1e-6 && i[cc][r].abs() < 1e-6, "{i:?}");
}
}
#[test]
fn mass_properties_of_a_cylinder_are_close() {
// A cylinder of radius 10 and height 30 along z, 256 sides, capped by fans.
let (r, h, n) = (10.0, 30.0, 256usize);
let mut positions = vec![Vec3::new(0.0, 0.0, 0.0), Vec3::new(0.0, 0.0, h)];
for k in 0..n {
let t = std::f64::consts::TAU * k as f64 / n as f64;
positions.push(Vec3::new(r * t.cos(), r * t.sin(), 0.0));
positions.push(Vec3::new(r * t.cos(), r * t.sin(), h));
}
let mut triangles = Vec::new();
for k in 0..n as u32 {
let (b0, t0) = (2 + 2 * k, 3 + 2 * k);
let (b1, t1) = (2 + 2 * ((k + 1) % n as u32), 3 + 2 * ((k + 1) % n as u32));
triangles.push([0, b1, b0]);
triangles.push([1, t0, t1]);
triangles.push([b0, b1, t1]);
triangles.push([b0, t1, t0]);
}
let m = Mesh { positions, triangles, ..Default::default() }.mass_properties().expect("closed");
let v = std::f64::consts::PI * r * r * h;
assert!(close(m.volume, v, 1e-3), "{m:?}");
assert!(close(m.inertia[2][2], m.volume * r * r / 2.0, 1e-3), "{m:?}");
assert!(close(m.inertia[0][0], m.volume * (3.0 * r * r + h * h) / 12.0, 1e-3), "{m:?}");
}
#[test]
fn mass_properties_are_translation_invariant_about_the_centroid() {
let at_origin = offset_box(Vec3::ZERO, 20.0, 30.0, 40.0).mass_properties().unwrap();
let moved = offset_box(Vec3::new(100.0, -50.0, 7.0), 20.0, 30.0, 40.0).mass_properties().unwrap();
assert!((moved.centroid - Vec3::new(110.0, -35.0, 27.0)).len() < 1e-9, "{moved:?}");
for r in 0..3 {
for c in 0..3 {
assert!((moved.inertia[r][c] - at_origin.inertia[r][c]).abs() <= 1e-9 * at_origin.inertia[0][0], "{r}{c}");
}
}
}
#[test]
fn mass_properties_of_an_inward_facing_box_are_positive() {
let mut m = offset_box(Vec3::ZERO, 2.0, 3.0, 4.0);
for t in &mut m.triangles {
t.swap(1, 2);
}
let p = m.mass_properties().unwrap();
assert!(close(p.volume, 24.0, 1e-12) && p.inertia[0][0] > 0.0, "{p:?}");
}
#[test]
fn mass_properties_of_an_open_or_empty_mesh_are_none() {
let mut open = offset_box(Vec3::ZERO, 20.0, 30.0, 40.0);
open.triangles.truncate(10);
assert!(open.mass_properties().is_none(), "a box missing a face has no mass");
assert!(Mesh::default().mass_properties().is_none());
}
#[test]
fn mass_properties_need_consistently_oriented_faces() {
// Every edge still has two triangles, but one face of the box points inwards: its signed
// volume would cancel part of the rest.
let mut m = offset_box(Vec3::ZERO, 20.0, 30.0, 40.0);
for t in m.triangles.iter_mut().take(2) {
t.swap(1, 2);
}
assert!(m.mass_properties().is_none());
}
mod close_pairs {
use super::*;
+9 -2
View File
@@ -1,6 +1,6 @@
//! File formats. Designs use JSON or versioned containers (`.solidcraft`); the authoritative
//! document rebuilds on load and optional display caches remain provisional. Bodies export
//! as STL (binary or ASCII), OBJ, STEP and 3MF; STEP
//! as STL (binary or ASCII), OBJ, STEP and 3MF, and assemblies as URDF ([`urdf()`]); STEP
//! files import as a base feature (see [`step_import_feature`]), 3MF and STL files as mesh bodies
//! (see [`mesh_import_feature`]).
#![deny(clippy::unwrap_used, clippy::expect_used, clippy::panic, clippy::unimplemented, clippy::todo, clippy::unreachable)]
@@ -20,6 +20,7 @@ pub mod safe_write;
mod sketch2d;
mod svg;
mod threemf;
pub mod urdf;
pub mod vfs;
pub use dxf::{read_dxf, write_dxf};
@@ -30,6 +31,7 @@ pub use svg::read_svg;
mod obj;
pub use obj::read_obj;
pub use threemf::{MAX_3MF_BYTES, MeshObject, model_xml, read_3mf, weld, write_3mf};
pub use urdf::{UrdfOptions, UrdfPackage, urdf};
use solidcraft_doc::{Document, ModelState};
use solidcraft_geom::{Mesh, Vec3};
@@ -54,7 +56,7 @@ pub const MAX_DESIGN_BYTES: usize = 256 << 20;
#[derive(Debug, thiserror::Error)]
pub enum IoError {
#[error("unsupported format `{0}` (use stl, obj, step, 3mf or solidcraft)")]
#[error("unsupported format `{0}` (use stl, obj, step, 3mf, urdf or solidcraft)")]
Format(String),
#[error("nothing to export: the design has no bodies")]
Empty,
@@ -202,6 +204,9 @@ pub enum Format {
/// `.3mf` path alone means [`Format::ThreeMf`].
ThreeMfTagged,
Design,
/// A robot description of the assembly with its mesh files ([`urdf()`]); written by the
/// engine, since it is several files.
Urdf,
}
impl Format {
@@ -216,6 +221,7 @@ impl Format {
"igs" | "iges" => Format::Iges,
"3mf" => Format::ThreeMf,
"3mf-tagged" => Format::ThreeMfTagged,
"urdf" => Format::Urdf,
"solidcraft" | "solvecraft" | "json" => Format::Design,
_ => return Err(IoError::Format(ext)),
})
@@ -336,6 +342,7 @@ pub fn export(state: &ModelState, bodies: &[String], format: Format, name: &str)
write_3mf(&objs)?
}
Format::Design => return Err(IoError::Format("solidcraft (save the document instead)".into())),
Format::Urdf => return Err(IoError::Format("urdf (an assembly package; use file.export)".into())),
})
}
+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);
}
/// A URDF goes through `export` with its own options (#719).
#[test]
fn export_tool_writes_a_urdf_package() {
let mut s = server();
tool(&mut s, "execute", json!({"command": "solid.box", "params": {"length": 10, "width": 10, "height": 10}}));
let dir = std::env::temp_dir().join(format!("solidcraft-mcp-urdf-{}", std::process::id()));
std::fs::create_dir_all(&dir).unwrap();
let urdf = dir.join("robot.urdf");
let r = tool(&mut s, "export", json!({"path": urdf.to_string_lossy(), "mesh_prefix": "package://robot/meshes/"}));
assert_eq!(r["format"], "Urdf", "{r}");
assert!(std::fs::read_to_string(&urdf).unwrap().contains(r#"filename="package://robot/meshes/robot-base.stl""#));
assert!(dir.join("robot-base.stl").exists());
// effort and velocity reach file.export, which refuses a bad one.
let r = raw_tool(&mut s, "export", json!({"path": urdf.to_string_lossy(), "effort": -1}));
assert_eq!(r["result"]["isError"], json!(true), "{r}");
let _ = std::fs::remove_dir_all(&dir);
}
/// Box → fillet → hole in one batch, then verify with measure / inspect_design and look at it.
#[test]
fn agent_builds_box_fillet_hole() {
+9 -4
View File
@@ -190,12 +190,17 @@ pub fn tool_definitions() -> Vec<Value> {
"export",
"Export",
"Export bodies to STEP (.step/.stp), 3MF (.3mf), binary STL (.stl), ASCII STL (format stla) or OBJ. The format \
comes from the extension unless given.",
comes from the extension unless given. URDF (.urdf) exports the whole assembly for robot simulators: a link per \
component placement with mass and inertia, joints from the assembly's joints, and STL meshes in metres written \
next to it (docs/urdf.md); RViz needs mesh_prefix such as package://robot/meshes/.",
obj(
json!({
"path": string("Output file"),
"format": {"type": "string", "enum": ["step", "3mf", "3mf-tagged", "stl", "stla", "obj"]},
"bodies": {"type": "array", "items": {"type": "string"}, "description": "Body names (default: all)"},
"format": {"type": "string", "enum": ["step", "3mf", "3mf-tagged", "stl", "stla", "obj", "urdf"]},
"bodies": {"type": "array", "items": {"type": "string"}, "description": "Body names (default: all; not with urdf)"},
"mesh_prefix": string("urdf: put before each mesh file name (default: none, so relative to the .urdf)"),
"effort": {"type": "number", "description": "urdf: joint effort limit, N or N m (default 1000)"},
"velocity": {"type": "number", "description": "urdf: joint velocity limit, m/s or rad/s (default 10)"},
}),
&["path"],
),
@@ -387,7 +392,7 @@ fn dispatch(b: &mut dyn Backend, name: &str, a: &Map<String, Value>) -> Result<V
p["expression"] = a.get("value").cloned().unwrap_or(Value::Null);
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") {
Some(_) => exec(b, "diagnostics.crash_report", pick(a, &["id"])),
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");
close = true;
}
if item_if(ui, "Export URDF…", "", shown) {
export(app, "urdf");
close = true;
}
if item(ui, "Export Design as JSON…", "") {
export(app, "json");
close = true;
@@ -1452,7 +1456,8 @@ mod home_tests {
let name = if cmd { format!("Ctrl+{key}") } else { key.to_string() };
actions.push((name, Box::new(move |h| drop(h.call("ui.key", json!({"key": key, "cmd": cmd}))))));
}
for item in ["Insert STEP…", "Insert Mesh…", "Save", "Save As…", "Export STL…", "Export STEP…", "Export 3MF…", "Export OBJ…"] {
for item in ["Insert STEP…", "Insert Mesh…", "Save", "Save As…", "Export STL…", "Export STEP…", "Export 3MF…", "Export OBJ…", "Export URDF…"]
{
actions.push((
format!("File › {item}"),
Box::new(move |h| {
@@ -1489,6 +1494,22 @@ mod home_tests {
assert!(h.app.session.path.is_some() && !h.app.session.is_dirty(), "the design saves once Home is closed");
}
/// File › Export URDF… writes the robot description and its meshes next to the picked file.
#[test]
fn file_menu_exports_a_urdf() {
let mut h = crate::scenario::Harness::new();
h.app.home.open = false;
h.app.run("solid.box", json!({"length": 10, "width": 10, "height": 10})).unwrap();
h.frames(2);
h.ctx.data_mut(|d| d.insert_temp(egui::Id::new("sc_file_menu"), false));
let at = crate::scenario::handle_at("appbar:folder").unwrap();
h.call("ui.click", json!({"x": at.x, "y": at.y}));
let _ = h.step(&json!({"widget": "Export URDF…"}));
h.settle();
let files: Vec<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).
#[test]
fn needs_design_tooltip_is_translated() {