Files
solidcraft/crates/engine/src/cmd/urdf_tests.rs
T
Karl Von Richter 0dffb206b1 Assembly: ground the first part; drag free joint chains (#2804) (#2805)
* assembly(feat): ground first parts and drag free joint chains

Ground the first top-level part when it is created and let face dragging reposition free occurrences or any member of a floating joint chain in one undo step. Preserve exact repeated-placement frames, joint values, modifier selection and Escape cancellation. Keep the accumulated fixture, viewport layout and documentation integration fixes from the existing PR.

Signed-off-by: Karl Von Richter<karl@embeint.com>

* assembly(fix): update movable fixtures for first-part grounding

Accept the documented grounded override in component creation, explicitly free movement fixtures, and keep linked clipboard pastes movable even in an empty document. Cover first and reused linked pastes, default insertion grounding, and single-step undo in the existing link tests.

Signed-off-by: Karl Von Richter<karl@embeint.com>

* assembly(test): keep the motion-cost crank free

Unground the first component in the document-level joint-face measurement fixture so its motion samples can rotate after first-part grounding. Preserve the rotation and shared-measurement assertions that caught the Linux CI failure.

Signed-off-by: Karl Von Richter<karl@embeint.com>

* assembly(test): free the frame placement fixture

Create the frame-member placement fixture ungrounded before moving it above its root sketch. Keep its world-space member assertions while respecting first-part grounding in the frame tests added on main.

Signed-off-by: Karl Von Richter<karl@embeint.com>

---------

Signed-off-by: Karl Von Richter<karl@embeint.com>
2026-10-23 23:15:34 +00:00

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, "grounded": false}));
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);
}