99d8d382b9
* Bill of materials: part properties on components and assembly.bom A component definition now carries part properties (part number, description, revision, other tools' keys) and free text properties, saved with the component and shared by its occurrences. component.metadata and component.metadata.set read and change them in one undo step; Paste New copies them; parts.insert numbers standard parts from their standard and size, and parts.resize renumbers them. assembly.bom counts the placed components (flat: parts at any depth through their subassemblies; top: direct children), grouped by part number or by definition, sorted by part number, and writes CSV (fixed, quoted columns) or JSON. The app gets a Bill of Materials window on the INSPECT panel that lists, edits and exports the rows. Co-Authored-By: Claude Opus 5.5 (1M context) <noreply@anthropic.com> * Bill of materials: name a merged row's equal definitions once Two standard parts with the same size are separate definitions with the same name; their row now reads "SHCS M6x20" instead of repeating it. Co-Authored-By: Claude Opus 5.5 (1M context) <noreply@anthropic.com> * Window widths test: close the Bill of Materials window between commands The test closes the floating windows a command opens before starting the next one; the Bill of Materials window was left open and measured against the next dialogs' limits. Co-Authored-By: Claude Opus 5.5 (1M context) <noreply@anthropic.com> * Bill of materials: sub-parts in their part's row, and part properties without re-evaluating assembly.bom takes group_children: true. A part with a part number keeps what is placed inside it (a bolt's washer and nut) in its own row, listed under it as children (items 1.1, 1.2, with each and count) instead of rows of their own; bolts with different contents stay different rows, and total_occurrences is the same either way. The window has a switch for it and shows the children under their part; the CSV lists them after their row. Changing part properties re-evaluated the model, re-seated every joint and rebuilt the placed model: 361 ms per change with 30 jointed bolts. A change that only touches components' metadata and properties now takes the same path as a render-only change (proved on the whole Document), also for its undo and redo: 0.6 ms. Co-Authored-By: Claude Opus 5.5 (1M context) <noreply@anthropic.com> * Bill of materials: look the structure up once, not per subassembly Deciding whether a subassembly is a part (it has bodies of its own) scanned every body of the model for each subassembly, and each body looked up its feature by a linear search of the timeline: subassemblies × bodies × features. assembly.bom now indexes the placements, the body owners (Document::body_owners: one pass over the bodies with the timeline in a map, the same rule as model_body_component) and the numbered components once per call. 600 bodies and features with 60 subassemblies: 8.1 ms → 1.3 ms; the old cost grows with the cube of the design's size. Co-Authored-By: Claude Opus 5.5 (1M context) <noreply@anthropic.com> --------- Co-authored-by: Claude Opus 5.5 (1M context) <noreply@anthropic.com> Co-authored-by: bherbruck <brennen.herbruck@gmail.com>
653 lines
30 KiB
Rust
653 lines
30 KiB
Rust
//! 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 = solidcraft_io::vfs::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 },
|
|
..Default::default()
|
|
});
|
|
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, ..Default::default() });
|
|
doc.components.push(solidcraft_doc::Component { id: 3002, name: "Foot".into(), parent: 3001, ..Default::default() });
|
|
// 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);
|
|
}
|