Joints: link offsets in b's units, new joints start inside their limits, loop dof at dead points (#1415)

joint.motion_link offset is read in the linked value's units (degrees or mm, or an expression); links are still stored in radians, so saved designs open unchanged. A new joint whose values fall outside its limits starts at the nearer limit, stores it, and says so in warnings. Loop dof also counts at a nearby pose, so a part at a dead point of its loop keeps its mobility. The mechanisms skill is updated to match.
This commit is contained in:
Mateusz Sych authored and GitHub committed 2026-10-15 12:45:48 -04:00
1 parent d0a4443939
commit c2ba3657f3
6 files changed
+270 -66

No files matched your search

+114 -41
View File
@@ -624,6 +624,16 @@ fn unknowns(tree: &Tree, around: &BTreeSet<u64>, asm: &Assembly, linked: &BTreeS
vars
}
/// Each value's limits, or none.
fn limit_bounds(tree: &Tree, vars: &[(u64, usize)]) -> Vec<(f64, f64)> {
vars.iter()
.map(|(id, i)| match tree.joints.get(id).and_then(|j| j.limits.get(*i).copied().flatten()) {
Some((lo, hi)) => (lo.min(hi), hi.max(lo)),
None => (f64::NEG_INFINITY, f64::INFINITY),
})
.collect()
}
/// The loops' size: the diagonal of the box around their joints' A frames, 1 mm to 1 km. Points
/// that far apart weigh turns like moves.
fn loop_scale(tree: &Tree, around: &BTreeSet<u64>, vals: &Values) -> f64 {
@@ -658,11 +668,72 @@ fn closure_gaps(tree: &Tree, closing: &[u64], vals: &Values, scale: f64, out: &m
/// keep the tree's count.
const MAX_DOF_OCCURRENCES: usize = 256;
/// How far `nearby_pose` moves each value: radians for an angle, a share of the loops' size for a
/// distance. Far enough for a part's motion that vanishes at a dead point to show in a rank, near
/// enough to stay on the mechanism's branch.
const NUDGE: f64 = 1e-3;
/// A pose of the loops near `vals`: each value they move nudged by an amount of its own within
/// its limits (so the nudge points in no special direction), then each group of loops closed
/// again. A group that can't close again, or only further away than a few nudges (on another
/// branch), keeps its values. It has a work budget of its own.
fn nearby_pose(tree: &Tree, groups: &[Group], asm: &Assembly, linked: &BTreeSet<(u64, usize)>, vals: &Values) -> Values {
let mut near = vals.clone();
let mut spent = 0usize;
for (closing, around, _) in groups {
let vars = unknowns(tree, around, asm, linked, &Held::new());
if vars.is_empty() || vars.len() > MAX_LOOP_VALUES {
continue;
}
let scale = loop_scale(tree, around, vals);
let step: Vec<f64> =
vars.iter().map(|(id, i)| NUDGE * if tree.joints.get(id).is_some_and(|j| j.kind.is_angle(*i)) { 1.0 } else { scale }).collect();
let bounds = limit_bounds(tree, &vars);
let x0: Vec<f64> = vars.iter().map(|(id, i)| near.get(id).and_then(|q| q.get(*i)).copied().unwrap_or(0.0)).collect();
let mut x: Vec<f64> = x0
.iter()
.zip(step.iter().zip(&bounds))
.enumerate()
.map(|(k, (v, (d, (lo, hi))))| {
// 0.5 to 1.5 nudges, alternately up and down.
let w = 0.5 + (k as f64 * 0.618_034).fract();
(v + d * if k % 2 == 0 { w } else { -w }).max(*lo).min(*hi)
})
.collect();
let mut work = near.clone();
let mut gaps = |x: &[f64]| -> Option<Vec<f64>> {
spent += 1;
if spent > MAX_LOOP_WORK {
return None;
}
for ((id, i), v) in vars.iter().zip(x) {
if let Some(slot) = work.get_mut(id).and_then(|q| q.get_mut(*i)) {
*slot = *v;
}
}
apply_links(asm, &mut work);
let mut out = Vec::with_capacity(closing.len() * 9);
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))
&& x.iter().zip(&x0).zip(&step).all(|((a, b), d)| (a - b).abs() <= 4.0 * d)
&& gaps(&x).is_some()
{
near = work;
}
}
near
}
/// The degrees of freedom of the occurrences loops constrain: how many independent ways each can
/// still move with every loop kept closed, the rank of its placement over the joint values once
/// the loops' closure is taken out (rank [J_loops; J_placement] - rank J_loops). An occurrence's
/// values are those on its tree path and the unknowns of the loops on that path. The tree's
/// count stands where a loop group is over the solver's caps or the work runs out.
/// values are those on its tree path and the unknowns of the loops on that path. It is counted at
/// the pose and at a pose nearby, and the larger count stands: at a dead point of its loop (an
/// actuator in line with the path of its tip pin) a part's motion vanishes to first order, so the
/// pose alone counts 0. The tree's count stands where a loop group is over the solver's caps or
/// the work runs out.
fn loop_dof(tree: &Tree, closing: &[u64], asm: &Assembly, vals: &Values, dof: &mut BTreeMap<u64, usize>) {
if closing.len() > MAX_LOOPS {
return;
@@ -690,6 +761,8 @@ fn loop_dof(tree: &Tree, closing: &[u64], asm: &Assembly, vals: &Values, dof: &m
}
}
let (mut spent, mut counted) = (0usize, 0usize);
// A pose nearby, when one differs.
let near = (!candidates.is_empty()).then(|| nearby_pose(tree, &groups, asm, &linked, vals)).filter(|n| n != vals);
for occ in tree.order.iter().filter(|o| candidates.contains(o)) {
let path: Vec<u64> = tree.path(*occ).1.iter().map(|(k, _)| *k).collect();
let (mut values, mut have): (Vec<(u64, usize)>, BTreeSet<(u64, usize)>) = (Vec::new(), BTreeSet::new());
@@ -751,41 +824,47 @@ fn loop_dof(tree: &Tree, closing: &[u64], asm: &Assembly, vals: &Values, dof: &m
continue;
}
let scale = loop_scale(tree, &around, vals);
let mut work = vals.clone();
// The loops' gaps, then three points of the occurrence `scale` apart.
let mut f = |x: &[f64]| -> Option<Vec<f64>> {
spent += 1;
for ((id, i), v) in values.iter().zip(x) {
if let Some(slot) = work.get_mut(id).and_then(|q| q.get_mut(*i)) {
*slot = *v;
}
}
apply_links(asm, &mut work);
let mut out = Vec::with_capacity(closers.len() * 9 + 9);
closure_gaps(tree, &closers, &work, scale, &mut out)?;
let t = tree.world(*occ, &work)?;
for p in [Vec3::ZERO, Vec3::X * scale, Vec3::Y * scale] {
let q = apply_point(&t, p);
out.extend([q.x, q.y, q.z]);
}
out.iter().all(|v| v.is_finite()).then_some(out)
};
let x0: Vec<f64> = values.iter().map(|(id, i)| vals.get(id).and_then(|q| q.get(*i)).copied().unwrap_or(0.0)).collect();
let (n, gaps) = (values.len(), closers.len() * 9);
let Some(jac) = jacobian(&mut f, &x0, gaps + 9) else { continue };
let rank = |rows: usize| {
let mut a = linalg::Mat::zeros(rows, n);
let mut big = 0.0f64;
for r in 0..rows {
for c in 0..n {
let v = jac.get(r * n + c).copied().unwrap_or(0.0);
big = big.max(v.abs());
a.set(r, c, v);
// How many ways the occurrence moves at a pose, with the loops kept closed.
let mut free_at = |at: &Values| -> Option<usize> {
let mut work = at.clone();
// The loops' gaps, then three points of the occurrence `scale` apart.
let mut f = |x: &[f64]| -> Option<Vec<f64>> {
spent += 1;
for ((id, i), v) in values.iter().zip(x) {
if let Some(slot) = work.get_mut(id).and_then(|q| q.get_mut(*i)) {
*slot = *v;
}
}
}
linalg::rref(&mut a, 1e-6 * big.max(1.0)).len()
apply_links(asm, &mut work);
let mut out = Vec::with_capacity(closers.len() * 9 + 9);
closure_gaps(tree, &closers, &work, scale, &mut out)?;
let t = tree.world(*occ, &work)?;
for p in [Vec3::ZERO, Vec3::X * scale, Vec3::Y * scale] {
let q = apply_point(&t, p);
out.extend([q.x, q.y, q.z]);
}
out.iter().all(|v| v.is_finite()).then_some(out)
};
let x0: Vec<f64> = values.iter().map(|(id, i)| at.get(id).and_then(|q| q.get(*i)).copied().unwrap_or(0.0)).collect();
let jac = jacobian(&mut f, &x0, gaps + 9)?;
let rank = |rows: usize| {
let mut a = linalg::Mat::zeros(rows, n);
let mut big = 0.0f64;
for r in 0..rows {
for c in 0..n {
let v = jac.get(r * n + c).copied().unwrap_or(0.0);
big = big.max(v.abs());
a.set(r, c, v);
}
}
linalg::rref(&mut a, 1e-6 * big.max(1.0)).len()
};
Some(rank(gaps + 9).saturating_sub(rank(gaps)))
};
let free = rank(gaps + 9).saturating_sub(rank(gaps));
let here = free_at(vals);
let there = near.as_ref().and_then(&mut free_at);
let Some(free) = here.max(there) else { continue };
dof.insert(*occ, free.min(6));
}
}
@@ -814,13 +893,7 @@ fn close_group(
if vars.len() > MAX_LOOP_VALUES {
return Some(BTreeMap::new());
}
let bounds: Vec<(f64, f64)> = vars
.iter()
.map(|(id, i)| match tree.joints.get(id).and_then(|j| j.limits.get(*i).copied().flatten()) {
Some((lo, hi)) => (lo.min(hi), hi.max(lo)),
None => (f64::NEG_INFINITY, f64::INFINITY),
})
.collect();
let bounds = limit_bounds(tree, &vars);
let scale = loop_scale(tree, around, vals);
// Closer than the conflict check needs: 1e-7 mm, and turns of 1e-10 rad across the loop.
let tol = (1e-10 * scale).min(1e-7);
+46 -5
View File
@@ -39,7 +39,7 @@ pub static COMMANDS: &[CommandSpec] = &[
CommandSpec::new("joint.motion_link", "Motion Link", motion_link)
.at("SOLID", "ASSEMBLE")
.icon("motion_link")
.params("a, b: joints; ratio (b = ratio · a + offset); offset?; ia?, ib?: value indices (default 0)"),
.params("a, b: joints; ratio (b = ratio · a + offset); offset?: in b's units (deg for an angle, mm for a distance, or an expression); ia?, ib?: value indices (default 0)"),
CommandSpec::new("joint.limits", "Joint Limits", limits).params("joint; index?: value index (default 0); min?, max? (omit both to clear)"),
CommandSpec::new("joint.edit", "Edit Joint", edit).params("joint; type?, flip?, offset?, angle?, suppressed?, name?"),
CommandSpec::new("joint.delete", "Delete Joint", delete).params("joint: id|name (or origin: name, link: index)"),
@@ -314,7 +314,7 @@ fn next_joint_id(s: &Session) -> u64 {
s.doc.assembly.joints.iter().map(|j| j.id).max().unwrap_or(0) + 1
}
fn add_joint(s: &mut Session, j: Joint, p: &Value) -> Result<Value> {
fn add_joint(s: &mut Session, mut j: Joint, p: &Value) -> Result<Value> {
if s.doc.assembly.joints.len() >= 10_000 {
return Err(EngineError::Other("too many joints".into()));
}
@@ -327,6 +327,30 @@ fn add_joint(s: &mut Session, j: Joint, p: &Value) -> Result<Value> {
(component.from_bodies)",
));
}
// It starts at its values inside its limits. A value moved to a limit moves the part as it
// is joined, which the answer says.
let mut q = j.values.clone();
q.resize(j.kind.dofs().len(), 0.0);
joints::clamp(&j, &mut q);
let round = |v: &Value| v.as_f64().map(|x| (x * 1e6).round() / 1e6).unwrap_or(f64::NAN);
let warnings: Vec<String> = named_values(&j, &q)
.iter()
.zip(named_values(&j, &j.values))
.filter(|(now, was)| now["value"] != was["value"])
.map(|(now, was)| {
let unit = now["unit"].as_str().unwrap_or("");
format!(
"{} {} starts at {} {unit}, inside its limits [{}, {}], not at {} {unit}: the part moved there as it was joined",
j.name,
now["name"].as_str().unwrap_or(""),
round(&now["value"]),
round(&now["limits"][0]),
round(&now["limits"][1]),
round(&was["value"])
)
})
.collect();
j.values = q;
let id = j.id;
let name = j.name.clone();
// A new joint can take a deleted one's id; it starts shown.
@@ -336,6 +360,9 @@ fn add_joint(s: &mut Session, j: Joint, p: &Value) -> Result<Value> {
let mut out = sol_json(&sol, p);
out["joint"] = json!(id);
out["name"] = json!(name);
if !warnings.is_empty() {
out["warnings"] = json!(warnings);
}
Ok(out)
}
@@ -493,7 +520,12 @@ fn motion_link(s: &mut Session, p: &Value) -> Result<Value> {
if ia >= dofs(a) || ib >= dofs(b) {
return Err(bad(cmd, "the linked joints need moving values at those indices"));
}
let offset = num(p, "offset").unwrap_or(0.0);
// In b's units, like its values and limits (stored in radians for an angle).
let angle = s.doc.assembly.joints.iter().find(|j| j.id == b).is_some_and(|j| j.kind.is_angle(ib));
let offset = match p.get("offset") {
Some(v) => value(s, v, angle, cmd)?,
None => 0.0,
};
s.doc_mut().assembly.links.push(MotionLink { a, ia, b, ib, ratio, offset });
let sol = resolve(s);
Ok(sol_json(&sol, p))
@@ -614,8 +646,17 @@ fn list(s: &mut Session, p: &Value) -> Result<Value> {
"flip": j.flip, "offset": j.offset, "suppressed": j.suppressed, "visible": s.joint_visible(j.id), "frame_a": world(&j.a), "frame_b": world(&j.b)})
})
.collect();
let links: Vec<Value> =
s.doc.assembly.links.iter().map(|l| json!({"a": l.a, "ia": l.ia, "b": l.b, "ib": l.ib, "ratio": l.ratio, "offset": l.offset})).collect();
let links: Vec<Value> = s
.doc
.assembly
.links
.iter()
.map(|l| {
let angle = s.doc.assembly.joints.iter().find(|j| j.id == l.b).is_some_and(|j| j.kind.is_angle(l.ib));
let offset = if angle { l.offset.to_degrees() } else { l.offset };
json!({"a": l.a, "ia": l.ia, "b": l.b, "ib": l.ib, "ratio": l.ratio, "offset": offset, "unit": if angle { "deg" } else { "mm" }})
})
.collect();
let origins: Vec<Value> = s.doc.assembly.origins.iter().map(|o| json!({"name": o.name, "occurrence": o.origin.occurrence})).collect();
let mut out = sol_json(&sol, p);
out["joints"] = json!(js);
+95
View File
@@ -808,6 +808,32 @@ fn loops_report_their_mobility() {
}
}
/// An actuator in line with the path of its tip pin, as modelled: a panel hinged at the origin,
/// its tip pin at (30, 0) moving along Y, and a barrel and rod along Y from a root pin at
/// (30, -40). There the rod only extends and the barrel's turn is zero to first order, but the
/// barrel turns as soon as the panel does: it has one degree of freedom, at that pose too.
#[test]
fn a_part_at_a_dead_point_of_its_loop_keeps_its_mobility() {
let mut s = Session::default();
let (tip, root) = (Vec3::new(30.0, 0.0, 0.0), Vec3::new(30.0, -40.0, 0.0));
for (name, c) in [("Ground", Vec3::new(0.0, -40.0, 0.0)), ("Panel", tip), ("Barrel", root), ("Rod", (tip + root) * 0.5)] {
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", "Hinge", "Ground", "Panel", Vec3::ZERO, Vec3::Z);
pin(&mut s, "revolute", "Root pin", "Ground", "Barrel", root, Vec3::Z);
pin(&mut s, "slider", "Stroke", "Barrel", "Rod", root, Vec3::Y);
pin(&mut s, "revolute", "Tip pin", "Rod", "Panel", tip, Vec3::Z);
for part in ["Panel", "Barrel", "Rod"] {
assert_eq!(dof_of(&mut s, part), 1, "dead point: {part}");
}
run(&mut s, "joint.drive", json!({"joint": "Hinge", "value": 5}));
for part in ["Panel", "Barrel", "Rod"] {
assert_eq!(dof_of(&mut s, part), 1, "driven: {part}");
}
}
/// A design file path in a fresh temporary folder (removed by the caller).
fn temp_design(tag: &str, ext: &str) -> (std::path::PathBuf, String) {
let dir = std::env::temp_dir().join(format!("solidcraft-joint-visibility-{tag}-{}", std::process::id()));
@@ -1054,3 +1080,72 @@ fn a_single_value_keeps_the_joints_other_values() {
let r = run(&mut s, "joint.drive", json!({"joint": "Spindle", "values": [60]}));
assert!((at(&r)[0] - 60.0).abs() < 1e-9 && (at(&r)[1] - 5.0).abs() < 1e-9, "{r}");
}
/// A motion link's offset is in the linked value's units, like joint values and limits: degrees
/// for an angle, mm for a distance, or an expression. `joint.list` answers it the same way, it is
/// stored in radians as before, and a saved link opens with the offset it had.
#[test]
fn a_link_offset_is_in_the_linked_values_units() {
let mut s = Session::default();
for (name, x) in [("Ground", 0.0), ("Dial", 80.0), ("Gear", 100.0), ("Car", 120.0), ("Cart", 140.0)] {
part(&mut s, name, [x, 0.0, -5.0]);
}
let g = occ(&s, "Ground");
run(&mut s, "occurrence.ground", json!({"occurrence": g, "grounded": true}));
for (kind, name, x) in [("revolute", "Dial", 80.0), ("revolute", "Gear", 100.0), ("slider", "Car", 120.0), ("slider", "Cart", 140.0)] {
pin(&mut s, kind, name, "Ground", name, Vec3::new(x, 0.0, 0.0), Vec3::Z);
}
run(&mut s, "joint.motion_link", json!({"a": "Dial", "b": "Gear", "ratio": 1, "offset": 10}));
run(&mut s, "joint.motion_link", json!({"a": "Car", "b": "Cart", "ratio": 1, "offset": "5 mm"}));
run(&mut s, "joint.drive", json!({"joint": "Dial", "value": 20}));
run(&mut s, "joint.drive", json!({"joint": "Car", "value": 3}));
let value = |l: &Value, name: &str| {
l["joints"].as_array().into_iter().flatten().find(|j| j["name"] == name).and_then(|j| j["values"][0]["value"].as_f64()).unwrap_or(f64::NAN)
};
let l = run(&mut s, "joint.list", json!({}));
assert!((value(&l, "Gear") - 30.0).abs() < 1e-9 && (value(&l, "Cart") - 8.0).abs() < 1e-9, "{l}");
let links: Vec<(f64, &str)> =
l["links"].as_array().into_iter().flatten().map(|k| (k["offset"].as_f64().unwrap_or(f64::NAN), k["unit"].as_str().unwrap_or(""))).collect();
assert!(
links.len() == 2 && (links[0].0 - 10.0).abs() < 1e-9 && links[0].1 == "deg" && (links[1].0 - 5.0).abs() < 1e-9 && links[1].1 == "mm",
"{l}"
);
assert!((s.doc.assembly.links[0].offset - 10f64.to_radians()).abs() < 1e-12);
// A design saved before offsets took degrees stored 0.1 rad, and opens turning Gear 5.73° past Dial.
s.doc_mut().assembly.links[0].offset = 0.1;
let (dir, path) = temp_design("link-offset", "solidcraft");
run(&mut s, "file.save", json!({"path": path}));
run(&mut s, "file.new", json!({}));
run(&mut s, "doc.open", json!({"path": path}));
let l = run(&mut s, "joint.list", json!({}));
assert!((value(&l, "Gear") - value(&l, "Dial") - 0.1f64.to_degrees()).abs() < 1e-9, "{l}");
let _ = std::fs::remove_dir_all(&dir);
}
/// A new joint starts at its values inside its limits. Limits that leave out 0 (or the values
/// given) start it at the nearer limit, which it keeps, and the answer says the part moved there.
#[test]
fn a_new_joint_starts_inside_its_limits_and_says_so() {
let mut s = two_cubes();
let r = run(&mut s, "joint.as_built", json!({"type": "slider", "a": "A", "b": "B", "name": "Rail", "limits": [[2, 5]]}));
let w = r["warnings"].as_array().cloned().unwrap_or_default();
assert!(
w.len() == 1 && w[0].as_str().is_some_and(|w| w.starts_with("Rail distance starts at 2 mm, inside its limits [2, 5], not at 0 mm")),
"{r}"
);
assert!(near(place(&s, "B", Vec3::new(20.0, 0.0, 0.0)), Vec3::new(20.0, 0.0, 2.0)), "{:?}", place(&s, "B", Vec3::new(20.0, 0.0, 0.0)));
assert_eq!(s.doc.assembly.joints[0].values, [2.0]);
// Values given outside the limits start at the nearer one too; angles are answered in degrees.
let mut s = two_cubes();
let r = run(
&mut s,
"joint.create",
json!({"type": "revolute", "a": {"face": [5, 5, 10]}, "b": {"face": [25, 5, 0]}, "values": [90], "limits": [[0, 60]], "name": "Hinge"}),
);
let w = r["warnings"].as_array().cloned().unwrap_or_default();
assert!(w.len() == 1 && w[0].as_str().is_some_and(|w| w.contains("starts at 60 deg") && w.contains("not at 90 deg")), "{r}");
// Inside the limits, nothing to say.
let mut s = two_cubes();
let r = run(&mut s, "joint.as_built", json!({"type": "cylindrical", "a": "A", "b": "B", "name": "Spindle", "limits": [[-30, 30], [0, 5]]}));
assert!(r.get("warnings").is_none(), "{r}");
}
+3 -2
View File
@@ -878,8 +878,9 @@ pub fn commands(app: &SolveApp, k: &Asm, inputs: &[SelInput]) -> Result<Vec<(Str
Asm::MotionLink { a, b, ia, ib, ratio, offset } => {
let js = &s.doc.assembly.joints;
let (Some(ja), Some(jb)) = (js.get(*a), js.get(*b)) else { return Err("needs two joints".into()) };
let num = |e: &str| eval(e, ValueKind::Unitless);
cmd("joint.motion_link", json!({"a": ja.id, "b": jb.id, "ia": ia, "ib": ib, "ratio": num(ratio)?, "offset": num(offset)?}))
// The offset as typed: the engine reads it in Joint 2's units (deg or mm).
let ratio = eval(ratio, ValueKind::Unitless)?;
cmd("joint.motion_link", json!({"a": ja.id, "b": jb.id, "ia": ia, "ib": ib, "ratio": ratio, "offset": offset.trim()}))
}
// The check runs from its Compute button; OK only closes.
Asm::Interference { .. } => Vec::new(),