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 <noreply@anthropic.com>
This commit is contained in:
SS-EN
2026-10-20 16:41:37 +00:00
committed by GitHub
parent 932d8d6b4f
commit 34afcf75c1
3 changed files with 145 additions and 16 deletions
+33 -6
View File
@@ -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<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(),
+14 -10
View File
@@ -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, Stri
let 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, Stri
let 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 {
+98
View File
@@ -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}})