From 34afcf75c15dcd2d5da459ed41f43c6d085130f5 Mon Sep 17 00:00:00 2001 From: SS-EN <84559909+SS-EN@users.noreply.github.com> Date: Tue, 20 Oct 2026 16:41:37 +0000 Subject: [PATCH] Joints: a loop that meets only to within nanometres still moves (#3475) A closed loop whose joints meet a few nanometres apart along their parallel axes (faces modelled that far apart) passes the conflict check, but the loop solver wanted 1e-7 mm before it moved anything. No value of a planar loop takes such a gap out, so every drive of it stopped at once (stopped_by_loop, fraction 0) and joint.reach called the loop open. The solver now closes such a loop as far as it comes and, when what is left is within the conflict check, keeps it that closed on the way. joint.drive, joint.reach and the nearby pose of the dof count share it. Co-authored-by: Claude Opus 5.5 --- crates/doc/src/joints.rs | 39 +++++++++-- crates/doc/src/joints_reach.rs | 24 ++++--- crates/engine/src/cmd/joints_tests.rs | 98 +++++++++++++++++++++++++++ 3 files changed, 145 insertions(+), 16 deletions(-) diff --git a/crates/doc/src/joints.rs b/crates/doc/src/joints.rs index a099d7963..f929d1714 100644 --- a/crates/doc/src/joints.rs +++ b/crates/doc/src/joints.rs @@ -1235,6 +1235,29 @@ fn closure_gaps(tree: &Tree, closing: &[u64], vals: &Values, scale: f64, out: &m Some(()) } +/// How far a closing joint may be off before it is a conflict: its frames apart by more than +/// this many millimetres or turned by more than this many radians. +const CONFLICT_MM: f64 = 1e-6; +const CONFLICT_RAD: f64 = 1e-8; + +/// The largest gap (`closure_gaps`, for loops of size `scale`) that still passes the conflict +/// check, with room to spare. +fn loose_gap(scale: f64) -> f64 { + CONFLICT_MM.min(CONFLICT_RAD * scale) / 4.0 +} + +/// Close loops whose gaps `f` gives from `x`, and the tolerance to keep them closed to from here +/// on: `tol`, or, where they come no closer than that but within `loose`, a little more than they +/// do come. Faces modelled a few nanometres apart along parallel axes leave a gap no value of a +/// planar loop takes out; the loop still moves, as closed as it is. `None` when they can't close. +fn close_within(f: &mut dyn FnMut(&[f64]) -> Option>, x: &mut Vec, bounds: &[(f64, f64)], tol: f64, loose: f64) -> Option { + if least_squares(f, x, bounds, tol) { + return Some(tol); + } + let left = largest(&f(x)?); + (left <= loose).then(|| (4.0 * left).clamp(tol, loose.max(tol))) +} + /// Occurrences whose degrees of freedom are worked out through their loops, at most; the rest /// keep the tree's count. const MAX_DOF_OCCURRENCES: usize = 256; @@ -1287,7 +1310,7 @@ fn nearby_pose(tree: &Tree, groups: &[Group], asm: &Assembly, linked: &BTreeSet< closure_gaps(tree, closing, &work, scale, &mut out)?; out.iter().all(|v| v.is_finite()).then_some(out) }; - if least_squares(&mut gaps, &mut x, &bounds, (1e-10 * scale).min(1e-7)) + if close_within(&mut gaps, &mut x, &bounds, (1e-10 * scale).min(1e-7), loose_gap(scale)).is_some() && x.iter().zip(&x0).zip(&step).all(|((a, b), d)| (a - b).abs() <= 4.0 * d) && gaps(&x).is_some() { @@ -1525,11 +1548,15 @@ fn close_group( if moves.is_empty() && gaps(&x0, 1.0).is_some_and(|r| largest(&r) <= tol) { return Some(BTreeMap::new()); } - // A loop open at the start closes first. + // A loop open at the start closes first. One that comes only to within the conflict check + // (`close_within`) moves on as closed as it is; with nothing held moving, one that started + // that close is left as it is. let mut x = x0; - if largest(&gaps(&x, 0.0)?) > tol && !least_squares(&mut |x| gaps(x, 0.0), &mut x, &bounds, tol) { - return None; - } + let start = largest(&gaps(&x, 0.0)?); + let tol = match close_within(&mut |x| gaps(x, 0.0), &mut x, &bounds, tol, loose_gap(scale))? { + t if t > tol && moves.is_empty() && start <= t => return Some(BTreeMap::new()), + t => t, + }; let (mut t, mut dt) = (if moves.is_empty() { 1.0 } else { 0.0 }, first); let mut at = effective(&x, t); while t < 1.0 { @@ -1680,7 +1707,7 @@ pub fn solve_holding(doc: &Document, st: &ModelState, held: &Held) -> Solution { let (Some(want), Some((_, fb))) = (tree.step(*id, true, ta, &vals), tree.frames.get(id)) else { continue }; // Measured at the joint, where the loop solver closes it. let (dt, da) = mismatch(&mat_mul(&want, fb), &mat_mul(tb, fb)); - if dt > 1e-6 || da > 1e-8 { + if dt > CONFLICT_MM || da > CONFLICT_RAD { sol.conflicts.push(( j.id, j.name.clone(), diff --git a/crates/doc/src/joints_reach.rs b/crates/doc/src/joints_reach.rs index 07bd137f4..4a5f4fbf2 100644 --- a/crates/doc/src/joints_reach.rs +++ b/crates/doc/src/joints_reach.rs @@ -9,8 +9,8 @@ use solidcraft_geom::Vec3; use solidcraft_sketch::linalg; use super::{ - Assembly, Held, MAX_LOOP_VALUES, MAX_LOOP_WORK, MAX_LOOPS, Solution, Tree, Values, apply_links, build, closure_gaps, jacobian, joint_values, - largest, least_squares, limit_bounds, link_chains, loop_groups, loop_scale, unknowns, + Assembly, Held, MAX_LOOP_VALUES, MAX_LOOP_WORK, MAX_LOOPS, Solution, Tree, Values, apply_links, build, close_within, closure_gaps, jacobian, + joint_values, largest, least_squares, limit_bounds, link_chains, loop_groups, loop_scale, loose_gap, unknowns, }; use crate::{Document, ModelState, apply_point, apply_vector, mat_mul}; @@ -159,7 +159,7 @@ pub fn reach(doc: &Document, st: &ModelState, r: &Reach) -> Result().sqrt(); let before = f(&x0).map_or(f64::INFINITY, |r| miss(&r)); - let res = approach(&mut f, &|a, b| stride(asm, &vals, &vars, &paced, a, b), mt, &mut x, &bounds, &cap, tol) + let res = approach(&mut f, &|a, b| stride(asm, &vals, &vars, &paced, a, b), mt, &mut x, &bounds, &cap, tol, loose_gap(scale)) .ok_or("a closed loop on its chain is open and can't close (joint.list shows its conflict)")?; let mut out = Reached::default(); let point_miss = res.iter().take(3).map(|v| v * v).sum::().sqrt(); @@ -219,7 +219,7 @@ pub fn reach(doc: &Document, st: &ModelState, r: &Reach) -> Result Option>, stride: &dyn Fn(&[f64], &[f64]) -> f64, @@ -322,16 +322,20 @@ fn approach( bounds: &[(f64, f64)], cap: &[f64], tol: f64, + loose: f64, ) -> Option> { let n = x.len(); let miss = |r: &[f64]| r.iter().take(mt).map(|v| v * v).sum::(); - let close = |f: &mut dyn FnMut(&[f64]) -> Option>, x: &mut Vec| { - least_squares(&mut |y| f(y).map(|r| r.get(mt..).unwrap_or_default().to_vec()), x, bounds, tol * LOOP_WEIGHT) - }; + // The loops close first, to `tol` or as near as they come within `loose` (`close_within`), + // and stay that closed. let loops = f(x)?.len() > mt; - if loops && !close(f, x) { - return None; + let mut loop_tol = tol * LOOP_WEIGHT; + if loops { + loop_tol = close_within(&mut |y| f(y).map(|r| r.get(mt..).unwrap_or_default().to_vec()), x, bounds, loop_tol, loose * LOOP_WEIGHT)?; } + let close = |f: &mut dyn FnMut(&[f64]) -> Option>, x: &mut Vec| { + least_squares(&mut |y| f(y).map(|r| r.get(mt..).unwrap_or_default().to_vec()), x, bounds, loop_tol) + }; let mut r = f(x)?; let (mut lambda, mut kicked) = (1e-3, false); for _ in 0..MAX_REACH_STEPS { diff --git a/crates/engine/src/cmd/joints_tests.rs b/crates/engine/src/cmd/joints_tests.rs index af2590786..736757088 100644 --- a/crates/engine/src/cmd/joints_tests.rs +++ b/crates/engine/src/cmd/joints_tests.rs @@ -549,6 +549,104 @@ fn slider_crank_moves_the_piston() { } } +/// A slotted-arm drive: a clamp turns about the origin (Yaw) and carries a pin 26 from it; an arm +/// turns about a shaft 52 out on a motor fixed to the ground, and the pin slides in the arm's slot +/// (Slot: its frame at the shaft, x along the slot towards the pin). Every joint of the loop +/// moves when it does, the slot's included. The pin's face sits `gap` mm off the arm's along +/// the axes: a gap no value of the loop takes out, within the conflict check. +fn slotted_arm(yaw_first: bool, gap: f64) -> Session { + let mut s = Session::default(); + for (name, x) in [("Ground", 0.0), ("Clamp", 26.0), ("Motor", 52.0), ("Arm", 40.0)] { + part(&mut s, name, [x - 1.0, -1.0, -5.0]); + } + let g = occ(&s, "Ground"); + run(&mut s, "occurrence.ground", json!({"occurrence": g, "grounded": true})); + // The joint order sets the tree: with the yaw first, the arm hangs from the clamp through the + // slot and the arm's shaft closes the loop; with the motor first, the slot closes it. + if yaw_first { + pin(&mut s, "revolute", "Yaw", "Ground", "Clamp", Vec3::ZERO, Vec3::Z); + } + pin(&mut s, "rigid", "Mount", "Ground", "Motor", Vec3::new(52.0, 0.0, 0.0), Vec3::Z); + if !yaw_first { + pin(&mut s, "revolute", "Yaw", "Ground", "Clamp", Vec3::ZERO, Vec3::Z); + } + pin(&mut s, "revolute", "Shaft", "Arm", "Motor", Vec3::new(52.0, 0.0, 0.0), Vec3::Z); + let r = run( + &mut s, + "joint.create", + json!({"type": "pin_slot", "a": framed("Arm", [52.0, 0.0, 0.0], [0.0, 0.0, 1.0], [-1.0, 0.0, 0.0]), + "b": framed("Clamp", [26.0, 0.0, gap], [0.0, 0.0, 1.0], [-1.0, 0.0, 0.0]), "flip": true, "values": [0, 26], "name": "Slot"}), + ); + assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}"); + s +} + +/// A joint's stored values. +fn values_of(s: &Session, joint: &str) -> Vec { + s.doc.assembly.joints.iter().find(|j| j.name == joint).map(|j| j.values.clone()).unwrap_or_default() +} + +/// Driving either end of the slotted arm moves the other: the arm turns atan(26 sin θ / (52 − +/// 26 cos θ)) for a yaw θ, and the pin slides out from 26 to √(26² + 52² − 2·26·52 cos θ). +#[test] +fn slotted_arm_drives_both_ways() { + for (yaw_first, gap) in [(false, 0.0), (true, 0.0), (false, 3e-8), (true, 3e-8)] { + slotted_arm_drives(yaw_first, gap); + } +} + +fn slotted_arm_drives(yaw_first: bool, gap: f64) { + let mut s = slotted_arm(yaw_first, gap); + let (shaft, pin_at) = (Vec3::new(52.0, 0.0, 0.0), Vec3::new(26.0, 0.0, 0.0)); + let check = |s: &Session, what: &str| { + let th = values_of(s, "Yaw").first().copied().unwrap_or(f64::NAN); + let p = Vec3::new(26.0 * th.cos(), 26.0 * th.sin(), 0.0); + assert!(near(place(s, "Clamp", pin_at), p), "{what}: pin {:?} wants {p:?}", place(s, "Clamp", pin_at)); + let arm = values_of(s, "Shaft").first().copied().unwrap_or(f64::NAN); + let want = (26.0 * th.sin()).atan2(52.0 - 26.0 * th.cos()); + assert!((arm.abs() - want.abs()).abs() < 1e-6, "{what}: arm {arm} wants ±{want}"); + // The arm's slot points at the pin, which sits as far out as the slot says. + let tip = place(s, "Arm", Vec3::new(40.0, 0.0, 0.0)); + let along = (p - shaft) * (1.0 / (p - shaft).len()); + assert!(near(tip, shaft + along * 12.0), "{what}: arm tip {tip:?}"); + let slide = values_of(s, "Slot").get(1).copied().unwrap_or(f64::NAN); + assert!((slide.abs() - (p - shaft).len()).abs() < 1e-6, "{what}: slide {slide} wants {}", (p - shaft).len()); + }; + check(&s, &format!("yaw first {yaw_first}, gap {gap}, at rest")); + for v in [10.0, 40.0, -40.0, 0.0] { + let r = run(&mut s, "joint.drive", json!({"joint": "Yaw", "value": v})); + assert!( + r["stopped_by_loop"].is_null() && r["conflicts"].as_array().map(Vec::len) == Some(0), + "yaw first {yaw_first}, gap {gap}, yaw {v}°: {r}" + ); + assert!((values_of(&s, "Yaw").first().copied().unwrap_or(f64::NAN) - f64::to_radians(v)).abs() < 1e-9); + check(&s, &format!("yaw first {yaw_first}, gap {gap}, yaw {v}°")); + } + let slide = values_of(&s, "Slot").get(1).copied().unwrap_or(f64::NAN).abs(); + assert!((slide - 26.0).abs() < 1e-6, "{slide}"); + for v in [15.0, -27.0, 27.0] { + let r = run(&mut s, "joint.drive", json!({"joint": "Shaft", "value": v})); + assert!( + r["stopped_by_loop"].is_null() && r["conflicts"].as_array().map(Vec::len) == Some(0), + "yaw first {yaw_first}, gap {gap}, arm {v}°: {r}" + ); + assert!((values_of(&s, "Shaft").first().copied().unwrap_or(f64::NAN) - f64::to_radians(v)).abs() < 1e-9); + check(&s, &format!("yaw first {yaw_first}, gap {gap}, arm {v}°")); + } + // The clamp's pin reaches a point on its circle, the arm following. + let (th, at) = (20f64.to_radians(), place(&s, "Clamp", pin_at)); + let r = run(&mut s, "joint.reach", json!({"occurrence": "Clamp", "at": [at.x, at.y, at.z], "point": [26.0 * th.cos(), 26.0 * th.sin(), 0.0]})); + assert!(r["reached"] == json!(true) && r["stopped_by_loop"].is_null(), "yaw first {yaw_first}, gap {gap}, reach: {r}"); + check(&s, &format!("yaw first {yaw_first}, gap {gap}, reach")); + // A limit on the arm stops the yaw where the arm meets it. + run(&mut s, "joint.limits", json!({"joint": "Shaft", "min": -20, "max": 20})); + let r = run(&mut s, "joint.drive", json!({"joint": "Yaw", "value": 40})); + assert!(r["stopped_by_loop"].is_object() && r["conflicts"].as_array().map(Vec::len) == Some(0), "yaw first {yaw_first}, gap {gap}, limit: {r}"); + check(&s, &format!("yaw first {yaw_first}, gap {gap}, limit")); + let arm = values_of(&s, "Shaft").first().copied().unwrap_or(f64::NAN).abs().to_degrees(); + assert!(arm > 19.9 && arm <= 20.0 + 1e-9, "yaw first {yaw_first}, gap {gap}: the arm stopped at {arm}°"); +} + /// A joint frame on an occurrence, by origin and axes. fn framed(o: &str, origin: [f64; 3], z: [f64; 3], x: [f64; 3]) -> Value { json!({"occurrence": o, "frame": {"origin": origin, "z": z, "x": x}})