Skip to content

SolidCraft: an open-source parametric 3D CAD application built in pure Rust. Desktop, web, and automation through MCP.

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

yhamil043afcd69a5c36
diff --git a/crates/doc/src/joints.rs b/crates/doc/src/joints.rs
index 257c7a49ca..ff7f93e068 100644
--- a/crates/doc/src/joints.rs
+++ b/crates/doc/src/joints.rs
@@ -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.
diff --git a/crates/doc/src/joints_reach.rs b/crates/doc/src/joints_reach.rs
index 2274bfc10a..7e87749e36 100644
--- a/crates/doc/src/joints_reach.rs
+++ b/crates/doc/src/joints_reach.rs
@@ -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);
diff --git a/crates/engine/src/cmd/joints_tests.rs b/crates/engine/src/cmd/joints_tests.rs
index 095f5856f8..c9d22a9313 100644
--- a/crates/engine/src/cmd/joints_tests.rs
+++ b/crates/engine/src/cmd/joints_tests.rs
@@ -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}})