* 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:
1 parent
a3483ed451
commit
3afcd69a5c
3 files changed
+77
-12
No files matched your search
@@ -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.
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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}})
|
||||
|
||||
Reference in new issue
Block a user