Joints: a loop whose axes are a hair out of parallel still moves (#6211) (#6602)

* claim: #6211

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

* Joints: a loop whose axes are a hair out of parallel still moves (#6211)

Holes picked on parts are rarely exactly in line. A closed loop through
axes a few hundredths of a microradian apart (a lever, link and a rod on
a cylindrical axle) can't close exactly: it closes to nanometres, and the
gap grows as it turns. The loop solver wanted 1e-7 mm, so it threw such a
closure away (the loop stayed open, its parts at 0 dof) or stopped a drive
or drag after a fraction of a degree.

The conflict check is now 1e-4 mm / 1e-6 rad (it was below what its
message can show), and joint.drive / motion and joint.reach close the
loop at each step as at the start: to the tight tolerance where they get
there, otherwise within the conflict check.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

---------

Co-authored-by: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
yhamil04andClaude Opus 5.5 authored and GitHub committed 2026-10-26 11:10:45 +00:00
1 parent a3483ed451
commit 3afcd69a5c
3 files changed
+77 -12

No files matched your search

+14 -8
View File
@@ -1393,9 +1393,11 @@ fn closure_gaps(workspace: &mut PlacementWorkspace, closing: &[u64], vals: &Valu
}
/// 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;
/// this many millimetres or turned by more than this many radians (a micrometre over a metre).
/// Holes picked on parts are rarely exactly in line: axes a few hundredths of a microradian
/// apart (#6211) are not a conflict, and the loop through them still moves.
const CONFLICT_MM: f64 = 1e-4;
const CONFLICT_RAD: f64 = 1e-6;
/// The largest gap (`closure_gaps`, for loops of size `scale`) that still passes the conflict
/// check, with room to spare.
@@ -1405,8 +1407,9 @@ fn loose_gap(scale: f64) -> f64 {
/// 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.
/// do come. Faces modelled a few nanometres apart along parallel axes, or axes a hair out of
/// parallel, leave a gap no value of a planar loop takes out (the second one that grows as the
/// loop turns); the loop still moves, as closed as it comes. `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);
@@ -1714,7 +1717,8 @@ fn close_group(
// that close is left as it is.
let mut x = x0;
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))? {
let loose = loose_gap(scale);
let tol = match close_within(&mut |x| gaps(x, 0.0), &mut x, &bounds, tol, loose)? {
t if t > tol && moves.is_empty() && start <= t => return Some(BTreeMap::new()),
t => t,
};
@@ -1726,7 +1730,9 @@ fn close_group(
}
let t1 = (t + dt).min(1.0);
let mut trial = x.clone();
if least_squares(&mut |x| gaps(x, t1), &mut trial, &bounds, tol) {
// Each step as closed as the start, or within the conflict check where the loop's gap
// grows as it moves (axes a hair out of parallel, #6211).
if close_within(&mut |x| gaps(x, t1), &mut trial, &bounds, tol, loose).is_some() {
// A step that moved some loop value too far (through a link and a limit, or close to
// a dead point) is taken again in halves.
let next = effective(&trial, t1);
@@ -1741,7 +1747,7 @@ fn close_group(
dt *= 0.5;
}
}
if largest(&gaps(&x, 1.0)?) > tol {
if largest(&gaps(&x, 1.0)?) > tol.max(loose) {
return None;
}
// The values that changed from what is stored.
+4 -4
View File
@@ -10,7 +10,7 @@ use solidcraft_sketch::linalg;
use super::{
Assembly, Held, MAX_LOOP_VALUES, MAX_LOOP_WORK, MAX_LOOPS, Solution, Tree, Values, apply_links, build, close_within, jacobian, joint_values,
largest, least_squares, limit_bounds, link_chains, loop_groups, loop_scale, loose_gap, unknowns,
largest, limit_bounds, link_chains, loop_groups, loop_scale, loose_gap, unknowns,
};
use crate::{Document, ModelState, apply_point, apply_vector, mat_mul};
@@ -325,7 +325,7 @@ fn stride(asm: &Assembly, vals: &Values, vars: &[(u64, usize)], paced: &[((u64,
/// Levenberg-Marquardt that stays on the loops: move `x` (within `bounds`, at most `cap` per value
/// and a step of every value `stride` measures, links included, a step) to bring the first `mt` values of `f` (the target's misses) toward zero, the rest (the
/// 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
/// step pushes past held, then put back on the loops (`close_within` 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 (within `loose`).
fn approach(
@@ -341,14 +341,14 @@ fn approach(
let n = x.len();
let miss = |r: &[f64]| r.iter().take(mt).map(|v| v * v).sum::<f64>();
// The loops close first, to `tol` or as near as they come within `loose` (`close_within`),
// and stay that closed.
// and stay that closed, or within `loose` where their gap grows as they move (#6211).
let loops = f(x)?.len() > mt;
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)
close_within(&mut |y| f(y).map(|r| r.get(mt..).unwrap_or_default().to_vec()), x, bounds, loop_tol, loose * LOOP_WEIGHT).is_some()
};
let mut r = f(x)?;
let (mut lambda, mut kicked) = (1e-3, false);
+59
View File
@@ -667,6 +667,65 @@ fn slotted_arm_drives(yaw_first: bool, gap: f64) {
assert!(arm > 19.9 && arm <= 20.0 + 1e-9, "yaw first {yaw_first}, gap {gap}: the arm stopped at {arm}°");
}
/// The four-bar with its rocker a rod on an axle (a cylindrical joint), and the rocker pin
/// joining the coupler to the rod 4 mm up the axle from where the rod sits, its axis turned
/// `tilt` rad from the others about Y. Returns the closing joint's answer and the rocker pin.
fn rod_on_an_axle(tilt: f64) -> (Session, Value, Vec3) {
let mut s = Session::default();
let (o2, o4, p0) = (Vec3::ZERO, Vec3::new(45.0, 0.0, 0.0), Vec3::new(0.0, 20.0, 0.0));
let [q0, _] = meet(p0, 50.0, o4, 40.0);
for (name, c) in [("Ground", o2), ("Crank", p0), ("Coupler", (p0 + q0) * 0.5), ("Rod", q0)] {
part(&mut s, name, [c.x, c.y, -5.0]);
}
let g = occ(&s, "Ground");
run(&mut s, "occurrence.ground", json!({"occurrence": g, "grounded": true}));
pin(&mut s, "revolute", "Crank", "Ground", "Crank", o2, Vec3::Z);
pin(&mut s, "revolute", "Crank pin", "Crank", "Coupler", p0, Vec3::Z);
pin(&mut s, "cylindrical", "Axle", "Ground", "Rod", o4, Vec3::Z);
let z = [tilt.sin(), 0.0, tilt.cos()];
let r = run(
&mut s,
"joint.create",
json!({"type": "revolute", "a": {"occurrence": "Coupler", "point": [q0.x, q0.y, 0.0], "z": z},
"b": {"occurrence": "Rod", "point": [q0.x, q0.y, 4.0], "z": z}, "flip": true, "name": "Rocker pin"}),
);
(s, r, q0)
}
/// Axes of holes picked on parts are seldom exactly parallel (#6211: a few hundredths of a
/// microradian apart on a lever). A closed loop through them leaves a gap of nanometres no value
/// takes out; it still closes, the rod sliding down its axle, and follows its crank. A tilt that
/// shows still locks it.
#[test]
fn a_loop_whose_axes_are_a_hair_out_of_parallel_still_moves() {
let (mut s, r, q0) = rod_on_an_axle(1e-7);
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
let up = Vec3::new(0.0, 0.0, 4.0);
let close = |a: Vec3, b: Vec3| a.dist(b) < 1e-4;
assert!(close(place(&s, "Rod", q0 + up), q0), "{:?}", place(&s, "Rod", q0 + up));
for part in ["Crank", "Coupler", "Rod"] {
assert_eq!(dof_of(&mut s, part), 1, "{part}");
}
for v in [30.0, 60.0, -45.0] {
let r = run(&mut s, "joint.drive", json!({"joint": "Crank", "value": v}));
assert!(r["stopped_by_loop"].is_null() && r["conflicts"].as_array().map(Vec::len) == Some(0), "{v}°: {r}");
let q = rocker_pin(0.0, v, q0);
assert!(close(place(&s, "Rod", q0 + up), q), "{v}°: {:?} wants {q:?}", place(&s, "Rod", q0 + up));
}
// Dragged by its crank pin (the viewport's drag of a part joints hold), the rod follows too.
let (at, th) = (place(&s, "Crank", Vec3::new(0.0, 20.0, 0.0)), 110f64.to_radians());
let r = run(&mut s, "joint.reach", json!({"occurrence": "Crank", "at": [at.x, at.y, at.z], "point": [20.0 * th.cos(), 20.0 * th.sin(), 0.0]}));
assert!(r["reached"] == json!(true) && r["stopped_by_loop"].is_null(), "{r}");
let q = rocker_pin(0.0, 20.0, q0);
assert!(close(place(&s, "Rod", q0 + up), q), "reach: {:?} wants {q:?}", place(&s, "Rod", q0 + up));
// Tilted a milliradian, the loop is locked: no longer planar, it can't turn.
let (mut s, r, _) = rod_on_an_axle(1e-3);
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
assert_eq!(dof_of(&mut s, "Rod"), 0);
let r = run(&mut s, "joint.drive", json!({"joint": "Crank", "value": 30}));
assert!(r["stopped_by_loop"].is_object() && r["values"][0]["value"].as_f64().is_some_and(|v| v.abs() < 0.1), "{r}");
}
/// 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}})