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}})