NOWN-DEV
08b6a8fe0f
On Windows the temporary folder often has an 8.3 short name, which the path scope refuses. vfs::long_names spells folders SolidCraft derives from the environment with long names, and the MCP scratch folder and the recovery folder use it, so screenshot without a path and the recovery commands work there again.
648 lines
29 KiB
Rust
648 lines
29 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 } });
|
|
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);
|
|
}
|