SolidCraft: an open-source parametric 3D CAD application built in pure Rust. Desktop, web, and automation through MCP.
Joints: a loop that meets only to within nanometres still moves (#3475)
SS-EN34afcf75c15d
diff --git a/crates/doc/src/joints.rs b/crates/doc/src/joints.rsindex a099d79637..f929d1714d 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: &mSome(())}+/// 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<Vec<f64>>, x: &mut Vec<f64>, bounds: &[(f64, f64)], tol: f64, loose: f64) -> Option<f64> {+ 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.rsindex 07bd137f42..4a5f4fbf20 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<Reached, Strilet mut x = x0.clone();let miss = |res: &[f64]| res.iter().take(mt).map(|v| v * v).sum::<f64>().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::<f64>().sqrt();@@ -219,7 +219,7 @@ pub fn reach(doc: &Document, st: &ModelState, r: &Reach) -> Result<Reached, Strilet mut g = |x: &[f64]| misses(tree, asm, r, scale, &drive, &[], x, &mut alone, &spent);let (mut xo, bo, co) = (x0.get(..k).unwrap_or_default().to_vec(), bounds.get(..k).unwrap_or_default(), cap.get(..k).unwrap_or_default());// The loops left open, nothing to pace.- if let Some(ro) = approach(&mut g, &|_, _| 0.0, mt, &mut xo, bo, co, tol)+ if let Some(ro) = approach(&mut g, &|_, _| 0.0, mt, &mut xo, bo, co, tol, loose_gap(scale))&& miss(&ro) < miss(&res) - REACH_TOL.max(1e-9 * miss(&res)){out.stopped_by_loop = Some(*first);@@ -313,7 +313,7 @@ fn stride(asm: &Assembly, vals: &Values, vars: &[(u64, usize)], paced: &[((u64,/// loops' weighted gaps) at zero. Each step is solved over all the rows, values on a limit the/// step pushes past held, then put back on the loops (`least_squares` on their gaps) and taken/// when it gets closer. It ends at the target, or as close as the limits and loops let it get.-/// Returns `f` there, or `None` when the loops don't close at the start.+/// Returns `f` there, or `None` when the loops don't close at the start (within `loose`).fn approach(f: &mut dyn FnMut(&[f64]) -> Option<Vec<f64>>,stride: &dyn Fn(&[f64], &[f64]) -> f64,@@ -322,16 +322,20 @@ fn approach(bounds: &[(f64, f64)],cap: &[f64],tol: f64,+ loose: f64,) -> Option<Vec<f64>> {let n = x.len();let miss = |r: &[f64]| r.iter().take(mt).map(|v| v * v).sum::<f64>();- let close = |f: &mut dyn FnMut(&[f64]) -> Option<Vec<f64>>, x: &mut Vec<f64>| {- 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<Vec<f64>>, x: &mut Vec<f64>| {+ 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.rsindex af2590786d..7367570883 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<f64> {+ 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}})