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
1 parent
d2e409f40c
commit
17db109c5d
19 files changed
+1993
-17
No files matched your search
@@ -125,7 +125,8 @@ rebuild with no errors, replayed here at 2× speed.
|
||||
provisional picture from warm geometry caches while rebuilding before editing; caches never
|
||||
replace the parametric design. Export to STEP (AP242 with body names, colours and
|
||||
components as assemblies; revolved faces as native planes, cylinders, cones, spheres and tori),
|
||||
IGES 5.3 (manifold solid B-reps with names and colours), 3MF, STL (binary/ASCII) and OBJ;
|
||||
IGES 5.3 (manifold solid B-reps with names and colours), 3MF, STL (binary/ASCII) and OBJ, and
|
||||
assemblies with joints as URDF for robot simulators (docs/urdf.md);
|
||||
3MF, STL and OBJ import as mesh bodies (they render, measure, move and export; solid features
|
||||
need B-rep bodies). Convert Mesh reconstructs supported closed constant-section parts
|
||||
as parametric solids with planar and circular cylindrical faces;
|
||||
|
||||
+1
-1
@@ -52,7 +52,7 @@ PCB / Package, Render, Animation, Simulation, Manufacture/CAM, Drawings.
|
||||
| M6 | UI depth: selection feedback and origin (done), timeline editing (done: hover and selection highlight modifier faces, including fillets and chamfers; edit feature, history marker drag, dependency-checked single and multi-item reorder, suppress, rename, delete with dependents, re-resolved references; redefining selections refreshes persistent edge/face references while retaining feature identity and dimension names; optional compact named left feature history with rollback bar, multi-selection and persistent folders above the parts browser), right-docked dialogs (done), live previews for every feature dialog (done), keyboard-first commands with on-canvas value boxes and sketch inline dimensions (done), drag manipulators with automatic join/cut (done: extrude incl. two sides, fillet, shell, hole depth, move, pattern spacing, revolve rotator; taper handle open), dark theme (done), right-click menu with Repeat (done), measure tool (done), section analysis (done), selection filter (done), drag-solve of sketch points (done); appearance controls now preserve body/face/component targeting, named transparent looks, reuse/search and override removal; preferences | in progress | 6 |
|
||||
| M7 | Sweep, loft (incl. to a point), rib and web (to next / depth), emboss (planar faces and exact radial emboss/deboss of bounded orthogonal profiles on complete exterior cylinders/sleeves), thread (cosmetic, and modelled M3–M20 outside and inside: the ISO profile screwed in as a faceted cut), coil (smooth B-spline helical walls), pipe, pattern on path, patterns of bodies and components, replace face (parallel), align, remove, feature copy/paste: done. Unique eligible cylindrical targets are inferred for new Emboss features. Left: general curved-face emboss and exact (unfaceted) modelled threads | in progress | 12 |
|
||||
| M8 | Surface workspace (#772). Done: Patch (sketch regions; edge loops filled G0), Stitch, Thicken (planes and cylinders), Extend (straight edges of planar surfaces), Trim (by a plane or a face), Delete Face (healing), Boundary Fill, zebra/draft/curvature-map analysis. Proposed, in order: surface Extrude/Revolve/Sweep/Loft from open and closed curves (#757), Offset surface (#758), Unstitch and Reverse Normal (#766), Delete Face without healing (#768); Extend curved faces and edges (#760), mutual trim and trim by projected curves (#767), Untrim (#761), Thicken every surface kind (#765); Ruled (#762), Boundary surface and curve networks with G0/G1/G2 (#763), Patch with G1/G2 and general loops (#759), Blend surface (#764), sweep rails, twist and scale (#771, loft rails #201), fillet between surface bodies (#769); continuity analysis, surface curvature combs and isocurves (#770) | deferred | — |
|
||||
| M9 | Components and assemblies: New Component / Create Components from Bodies, active component, per-component origin/bodies/sketches/timeline entries, occurrences with transforms, Copy/Paste vs Paste New, nested Browser tree with visibility and isolate; Ground, Move/Copy of occurrences, joints (rigid, revolute, slider, cylindrical, pin-slot, planar, ball) by joint origins with limits and Drive Joints, As-built Joint, Rigid Group, Contact Sets (driving and motion studies stop where bodies in a set would pass through each other), Motion Study (joint values keyed along a timeline, playback, export of occurrence positions as JSON/CSV), Exploded views (automatic or by hand, shown without changing placements); interference, per-component physical properties; STEP with assembly structure. Physical properties now measure material-aware component/occurrence placements, descendants and repeated instances, including pending moves and body reassignment offsets; contact geometry and automatic exploded centres respect those offsets. Groundwork done: component tree, occurrences with transforms (instances, Paste New), world placement, active component as authoring frame, ground, Capture/Revert Position, moving bodies and sketches. UI: ASSEMBLE panel dialogs for New Component, Joint (snaps on faces, edges, circles and vertices; animated motion preview; offsets, flip, limits), As-built Joint, Joint Origin, Rigid Group, Drive Joints (live slider), Motion Link and Interference (overlaps shown red); joints in the timeline and the Browser | in progress | 32 |
|
||||
| M9 | Components and assemblies: New Component / Create Components from Bodies, active component, per-component origin/bodies/sketches/timeline entries, occurrences with transforms, Copy/Paste vs Paste New, nested Browser tree with visibility and isolate; Ground, Move/Copy of occurrences, joints (rigid, revolute, slider, cylindrical, pin-slot, planar, ball) by joint origins with limits and Drive Joints, As-built Joint, Rigid Group, Contact Sets (driving and motion studies stop where bodies in a set would pass through each other), Motion Study (joint values keyed along a timeline, playback, export of occurrence positions as JSON/CSV), Exploded views (automatic or by hand, shown without changing placements); interference, per-component physical properties; STEP with assembly structure; URDF export for robot simulators (links with meshes, mass and inertia; joint chains whose values match Drive Joints; docs/urdf.md). Physical properties now measure material-aware component/occurrence placements, descendants and repeated instances, including pending moves and body reassignment offsets; contact geometry and automatic exploded centres respect those offsets. Groundwork done: component tree, occurrences with transforms (instances, Paste New), world placement, active component as authoring frame, ground, Capture/Revert Position, moving bodies and sketches. UI: ASSEMBLE panel dialogs for New Component, Joint (snaps on faces, edges, circles and vertices; animated motion preview; offsets, flip, limits), As-built Joint, Joint Origin, Rigid Group, Drive Joints (live slider), Motion Link and Interference (overlaps shown red); joints in the timeline and the Browser | in progress | 32 |
|
||||
| M10 | Sheet metal: flange (base, edge, contour), hem, unfold/refold, convert, flat pattern + DXF, rules: done. PLASTIC enclosure subset: Boss, Rib, Web, Lip/Groove, Snap Fit, Rest, Plastic Rules (library, edit, assign; rule draft and clearance as feature defaults): done, tested against hand-computed geometry. UI: SHEET METAL and PLASTIC tabs in Fusion's layout with dialogs (live preview, Edit Feature, pre-selection) for Flange, Hem, Unfold/Refold, Convert, Sheet Metal Rules, Create Flat Pattern (flat view with bend lines, bend table CSV/JSON and DXF export with exact circle/arc/line cutouts), Boss, Lip/Groove, Snap Fit, Rest, Manage and Assign Plastic Rules; Fold (bend along a sketch line); flanges meeting at a corner closed at any angle with the rule's gap; edge flanges with their own angle and height per edge in one feature; corner reliefs (notch, round, rectangular, tear) chosen by the rule; Convert to Sheet Metal from a solid or shelled body by its bend edges (flat faces, sharp bends) | in progress | 36 |
|
||||
| M11 | 2D drawings | deferred | — |
|
||||
| M12 | Release: cross-platform packages, verification and deliberate publication implemented; original current-UI demo refreshed. Trusted distribution signing and broader performance work remain | in progress | 16 |
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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)]
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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::*;
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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};
|
||||
|
||||
@@ -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 {
|
||||
|
||||
@@ -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::*;
|
||||
|
||||
|
||||
@@ -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())),
|
||||
})
|
||||
}
|
||||
|
||||
|
||||
@@ -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");
|
||||
}
|
||||
}
|
||||
@@ -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() {
|
||||
|
||||
@@ -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"])),
|
||||
|
||||
@@ -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() {
|
||||
|
||||
@@ -8,6 +8,7 @@
|
||||
| IGES 5.3 | manifold solids, or trimmed surfaces sewn into solids | manifold solids with names and colours |
|
||||
| 3MF, STL, OBJ | mesh bodies | yes |
|
||||
| 3MF with face tags | as 3MF | each triangle tagged with its B-rep face, exact coordinates |
|
||||
| URDF | | the assembly as a robot: links with meshes, mass and inertia, joints from the assembly's joints |
|
||||
| DXF, SVG | into a sketch | sketches and flat patterns as DXF |
|
||||
| PNG, JPEG | canvases and decals | |
|
||||
|
||||
@@ -35,6 +36,11 @@ is the tessellation's own double-precision value. Meshing and analysis tools can
|
||||
keep a model's faces (for loads, boundary conditions or feature edges); other 3MF readers ignore
|
||||
them. The format is specified in [`docs/formats/3mf-face-tags.md`](https://github.com/bherbruck/solidcraft/blob/main/docs/formats/3mf-face-tags.md).
|
||||
|
||||
**File → Export URDF…** (`file.export` with a `.urdf` path) writes the assembly for robot
|
||||
simulators and ROS: a link per component placement with its mesh, mass and inertia, and the
|
||||
assembly's joints, so the parts move in the simulator as they do in SolidCraft. The meshes are
|
||||
written as STL files next to the `.urdf`. See [`docs/urdf.md`](https://github.com/bherbruck/solidcraft/blob/main/docs/urdf.md).
|
||||
|
||||
Saves are atomic: a crash at any moment leaves either the old file or the new one, and the
|
||||
replaced version is kept as `<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
|
||||
|
||||
+1
-1
@@ -124,7 +124,7 @@ They cover the workflow, the command shapes, how to verify with `measure`, and t
|
||||
| `body_topology` | `body` | Edges (index, midpoint, length, ends) and faces (index, area, centroid, normal). An edge midpoint is the `[x,y,z]` that picks that edge for fillet and chamfer. |
|
||||
| `set_parameter` | `name`, `value` (number or expression), `unit?`, `comment?` | Create or change a user parameter and recompute; reports `recomputed` (features rebuilt), `restored` (features whose earlier result for the same inputs was reused, e.g. a value set back) and features that now fail. |
|
||||
| `screenshot` | `view?` (iso, top, front, back, bottom, left, right, home, fit, or a named view saved with `view.save`), `camera?` (`{target, yaw, pitch, distance, fov?}`, as `view.apply`), `path?`, `width?`, `height?`, `source?` (window, model) | A PNG image content block. Headless: CPU render fitted to the model, or from the named view or camera. Bridged: the app window (default) or a model render with the app's camera; a named view or camera moves the app's viewport there first (`execute` `view.apply` does the same without a screenshot). |
|
||||
| `export` | `path`, `format?` (step, 3mf, 3mf-tagged, stl, stla, obj), `bodies?` | STEP, 3MF or mesh export. |
|
||||
| `export` | `path`, `format?` (step, 3mf, 3mf-tagged, stl, stla, obj, urdf), `bodies?`, `mesh_prefix?`, `effort?`, `velocity?` | STEP, 3MF or mesh export; URDF writes the whole assembly with its meshes next to it (`docs/urdf.md`). |
|
||||
| `undo`, `redo` | — | Undo / redo one change. |
|
||||
| `crash_reports` | `id?`, `limit?` | The crash reports on this computer, newest first; with `id`, the whole report (backtrace, command, recent commands, `design_path`/`replay_path`). See [crash-reports.md](crash-reports.md). |
|
||||
| `new_design` | `name?` | Start an empty design. |
|
||||
|
||||
+115
@@ -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.
|
||||
@@ -56,7 +56,7 @@ Work the part out as numbers first. Then build it in a few large `batch` calls,
|
||||
- When the server is bridged, `source` defaults to the whole app window and `view` also turns the user's camera, so pass `"source":"model"` and leave out `view` unless the user wants the camera moved.
|
||||
|
||||
## Finish
|
||||
`export {path}` writes `.step` for CAD or `.3mf`/`.stl` for printing. `save {path}` writes `.solidcraft`.
|
||||
`export {path}` writes `.step` for CAD, `.3mf`/`.stl` for printing, or `.urdf` (an assembly with joints, for robot simulators). `save {path}` writes `.solidcraft`.
|
||||
|
||||
## Red flags
|
||||
|
||||
|
||||
Reference in new issue
Block a user