From 4d862cd76ccfa3a171c66c48d9a184209a33a945 Mon Sep 17 00:00:00 2001 From: Mateusz Sych <151755914+SychPL@users.noreply.github.com> Date: Fri, 16 Oct 2026 05:58:34 +0200 Subject: [PATCH] Joints: joint.reach (inverse kinematics), and a jointed part follows a drag (#1540) Thanks @SychPL. --- ROADMAP.md | 2 +- assets/locales/de.json | 1 + assets/locales/en.json | 1 + assets/locales/fr.json | 1 + assets/locales/ja.json | 1 + assets/locales/ko.json | 1 + assets/locales/ru.json | 1 + assets/locales/uk.json | 1 + assets/locales/zh.json | 1 + crates/doc/src/joints.rs | 100 +++-- crates/doc/src/joints_reach.rs | 445 ++++++++++++++++++++ crates/engine/src/cmd/joints.rs | 111 +++++ crates/engine/src/cmd/joints_reach_tests.rs | 315 ++++++++++++++ crates/engine/src/cmd/joints_tests.rs | 14 +- crates/ui-egui/src/dialogs_assembly.rs | 2 +- crates/ui-egui/src/joint_drag.rs | 214 ++++++++++ crates/ui-egui/src/lib.rs | 5 +- crates/ui-egui/src/viewport.rs | 22 + docs/book/src/guide/assemblies.md | 15 + skills/solidcraft-mechanisms/SKILL.md | 1 + skills/solidcraft-mechanisms/reference.md | 3 + 21 files changed, 1209 insertions(+), 48 deletions(-) create mode 100644 crates/doc/src/joints_reach.rs create mode 100644 crates/engine/src/cmd/joints_reach_tests.rs create mode 100644 crates/ui-egui/src/joint_drag.rs diff --git a/ROADMAP.md b/ROADMAP.md index 7cd14739..681339c8 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -56,7 +56,7 @@ PCB / Package, Render, Animation, Simulation, Manufacture/CAM, Drawings. | M8 | Surface workspace (#772). Done: Patch (sketch regions; edge loops of any shape and number of sides as one face, G0/G1/G2 to the faces beside each edge, through points, #759), Stitch, Thicken (planes and cylinders), Extend (straight edges of planar surfaces), Trim (by a plane or a face), Delete Face (healing), Boundary Fill, zebra/draft/curvature-map analysis. Proposed, in order: surface Extrude/Revolve/Sweep/Loft from open and closed curves (#757), Offset surface (#758), Unstitch and Reverse Normal (#766), Delete Face without healing (#768); Extend curved faces and edges (#760), mutual trim and trim by projected curves (#767), Untrim (#761), Thicken every surface kind (#765); Ruled (#762), Boundary surface and curve networks with G0/G1/G2 (#763), Blend surface (#764), sweep guide rails and two-rail sweeps (#771; orientation, twist and end scale done; loft rails #201), fillet between surface bodies (#769); continuity analysis, surface curvature combs and isocurves (#770) | deferred | — | | M9 | Components and assemblies: New Component / Create Components from Bodies, active component, per-component origin/bodies/sketches/timeline entries, occurrences with transforms, Copy/Paste vs Paste New, nested Browser tree with visibility and isolate; Ground, Move/Copy of occurrences, joints (rigid, revolute, slider, cylindrical, pin-slot, planar, ball) by joint origins with limits and Drive Joints, closed kinematic loops (four-bar, slider-crank: the loop's other joints follow the driven one on its branch, and driving stops where a loop can no longer close), As-built Joint, Rigid Group, Contact Sets (driving and motion studies stop where bodies in a set would pass through each other), Motion Study (joint values keyed along a timeline, playback, export of occurrence positions as JSON/CSV), Exploded views (automatic or by hand, shown without changing placements); interference, per-component physical properties; STEP with assembly structure; URDF export for robot simulators (links with meshes, mass and inertia; joint chains whose values match Drive Joints; docs/urdf.md). Physical properties now measure material-aware component/occurrence placements, descendants and repeated instances, including pending moves and body reassignment offsets; contact geometry and automatic exploded centres respect those offsets. Groundwork done: component tree, occurrences with transforms (instances, Paste New), world placement, active component as authoring frame, ground, Capture/Revert Position, moving bodies and sketches. UI: ASSEMBLE panel dialogs for New Component, Joint (snaps on faces, edges, circles and vertices; animated motion preview; offsets, flip, limits), As-built Joint, Joint Origin, Rigid Group, Drive Joints (live slider), Motion Link and Interference (overlaps shown red); joints in the timeline and the Browser | in progress | 32 | | M8 | Surface workspace (#772). Done: Patch (sketch regions; edge loops filled G0), Stitch, Thicken (planes and cylinders), Extend (straight edges of planar surfaces), Trim (by a plane or a face), Delete Face (healing), Boundary Fill, zebra/draft/curvature-map analysis. Proposed, in order: surface Extrude/Revolve/Sweep/Loft from open and closed curves (#757), Offset surface (#758), Unstitch and Reverse Normal (#766), Delete Face without healing (#768); Extend curved faces and edges (#760), mutual trim and trim by projected curves (#767), Untrim (#761), Thicken every surface kind (#765); Ruled (#762), Boundary surface and curve networks with G0/G1/G2 (#763), Patch with G1/G2 and general loops (#759), Blend surface (#764), sweep guide rails and two-rail sweeps (#771; orientation, twist and end scale done; loft rails #201), fillet between surface bodies (#769); continuity analysis, surface curvature combs and isocurves (#770) | deferred | — | -| M9 | Components and assemblies: New Component / Create Components from Bodies, active component, per-component origin/bodies/sketches/timeline entries, occurrences with transforms, Copy/Paste vs Paste New, nested Browser tree with visibility and isolate; Ground, Move/Copy of occurrences, joints (rigid, revolute, slider, cylindrical, pin-slot, planar, ball) by joint origins with limits and Drive Joints, closed kinematic loops (four-bar, slider-crank: the loop's other joints follow the driven one on its branch, and driving stops where a loop can no longer close), gears, rack and pinion and screws (motion links whose ratio follows picked radii, parameters or a pitch, also inside closed loops), As-built Joint, Rigid Group, Contact Sets (driving and motion studies stop where bodies in a set would pass through each other), Motion Study (joint values keyed along a timeline, playback, export of occurrence positions as JSON/CSV), Exploded views (automatic or by hand, shown without changing placements); interference, per-component physical properties; STEP with assembly structure; URDF export for robot simulators (links with meshes, mass and inertia; joint chains whose values match Drive Joints; docs/urdf.md). Physical properties now measure material-aware component/occurrence placements, descendants and repeated instances, including pending moves and body reassignment offsets; contact geometry and automatic exploded centres respect those offsets. Groundwork done: component tree, occurrences with transforms (instances, Paste New), world placement, active component as authoring frame, ground, Capture/Revert Position, moving bodies and sketches. UI: ASSEMBLE panel dialogs for New Component, Joint (snaps on faces, edges, circles and vertices; animated motion preview; offsets, flip, limits), As-built Joint, Joint Origin, Rigid Group, Drive Joints (live slider), Motion Link, Gear, Rack and Pinion, Screw and Interference (overlaps shown red); joints in the timeline and the Browser | in progress | 32 | +| M9 | Components and assemblies: New Component / Create Components from Bodies, active component, per-component origin/bodies/sketches/timeline entries, occurrences with transforms, Copy/Paste vs Paste New, nested Browser tree with visibility and isolate; Ground, Move/Copy of occurrences, joints (rigid, revolute, slider, cylindrical, pin-slot, planar, ball) by joint origins with limits and Drive Joints, closed kinematic loops (four-bar, slider-crank: the loop's other joints follow the driven one on its branch, and driving stops where a loop can no longer close), gears, rack and pinion and screws (motion links whose ratio follows picked radii, parameters or a pitch, also inside closed loops), inverse kinematics (`joint.reach`: a part's point to a target through the joints on its chain, within limits and with loops closed, nearest pose and its stopper when out of reach; dragging a jointed part in the viewport moves it as far as its joints allow), As-built Joint, Rigid Group, Contact Sets (driving and motion studies stop where bodies in a set would pass through each other), Motion Study (joint values keyed along a timeline, playback, export of occurrence positions as JSON/CSV), Exploded views (automatic or by hand, shown without changing placements); interference, per-component physical properties; STEP with assembly structure; URDF export for robot simulators (links with meshes, mass and inertia; joint chains whose values match Drive Joints; docs/urdf.md). Physical properties now measure material-aware component/occurrence placements, descendants and repeated instances, including pending moves and body reassignment offsets; contact geometry and automatic exploded centres respect those offsets. Groundwork done: component tree, occurrences with transforms (instances, Paste New), world placement, active component as authoring frame, ground, Capture/Revert Position, moving bodies and sketches. UI: ASSEMBLE panel dialogs for New Component, Joint (snaps on faces, edges, circles and vertices; animated motion preview; offsets, flip, limits), As-built Joint, Joint Origin, Rigid Group, Drive Joints (live slider), Motion Link, Gear, Rack and Pinion, Screw and Interference (overlaps shown red); joints in the timeline and the Browser | in progress | 32 | | M10 | Sheet metal: flange (base, edge, contour), hem, unfold/refold, convert, flat pattern + DXF, rules: done. PLASTIC enclosure subset: Boss, Rib, Web, Lip/Groove, Snap Fit, Rest, Plastic Rules (library, edit, assign; rule draft and clearance as feature defaults): done, tested against hand-computed geometry. UI: SHEET METAL and PLASTIC tabs in Fusion's layout with dialogs (live preview, Edit Feature, pre-selection) for Flange, Hem, Unfold/Refold, Convert, Sheet Metal Rules, Create Flat Pattern (flat view with bend lines, bend table CSV/JSON and DXF export with exact circle/arc/line cutouts), Boss, Lip/Groove, Snap Fit, Rest, Manage and Assign Plastic Rules; Fold (bend along a sketch line); flanges meeting at a corner closed at any angle with the rule's gap; edge flanges with their own angle and height per edge in one feature; corner reliefs (notch, round, rectangular, tear) chosen by the rule; Convert to Sheet Metal from a solid or shelled body by its bend edges (flat faces, sharp bends) | in progress | 36 | | M11 | 2D drawings | deferred | — | | M12 | Release: cross-platform packages, verification and deliberate publication implemented; original current-UI demo refreshed. Trusted distribution signing and broader performance work remain | in progress | 16 | diff --git a/assets/locales/de.json b/assets/locales/de.json index 95fa4098..d291fa09 100644 --- a/assets/locales/de.json +++ b/assets/locales/de.json @@ -204,6 +204,7 @@ "cmd.joint_motion_link": "Bewegungsverknüpfung", "cmd.joint_origin": "Gelenkursprung", "cmd.joint_rack_pinion": "Zahnstange und Ritzel", + "cmd.joint_reach": "Punkt anfahren", "cmd.joint_rigid_group": "Starrgruppe", "cmd.joint_screw": "Schraube", "cmd.joint_solve": "Gelenke lösen", diff --git a/assets/locales/en.json b/assets/locales/en.json index b9a583d1..fab0d438 100644 --- a/assets/locales/en.json +++ b/assets/locales/en.json @@ -406,6 +406,7 @@ "cmd.joint_motion_link": "Motion Link", "cmd.joint_origin": "Joint Origin", "cmd.joint_rack_pinion": "Rack and Pinion", + "cmd.joint_reach": "Reach Point", "cmd.joint_rigid_group": "Rigid Group", "cmd.joint_screw": "Screw", "cmd.joint_solve": "Solve Joints", diff --git a/assets/locales/fr.json b/assets/locales/fr.json index bff40076..acb46e48 100644 --- a/assets/locales/fr.json +++ b/assets/locales/fr.json @@ -386,6 +386,7 @@ "cmd.joint_motion_link": "Lien de mouvement", "cmd.joint_origin": "Origine de liaison", "cmd.joint_rack_pinion": "Pignon et crémaillère", + "cmd.joint_reach": "Atteindre un point", "cmd.joint_rigid_group": "Groupe rigide", "cmd.joint_screw": "Vis", "cmd.joint_solve": "Résoudre les liaisons", diff --git a/assets/locales/ja.json b/assets/locales/ja.json index a50b3417..676c707b 100644 --- a/assets/locales/ja.json +++ b/assets/locales/ja.json @@ -406,6 +406,7 @@ "cmd.joint_motion_link": "モーション リンク", "cmd.joint_origin": "ジョイント原点", "cmd.joint_rack_pinion": "ラック アンド ピニオン", + "cmd.joint_reach": "点に到達", "cmd.joint_rigid_group": "リジッド グループ", "cmd.joint_screw": "ねじ", "cmd.joint_solve": "ジョイントを解決", diff --git a/assets/locales/ko.json b/assets/locales/ko.json index b5cec8c1..877fbe9c 100644 --- a/assets/locales/ko.json +++ b/assets/locales/ko.json @@ -204,6 +204,7 @@ "cmd.joint_motion_link": "모션 링크", "cmd.joint_origin": "관절 원점", "cmd.joint_rack_pinion": "랙 앤 피니언", + "cmd.joint_reach": "점에 도달", "cmd.joint_rigid_group": "강체 그룹", "cmd.joint_screw": "나사", "cmd.joint_solve": "관절 해석", diff --git a/assets/locales/ru.json b/assets/locales/ru.json index 52fcd977..630f085f 100644 --- a/assets/locales/ru.json +++ b/assets/locales/ru.json @@ -362,6 +362,7 @@ "cmd.joint_motion_link": "Связь движений", "cmd.joint_origin": "Точка сопряжения", "cmd.joint_rack_pinion": "Реечная передача", + "cmd.joint_reach": "Достичь точки", "cmd.joint_rigid_group": "Жёсткая группа", "cmd.joint_screw": "Винт", "cmd.joint_solve": "Решить сопряжения", diff --git a/assets/locales/uk.json b/assets/locales/uk.json index f5c5d015..b8c77211 100644 --- a/assets/locales/uk.json +++ b/assets/locales/uk.json @@ -363,6 +363,7 @@ "cmd.joint_motion_link": "Зв'язок руху", "cmd.joint_origin": "Початок з'єднання", "cmd.joint_rack_pinion": "Рейкова передача", + "cmd.joint_reach": "Досягти точки", "cmd.joint_rigid_group": "Жорстка група", "cmd.joint_screw": "Гвинт", "cmd.joint_solve": "Розв'язати з'єднання", diff --git a/assets/locales/zh.json b/assets/locales/zh.json index 2e6162e5..8a4115b7 100644 --- a/assets/locales/zh.json +++ b/assets/locales/zh.json @@ -406,6 +406,7 @@ "cmd.joint_motion_link": "运动关联", "cmd.joint_origin": "接头原点", "cmd.joint_rack_pinion": "齿轮齿条", + "cmd.joint_reach": "到达点", "cmd.joint_rigid_group": "刚性组", "cmd.joint_screw": "螺旋", "cmd.joint_solve": "解算接头", diff --git a/crates/doc/src/joints.rs b/crates/doc/src/joints.rs index afeb93a2..f526d4c7 100644 --- a/crates/doc/src/joints.rs +++ b/crates/doc/src/joints.rs @@ -1211,8 +1211,61 @@ pub fn solve(doc: &Document, st: &ModelState) -> Solution { pub fn solve_holding(doc: &Document, st: &ModelState, held: &Held) -> Solution { let asm = &doc.assembly; let mut sol = Solution::default(); - let joints: Vec<&Joint> = asm.joints.iter().filter(|j| !j.suppressed).take(MAX_JOINTS).collect(); let mut vals = joint_values(asm); + let Built { tree, mut placed, closing, mut dof } = build(doc, st, &vals, &mut sol); + let mut stuck = BTreeSet::new(); + if !closing.is_empty() { + (sol.values, stuck) = close_loops(&tree, &closing, asm, held, &mut vals); + if !sol.values.is_empty() { + placed = tree.place(&vals); + } + } + for id in &closing { + let Some(j) = tree.joints.get(id) else { continue }; + let (Some(ta), Some(tb)) = (placed.get(&j.a.occurrence), placed.get(&j.b.occurrence)) else { continue }; + 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 { + sol.conflicts.push(( + j.id, + j.name.clone(), + format!("off by {dt:.4} mm and {:.3}° from the placement the other joints give", da.to_degrees()), + )); + } else if stuck.contains(id) { + // It meets there, but the loop would have to come apart on the way (a rocker + // driven a full turn ends where it started). + sol.conflicts.push((j.id, j.name.clone(), "the loop can't stay closed on the way to these values".into())); + } + } + if !closing.is_empty() { + loop_dof(&tree, &closing, asm, &vals, &mut dof); + } + for o in &doc.occurrences { + dof.entry(o.id).or_insert(6); + } + placed.remove(&0); + sol.world = placed; + sol.dof = dof; + sol +} + +/// The joint graph laid out for a solve: the tree, where it places each occurrence for the +/// values it was built with, the joints that close loops, and each occurrence's degrees of +/// freedom along the tree. +struct Built<'a> { + tree: Tree<'a>, + placed: BTreeMap, + closing: Vec, + dof: BTreeMap, +} + +/// Lay out the joint graph: the spanning tree out from the fixed occurrences (the root, grounded +/// ones; with neither involved, the first joint's A side) and the joints left over, which close +/// loops. Joints whose origins can't be resolved go to `sol.errors`; `sol.fixed` is set. +fn build<'a>(doc: &'a Document, st: &ModelState, vals: &Values, sol: &mut Solution) -> Built<'a> { + let asm = &doc.assembly; + let joints: Vec<&Joint> = asm.joints.iter().filter(|j| !j.suppressed).take(MAX_JOINTS).collect(); // Joint frames in component coordinates (errors leave the joint out). let mut frames: BTreeMap = BTreeMap::new(); for j in &joints { @@ -1270,7 +1323,7 @@ pub fn solve_holding(doc: &Document, st: &ModelState, held: &Held) -> Solution { if placed.contains_key(&other) { continue; } - let Some(m) = tree.step(j.id, from_a, &t, &vals) else { continue }; + let Some(m) = tree.step(j.id, from_a, &t, vals) else { continue }; used.insert(j.id); placed.insert(other, m); tree.parent.insert(other, (occ, j.id, from_a)); @@ -1279,48 +1332,15 @@ pub fn solve_holding(doc: &Document, st: &ModelState, held: &Held) -> Solution { queue.push_back(other); } } - // Joints that close loops: move the loops' values to meet them, then check they agree. + // Joints that close loops: the solve moves the loops' values to meet them, then checks they + // agree. let closing: Vec = joints .iter() .filter(|j| !used.contains(&j.id) && tree.frames.contains_key(&j.id)) .filter(|j| placed.contains_key(&j.a.occurrence) && placed.contains_key(&j.b.occurrence)) .map(|j| j.id) .collect(); - let mut stuck = BTreeSet::new(); - if !closing.is_empty() { - (sol.values, stuck) = close_loops(&tree, &closing, asm, held, &mut vals); - if !sol.values.is_empty() { - placed = tree.place(&vals); - } - } - for id in &closing { - let Some(j) = tree.joints.get(id) else { continue }; - let (Some(ta), Some(tb)) = (placed.get(&j.a.occurrence), placed.get(&j.b.occurrence)) else { continue }; - 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 { - sol.conflicts.push(( - j.id, - j.name.clone(), - format!("off by {dt:.4} mm and {:.3}° from the placement the other joints give", da.to_degrees()), - )); - } else if stuck.contains(id) { - // It meets there, but the loop would have to come apart on the way (a rocker - // driven a full turn ends where it started). - sol.conflicts.push((j.id, j.name.clone(), "the loop can't stay closed on the way to these values".into())); - } - } - if !closing.is_empty() { - loop_dof(&tree, &closing, asm, &vals, &mut dof); - } - for o in &doc.occurrences { - dof.entry(o.id).or_insert(6); - } - placed.remove(&0); - sol.world = placed; - sol.dof = dof; - sol + Built { tree, placed, closing, dof } } impl Document { @@ -1368,6 +1388,10 @@ impl Document { } } +#[path = "joints_reach.rs"] +mod reach; +pub use reach::{REACH_TOL, Reach, Reached, reach}; + /// The frame matrix for an explicit origin / z / x (public for commands). pub fn frame_of(o: Vec3, z: Vec3, x: Option) -> Option { frame(o, z, x) diff --git a/crates/doc/src/joints_reach.rs b/crates/doc/src/joints_reach.rs new file mode 100644 index 00000000..07bd137f --- /dev/null +++ b/crates/doc/src/joints_reach.rs @@ -0,0 +1,445 @@ +//! Inverse kinematics (`joint.reach`): move the values of the joints on an occurrence's chain so a +//! point of it reaches a target (and an axis of it points a given way), every value within its +//! limits, motion links followed, and the closed loops those values are in kept closed. + +use std::cell::Cell; +use std::collections::BTreeSet; + +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, +}; +use crate::{Document, ModelState, apply_point, apply_vector, mat_mul}; + +/// How close counts as reached: mm for the point, degrees for the axis. +pub const REACH_TOL: f64 = 1e-4; + +/// What a reach asks. +#[derive(Clone, Debug)] +pub struct Reach { + /// The occurrence that reaches. + pub occurrence: u64, + /// Its point that goes to `to`, in its own (component) coordinates. + pub point: Vec3, + /// Where the point goes (world). + pub to: Vec3, + /// A unit axis of it (its own coordinates) and the unit world direction it turns to. + pub axis: Option<(Vec3, Vec3)>, + /// The joints whose values move toward the target (`None`: every joint on its chain). The + /// other joints of a closed loop they are in follow. + pub joints: Option>, +} + +/// How a reach ended. +#[derive(Clone, Debug, Default)] +pub struct Reached { + /// The new stored values of the joints that moved (joint id → values). + pub values: Values, + /// Short of the target: the values it ended on a limit of, with the target pulling past it + /// (joint, value index, the upper limit). + pub limits: Vec<(u64, usize, bool)>, + /// Short of the target: the closing joint of a loop that stopped it (the chain alone, the + /// loop left open, gets closer). + pub stopped_by_loop: Option, +} + +/// How much a loop's gap (mm) outweighs a miss of the target (mm) in a step: enough to keep the +/// step along the mechanism, so putting it back on the loops moves it little. +const LOOP_WEIGHT: f64 = 1e3; +/// Steps a reach takes at most, each moving no value more than 10° or a tenth of the chain's +/// size (so it follows the mechanism's branch). +const MAX_REACH_STEPS: usize = 200; +const STEP_ANGLE: f64 = 10.0; +const STEP_SHARE: f64 = 0.1; + +/// Move the values of the joints on `r.occurrence`'s chain (those in `r.joints`, when given) so +/// its point gets to `r.to`, or as close as the limits and closed loops let it: the nearest pose, +/// found from where it is (it stays on the mechanism's branch). Values a motion link sets move +/// through the start of their link chain; the other values of the loops they are in follow. An +/// error says why nothing can move. +pub fn reach(doc: &Document, st: &ModelState, r: &Reach) -> Result { + let asm = &doc.assembly; + let vals = joint_values(asm); + let built = build(doc, st, &vals, &mut Solution::default()); + let tree = &built.tree; + if tree.fixed.contains_key(&r.occurrence) { + return Err("it is fixed in place (grounded, or the part its joints are placed from)".into()); + } + if !tree.parent.contains_key(&r.occurrence) { + return Err("no joint places it: it is not connected to anything grounded (ground a part, or join it to a grounded part)".into()); + } + let linked: BTreeSet<(u64, usize)> = asm.links.iter().map(|l| (l.b, l.ib)).collect(); + let allowed = |id: &u64| r.joints.as_ref().is_none_or(|js| js.contains(id)); + let path: BTreeSet = tree.path(r.occurrence).1.iter().map(|(k, _)| *k).collect(); + // The values that move it: those of the joints on its chain (the ones given), a linked one + // through the start of its link chain (given or not), and the starts given of the chain's + // other linked values. + let chosen: BTreeSet = path.iter().copied().filter(allowed).collect(); + let mut drive: Vec<(u64, usize)> = Vec::new(); + for j in chosen.iter().filter_map(|id| tree.joints.get(id)) { + drive.extend((0..j.kind.dofs().len()).map(|i| (j.id, i)).filter(|v| !linked.contains(v))); + } + let starts = link_chains(asm, tree, &path, &linked).1.into_iter().filter(|v| allowed(&v.0)); + for v in link_chains(asm, tree, &chosen, &linked).1.into_iter().chain(starts) { + if !drive.contains(&v) { + drive.push(v); + } + } + if drive.is_empty() { + return Err(if r.joints.is_some() { + "none of the joints given moves it" + } else { + "no joint on its chain moves (rigid, or set by motion links)" + } + .into()); + } + // The loops those values are in, and the values of theirs that follow. + if built.closing.len() > MAX_LOOPS { + return Err(format!("the design has more than {MAX_LOOPS} closed loops")); + } + let groups = loop_groups(tree, &built.closing, asm, &linked); + let (mut vars, mut closers, mut around) = (drive.clone(), Vec::new(), path.clone()); + let mut taken = BTreeSet::new(); + while vars.len() <= MAX_LOOP_VALUES { + let more: Vec = + groups.iter().enumerate().filter(|(g, (_, _, t))| !taken.contains(g) && t.iter().any(|v| vars.contains(v))).map(|(g, _)| g).collect(); + if more.is_empty() { + break; + } + for (c, a, _) in more.iter().filter_map(|g| groups.get(*g)) { + closers.extend(c.iter().copied()); + around.extend(a.iter().copied()); + for v in unknowns(tree, a, asm, &linked, &Held::new()) { + if !vars.contains(&v) { + vars.push(v); + } + } + } + taken.extend(more); + } + if vars.len() > MAX_LOOP_VALUES { + return Err(format!("its chain and loops have more than {MAX_LOOP_VALUES} values")); + } + let scale = loop_scale(tree, &around, &vals); + let bounds = limit_bounds(tree, &vars); + let x0: Vec = vars.iter().map(|(id, i)| vals.get(id).and_then(|q| q.get(*i)).copied().unwrap_or(0.0)).collect(); + // A distance steps by a share of the chain's size, of the way to the target or of the point's + // reach from its joints, whichever is longest (one slider on its own has no size, and a part + // turning about a joint far from its point moves that far). + let point = tree.world(r.occurrence, &vals).map(|t| apply_point(&t, r.point)); + let lever = path + .iter() + .filter_map(|id| { + let (j, (fa, _)) = (tree.joints.get(id)?, tree.frames.get(id)?); + Some(apply_point(&mat_mul(&tree.world(j.a.occurrence, &vals)?, fa), Vec3::ZERO).dist(point?)) + }) + .fold(0.0, f64::max); + let span = point.map_or(scale, |p| scale.max(p.dist(r.to)).max(lever)); + let cap: Vec = vars + .iter() + .map(|(id, i)| if tree.joints.get(id).is_some_and(|j| j.kind.is_angle(*i)) { STEP_ANGLE.to_radians() } else { STEP_SHARE * span }) + .collect(); + // Every value around the chain and its loops, links included, moves at most a step at a time: + // a value a link sets can turn far more than the one that drives it, and a loop must stay + // closed on the way, as joint.drive steps its loops. + let mut paced: Vec<((u64, usize), f64)> = Vec::new(); + for j in around.iter().filter_map(|id| tree.joints.get(id)) { + for i in 0..j.kind.dofs().len() { + paced.push(((j.id, i), if j.kind.is_angle(i) { STEP_ANGLE.to_radians() } else { STEP_SHARE * span })); + } + } + let tol = (1e-10 * scale).min(1e-7); + let mt = if r.axis.is_some() { 6 } else { 3 }; + let spent = Cell::new(0usize); + let mut work = vals.clone(); + let mut f = |x: &[f64]| misses(tree, asm, r, scale, &vars, &closers, x, &mut work, &spent); + let mut x = x0.clone(); + let miss = |res: &[f64]| res.iter().take(mt).map(|v| v * v).sum::().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) + .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::().sqrt(); + let turn_miss = (res.iter().skip(3).take(mt - 3).map(|v| v * v).sum::().sqrt() / scale).to_degrees(); + if point_miss > REACH_TOL || turn_miss > REACH_TOL { + // The limits it rests on: the target pulls the value past them (−Jᵀr over the target's + // rows, ignoring pulls at right angles to the miss: under a millionth of the value's own + // reach times the miss). + let n = x.len(); + if let Some(jac) = jacobian(&mut f, &x, res.len()) { + for (k, (((id, i), v), (lo, hi))) in vars.iter().zip(&x).zip(&bounds).enumerate() { + let column = |row: usize| jac.get(row * n + k).copied().unwrap_or(0.0); + let pull: f64 = -(0..mt).map(|row| column(row) * res.get(row).copied().unwrap_or(0.0)).sum::(); + let eps = 1e-6 * (0..mt).map(|row| column(row).powi(2)).sum::().sqrt() * miss(&res); + let at = |b: f64| b.is_finite() && (v - b).abs() <= 1e-9 * b.abs().max(1.0); + if at(*hi) && pull > eps { + out.limits.push((*id, *i, true)); + } else if at(*lo) && pull < -eps { + out.limits.push((*id, *i, false)); + } + } + } + // A value a motion link sets, held at its own limit (the link clamps it there): the + // target's pull on it as if it were free. + if f(&x).is_some() { + for (id, i) in linked.iter().filter(|(id, _)| around.contains(id)) { + let (Some(v), Some((lo, hi))) = (work.get(id).and_then(|q| q.get(*i)).copied(), limit_bounds(tree, &[(*id, *i)]).first().copied()) + else { + continue; + }; + let at = |b: f64| b.is_finite() && (v - b).abs() <= 1e-9 * b.abs().max(1.0); + if !at(lo) && !at(hi) { + continue; + } + let nudged = |dv: f64| { + let mut w = work.clone(); + if let Some(slot) = w.get_mut(id).and_then(|q| q.get_mut(*i)) { + *slot = v + dv; + } + target(tree, r, scale, &w) + }; + let (Some(up), Some(down)) = (nudged(1e-6), nudged(-1e-6)) else { continue }; + let pull: f64 = -up.iter().zip(&down).zip(&res).map(|((a, b), r)| (a - b) / 2e-6 * r).sum::(); + let eps = 1e-6 * up.iter().zip(&down).map(|(a, b)| ((a - b) / 2e-6).powi(2)).sum::().sqrt() * miss(&res); + if at(hi) && pull > eps { + out.limits.push((*id, *i, true)); + } else if at(lo) && pull < -eps { + out.limits.push((*id, *i, false)); + } + } + } + // A loop stopped it when the chain alone gets closer. + // ponytail: names the first loop of several; trying each one open would name the one that binds. + if let Some(first) = closers.first() { + let k = drive.len(); + let mut alone = vals.clone(); + 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) + && miss(&ro) < miss(&res) - REACH_TOL.max(1e-9 * miss(&res)) + { + out.stopped_by_loop = Some(*first); + } + } + } + // The stored values that changed, when that got it closer (by more than the solver's noise: + // a held part doesn't creep). + let gained = before - miss(&res) > 1e-7; + for ((id, i), v) in vars.iter().zip(&x).filter(|_| gained) { + let Some(j) = tree.joints.get(id) else { continue }; + if j.values.get(*i) == Some(v) { + continue; + } + let q = out.values.entry(*id).or_insert_with(|| { + let mut q = j.values.clone(); + q.resize(j.kind.dofs().len(), 0.0); + q + }); + if let Some(slot) = q.get_mut(*i) { + *slot = *v; + } + } + Ok(out) +} + +/// The target's misses with the values `vars` at `x`: the point's (mm), then the axis's (as mm +/// across the chain's `scale`), then the gaps of the loops `closers`, weighted. +fn misses( + tree: &Tree, + asm: &Assembly, + r: &Reach, + scale: f64, + vars: &[(u64, usize)], + closers: &[u64], + x: &[f64], + work: &mut Values, + spent: &Cell, +) -> Option> { + spent.set(spent.get() + 1); + if spent.get() > 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, work); + let mut out = target(tree, r, scale, work)?; + let n = out.len(); + closure_gaps(tree, closers, work, scale, &mut out)?; + for v in out.iter_mut().skip(n) { + *v *= LOOP_WEIGHT; + } + out.iter().all(|v| v.is_finite()).then_some(out) +} + +/// The target's misses for these values: the point's (mm), then the axis's (as mm across `scale`). +fn target(tree: &Tree, r: &Reach, scale: f64, vals: &Values) -> Option> { + let t = tree.world(r.occurrence, vals)?; + let d = apply_point(&t, r.point) - r.to; + let mut out = vec![d.x, d.y, d.z]; + if let Some((a, z)) = r.axis { + let w = (apply_vector(&t, a) - z) * scale; + out.extend([w.x, w.y, w.z]); + } + Some(out) +} + +/// How many steps apart the values `vars` at `a` and at `b` put the `paced` values (each with its +/// step), motion links followed. +fn stride(asm: &Assembly, vals: &Values, vars: &[(u64, usize)], paced: &[((u64, usize), f64)], a: &[f64], b: &[f64]) -> f64 { + let at = |x: &[f64]| { + let mut w = vals.clone(); + for ((id, i), v) in vars.iter().zip(x) { + if let Some(slot) = w.get_mut(id).and_then(|q| q.get_mut(*i)) { + *slot = *v; + } + } + apply_links(asm, &mut w); + w + }; + let (wa, wb) = (at(a), at(b)); + let get = |w: &Values, id: &u64, i: usize| w.get(id).and_then(|q| q.get(i)).copied().unwrap_or(0.0); + paced.iter().map(|((id, i), step)| (get(&wb, id, *i) - get(&wa, id, *i)).abs() / step).fold(0.0, f64::max) +} + +/// 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 +/// 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. +fn approach( + f: &mut dyn FnMut(&[f64]) -> Option>, + stride: &dyn Fn(&[f64], &[f64]) -> f64, + mt: usize, + x: &mut Vec, + bounds: &[(f64, f64)], + cap: &[f64], + tol: f64, +) -> Option> { + let n = x.len(); + let miss = |r: &[f64]| r.iter().take(mt).map(|v| v * v).sum::(); + let close = |f: &mut dyn FnMut(&[f64]) -> Option>, x: &mut Vec| { + least_squares(&mut |y| f(y).map(|r| r.get(mt..).unwrap_or_default().to_vec()), x, bounds, tol * LOOP_WEIGHT) + }; + let loops = f(x)?.len() > mt; + if loops && !close(f, x) { + return None; + } + let mut r = f(x)?; + let (mut lambda, mut kicked) = (1e-3, false); + for _ in 0..MAX_REACH_STEPS { + if largest(r.get(..mt).unwrap_or_default()) <= tol { + break; + } + let m = r.len(); + let Some(jac) = jacobian(f, x, m) else { break }; + // The normal equations JᵀJ δ = −Jᵀr, and the target rows' share of the diagonal (the + // damping scales with it, so the loops' weight doesn't slow the step along them). + let (mut jtj, mut g, mut diag) = (linalg::Mat::zeros(n, n), vec![0.0; n], vec![0.0; n]); + for (row, ri) in r.iter().enumerate() { + let a = jac.get(row * n..(row + 1) * n).unwrap_or_default(); + for (p, ap) in a.iter().enumerate().filter(|(_, v)| **v != 0.0) { + if let Some(gp) = g.get_mut(p) { + *gp -= ap * ri; + } + if row < mt + && let Some(dp) = diag.get_mut(p) + { + *dp += ap * ap; + } + for (q, aq) in a.iter().enumerate().skip(p) { + jtj.add(p, q, ap * aq); + } + } + } + for p in 0..n { + for q in 0..p { + jtj.set(p, q, jtj.get(q, p)); + } + } + let mut stepped = false; + while lambda < 1e12 && !stepped { + let mut held = vec![false; n]; + // Solved, then again with the values held that it pushes past their limits. + for _ in 0..=n { + let mut a = jtj.clone(); + let mut b = g.clone(); + for p in 0..n { + a.add(p, p, lambda * (1.0 + diag.get(p).copied().unwrap_or(0.0))); + if held.get(p).copied().unwrap_or(false) { + for q in 0..n { + a.set(p, q, if p == q { 1.0 } else { 0.0 }); + a.set(q, p, if p == q { 1.0 } else { 0.0 }); + } + if let Some(bp) = b.get_mut(p) { + *bp = 0.0; + } + } + } + let Some(d) = linalg::solve(a, b) else { break }; + let past: Vec = + x.iter().zip(&d).zip(bounds).map(|((v, dv), (lo, hi))| (*dv > 0.0 && *v >= *hi) || (*dv < 0.0 && *v <= *lo)).collect(); + if past.iter().zip(&held).any(|(p, h)| *p && !*h) { + held = held.iter().zip(&past).map(|(h, p)| *h || *p).collect(); + continue; + } + let full: Vec = x.iter().zip(&d).map(|(v, dv)| v + dv).collect(); + let over = d.iter().zip(cap).map(|(d, c)| d.abs() / c).fold(1.0f64, f64::max).max(stride(x, &full)); + let mut next: Vec = x.iter().zip(&d).zip(bounds).map(|((v, dv), (lo, hi))| (v + dv / over).max(*lo).min(*hi)).collect(); + // Checked again once taken: a link held at a limit or the loops closed again may + // move it further than foreseen (joint.drive halves such steps too). + if (!loops || close(f, &mut next)) + && stride(x, &next) <= 2.0 + && let Some(rn) = f(&next) + && miss(&rn) < miss(&r) + { + (*x, r) = (next, rn); + // A step cut down to the cap (as at a stretched, singular pose) damps the next one + // more instead of less, so it doesn't crawl along its longest value. + lambda = if over > 1.0 { (lambda * over).min(1e6) } else { (lambda / 3.0).max(1e-9) }; + stepped = true; + } + break; + } + if !stepped { + lambda *= 4.0; + } + } + if !stepped { + // Where nothing changes the misses to first order but they aren't least (the target + // straight across a pivot from the point), a small kick off it gets closer, once. + if kicked { + break; + } + kicked = true; + // Both ways: one may run into a limit. + for sign in [1.0, -1.0] { + let mut next: Vec = x + .iter() + .zip(cap.iter().zip(bounds)) + .enumerate() + .map(|(k, (v, (c, (lo, hi))))| (v + sign * 1e-3 * c * if k % 2 == 0 { 1.0 } else { -1.0 }).max(*lo).min(*hi)) + .collect(); + if (!loops || close(f, &mut next)) + && stride(x, &next) <= 2.0 + && let Some(rn) = f(&next) + && miss(&rn) < miss(&r) + { + (*x, r, lambda, stepped) = (next, rn, 1e-3, true); + break; + } + } + if !stepped { + break; + } + } + } + Some(r) +} diff --git a/crates/engine/src/cmd/joints.rs b/crates/engine/src/cmd/joints.rs index 92b42b68..4b8fdbfe 100644 --- a/crates/engine/src/cmd/joints.rs +++ b/crates/engine/src/cmd/joints.rs @@ -36,6 +36,9 @@ pub static COMMANDS: &[CommandSpec] = &[ reason: why the joint can move nothing (suppressed, its origins lost, or its chain joined to nothing grounded → unconnected: \ the occurrence named)", ), + CommandSpec::new("joint.reach", "Reach Point", reach).params( + "occurrence: id|name; point: [x,y,z] (where its point goes, world); at?: [x,y,z] (its point, in the world where it is now; default: the centre of its bodies' box); z?: [x,y,z] (the way its axis then points); axis?: [x,y,z] (that axis, in the world now; default: its own z); joints?: [ids or names] (only these move toward the point; the other joints of a closed loop they are in follow); quiet?: bool; continuous?: bool (a frame of a drag: the answer says continuous, and the app's command line doesn't echo it). Moves the values of the joints on its chain from where they are to the nearest pose, within their limits, through motion links, keeping closed loops closed as joint.drive does (contact sets are not checked) → reached; error_mm (and error_deg with z): how close it got; point: where its point is; values: [{joint, name, values}] of the joints that moved, as joint.list gives them; moved; stopped_by_limit: [{joint, value, limit: min|max}]: the limits it rests on short of the point; stopped_by_loop: {joint}: the closed loop that stopped it (its chain alone gets closer); reason: why nothing can move", + ), CommandSpec::new("joint.motion_link", "Motion Link", motion_link) .at("SOLID", "ASSEMBLE") .icon("motion_link") @@ -562,6 +565,111 @@ fn drive(s: &mut Session, p: &Value) -> Result { Ok(out) } +/// Joints `joint.reach` takes at most in `joints`. +const MAX_REACH_JOINTS: usize = 1000; + +fn reach(s: &mut Session, p: &Value) -> Result { + let cmd = "joint.reach"; + let occ = occurrence_id(s, p.get("occurrence"), cmd)?; + let given = |k: &str| p.get(k).and_then(vec3).filter(|v| v.is_finite() && v.len() < 1e9); + let to = given("point").ok_or_else(|| bad(cmd, "`point` must be [x, y, z]"))?; + let world = if occ == 0 { solidcraft_doc::IDENTITY } else { joints::occurrence_world(&s.doc, occ) }; + let inv = solidcraft_doc::mat_inverse(&world).ok_or_else(|| bad(cmd, "occurrence transform"))?; + let at = match p.get("at") { + Some(_) => solidcraft_doc::apply_point(&inv, given("at").ok_or_else(|| bad(cmd, "`at` must be [x, y, z]"))?), + None => body_centre(s, occ), + }; + let unit = |k: &str| -> Result> { + match p.get(k) { + None => Ok(None), + Some(_) => given(k).and_then(Vec3::normalized).map(Some).ok_or_else(|| bad(cmd, format!("`{k}` must be a direction [x, y, z]"))), + } + }; + let (z, axis) = (unit("z")?, unit("axis")?); + if axis.is_some() && z.is_none() { + return Err(bad(cmd, "`axis` goes with `z`, the way it points")); + } + let axis = z.map(|z| (axis.map(|a| solidcraft_doc::apply_vector(&inv, a)).and_then(Vec3::normalized).unwrap_or(Vec3::Z), z)); + let only = match p.get("joints") { + None => None, + Some(Value::Array(a)) if a.len() <= MAX_REACH_JOINTS => Some(a.iter().map(|v| joint_id(s, Some(v), cmd)).collect::>>()?), + Some(_) => return Err(bad(cmd, format!("`joints` must be a list of joints (ids or names), at most {MAX_REACH_JOINTS}"))), + }; + let before = super::motion::positions(s); + let st = s.model.state(); + let r = joints::reach(&s.doc, &st, &joints::Reach { occurrence: occ, point: at, to, axis, joints: only }); + let reached = match &r { + Ok(x) => { + // Only a reach that moves something touches the design (and so adds an undo step). + if !x.values.is_empty() { + let doc = s.doc_mut(); + for (id, q) in &x.values { + if let Some(j) = doc.assembly.joints.iter_mut().find(|j| j.id == *id) { + j.values.clone_from(q); + } + } + } + x.clone() + } + Err(_) => joints::Reached::default(), + }; + // Loops closed again from there, and where it ended up. + let sol = resolve(s); + let t = joints::occurrence_world(&s.doc, occ); + let got = solidcraft_doc::apply_point(&t, at); + let mut out = sol_json(&sol, p); + out["occurrence"] = json!(occurrence_name(s, occ)); + out["point"] = json!([got.x, got.y, got.z]); + out["error_mm"] = json!(got.dist(to)); + let mut there = got.dist(to) <= joints::REACH_TOL; + if let Some((a, z)) = axis { + // From the sine too: the cosine alone loses small angles to rounding. + let v = solidcraft_doc::apply_vector(&t, a); + let deg = v.cross(z).len().atan2(v.dot(z)).to_degrees(); + out["error_deg"] = json!(deg); + there &= deg <= joints::REACH_TOL; + } + out["reached"] = json!(there); + let name = |id: u64| s.doc.assembly.joints.iter().find(|j| j.id == id); + // As joint.list gives them: with what motion links set. + let effective = joints::joint_values(&s.doc.assembly); + out["values"] = reached + .values + .keys() + .filter_map(|id| name(*id)) + .map(|j| json!({"joint": j.id, "name": j.name, "values": named_values(j, effective.get(&j.id).unwrap_or(&j.values))})) + .collect(); + out["moved"] = json!(super::motion::moved(&before, &super::motion::positions(s)).next().is_some()); + out["stopped_by_limit"] = if reached.limits.is_empty() { + Value::Null + } else { + reached + .limits + .iter() + .filter_map(|(id, i, upper)| { + let j = name(*id)?; + Some(json!({"joint": j.name, "value": j.kind.dofs().get(*i), "limit": if *upper { "max" } else { "min" }})) + }) + .collect() + }; + out["stopped_by_loop"] = json!(reached.stopped_by_loop.and_then(name).map(|j| json!({"joint": j.name}))); + if let Err(why) = r { + out["reason"] = json!(why); + } + if bool_(p, "continuous") == Some(true) { + out["continuous"] = json!(true); + } + Ok(out) +} + +/// The centre of the box around an occurrence's bodies, in its own coordinates (its origin when +/// it has none). +fn body_centre(s: &Session, occ: u64) -> Vec3 { + let comp = s.doc.occurrences.iter().find(|o| o.id == occ).map(|o| o.component).unwrap_or(0); + let st = s.model.state(); + st.bodies.iter().filter(|b| s.doc.model_body_component(b) == comp).fold(Aabb3::EMPTY, |bb, b| bb.union(&b.mesh().bounds())).center() +} + fn motion_link(s: &mut Session, p: &Value) -> Result { let cmd = "joint.motion_link"; let (a, b) = (joint_id(s, p.get("a"), cmd)?, joint_id(s, p.get("b"), cmd)?); @@ -959,6 +1067,9 @@ fn overlapping_pairs(boxes: &[Aabb3], limit: usize) -> (Vec<(usize, usize)>, usi (pairs, skipped) } +#[cfg(test)] +#[path = "joints_reach_tests.rs"] +mod reach_tests; #[cfg(test)] #[path = "joints_tests.rs"] mod tests; diff --git a/crates/engine/src/cmd/joints_reach_tests.rs b/crates/engine/src/cmd/joints_reach_tests.rs new file mode 100644 index 00000000..ee4918b1 --- /dev/null +++ b/crates/engine/src/cmd/joints_reach_tests.rs @@ -0,0 +1,315 @@ +//! `joint.reach`: inverse kinematics against closed-form answers (planar arms), the nearest pose +//! out of reach, limits, and a four-bar reached through its rocker. + +use serde_json::{Value, json}; +use solidcraft_geom::Vec3; + +use super::tests::{four_bar, meet, occ, part, pin, place, run}; +use crate::Session; + +/// A planar arm about +Z on a grounded base: Shoulder at the origin, Upper 60 long, Elbow, Fore +/// 40 long, and with `hand` a Wrist and Hand 20 long, all along +X at 0°. Elbow starts at `elbow`°. +fn arm(hand: bool, elbow: f64) -> Session { + let mut s = Session::default(); + part(&mut s, "Base", [-1.0, -1.0, -1.0]); + part(&mut s, "Upper", [30.0, -1.0, -1.0]); + part(&mut s, "Fore", [80.0, -1.0, -1.0]); + let base = occ(&s, "Base"); + run(&mut s, "occurrence.ground", json!({"occurrence": base, "grounded": true})); + pin(&mut s, "revolute", "Shoulder", "Base", "Upper", Vec3::ZERO, Vec3::Z); + pin(&mut s, "revolute", "Elbow", "Upper", "Fore", Vec3::new(60.0, 0.0, 0.0), Vec3::Z); + if hand { + part(&mut s, "Hand", [110.0, -1.0, -1.0]); + pin(&mut s, "revolute", "Wrist", "Fore", "Hand", Vec3::new(100.0, 0.0, 0.0), Vec3::Z); + } + run(&mut s, "joint.drive", json!({"joint": "Elbow", "value": elbow})); + s +} + +/// A joint's first value as `joint.list` gives it (degrees for an angle). +fn value(s: &mut Session, joint: &str) -> f64 { + let l = run(s, "joint.list", json!({"quiet": true})); + l["joints"] + .as_array() + .and_then(|js| js.iter().find(|j| j["name"] == joint)) + .map_or(f64::NAN, |j| j["values"][0]["value"].as_f64().unwrap_or(f64::NAN)) +} + +/// `joint.reach` of the occurrence's point `local` (its own coordinates). +fn reach(s: &mut Session, occurrence: &str, local: Vec3, mut p: Value) -> Value { + p["occurrence"] = json!(occurrence); + p["at"] = pt(place(s, occurrence, local)); + run(s, "joint.reach", p) +} + +fn pt(v: Vec3) -> Value { + json!([v.x, v.y, v.z]) +} + +fn got(r: &Value) -> Vec3 { + Vec3::new(r["point"][0].as_f64().unwrap_or(f64::NAN), r["point"][1].as_f64().unwrap_or(f64::NAN), r["point"][2].as_f64().unwrap_or(f64::NAN)) +} + +/// The two-link arm's closed-form answer for a tip at (x, y), elbow bent the positive way: the +/// shoulder and elbow angles in degrees. +fn two_link(x: f64, y: f64, l1: f64, l2: f64) -> (f64, f64) { + let c2 = (x * x + y * y - l1 * l1 - l2 * l2) / (2.0 * l1 * l2); + let t2 = c2.acos(); + let t1 = y.atan2(x) - (l2 * t2.sin()).atan2(l1 + l2 * t2.cos()); + (t1.to_degrees(), t2.to_degrees()) +} + +#[test] +fn a_two_link_arm_reaches_its_closed_form_pose() { + let mut s = arm(false, 30.0); + let tip = Vec3::new(100.0, 0.0, 0.0); + let to = Vec3::new(50.0, 50.0, 0.0); + let r = reach(&mut s, "Fore", tip, json!({"point": pt(to)})); + assert_eq!( + (&r["reached"], &r["moved"], &r["stopped_by_limit"], &r["stopped_by_loop"]), + (&json!(true), &json!(true), &Value::Null, &Value::Null), + "{r}" + ); + assert!(r["error_mm"].as_f64().is_some_and(|e| e < 1e-6), "{r}"); + assert!(place(&s, "Fore", tip).dist(to) < 1e-6, "{:?}", place(&s, "Fore", tip)); + let (t1, t2) = two_link(50.0, 50.0, 60.0, 40.0); + assert!((value(&mut s, "Shoulder") - t1).abs() < 1e-6 && (value(&mut s, "Elbow") - t2).abs() < 1e-6, "{r}"); + assert_eq!(r["values"].as_array().map(Vec::len), Some(2), "{r}"); + // Already there, a reach moves nothing and adds no undo step. + let steps = s.undo.len(); + let r = reach(&mut s, "Fore", tip, json!({"point": pt(to)})); + assert_eq!((&r["reached"], &r["moved"], s.undo.len()), (&json!(true), &json!(false), steps), "{r}"); + // One undo step puts it back. + run(&mut s, "edit.undo", json!({})); + assert!((value(&mut s, "Elbow") - 30.0).abs() < 1e-9 && value(&mut s, "Shoulder").abs() < 1e-9); +} + +/// A three-link arm puts its tip on the point and its hand along the axis given: the wrist is +/// then the two-link answer, and the hand's turn makes up the rest. +#[test] +fn a_three_link_arm_reaches_a_point_and_an_axis() { + let mut s = arm(true, 30.0); + let tip = Vec3::new(120.0, 0.0, 0.0); + // The hand's length, along its own X: at 30° in the world now (the elbow's turn). + let along = place(&s, "Hand", tip) - place(&s, "Hand", Vec3::new(100.0, 0.0, 0.0)); + let r = reach(&mut s, "Hand", tip, json!({"point": [50, 50, 0], "z": [0, 1, 0], "axis": pt(along)})); + assert_eq!(r["reached"], true, "{r}"); + assert!(r["error_deg"].as_f64().is_some_and(|e| e < 1e-6), "{r}"); + let (t1, t2) = two_link(50.0, 30.0, 60.0, 40.0); + let t3 = 90.0 - t1 - t2; + for (joint, want) in [("Shoulder", t1), ("Elbow", t2), ("Wrist", t3)] { + assert!((value(&mut s, joint) - want).abs() < 1e-6, "{joint}: {} wants {want}: {r}", value(&mut s, joint)); + } +} + +#[test] +fn out_of_reach_the_arm_points_at_the_target() { + let tip = Vec3::new(100.0, 0.0, 0.0); + // 150 mm away: the arm stretches toward it, 50 mm short. + let mut s = arm(false, 30.0); + let r = reach(&mut s, "Fore", tip, json!({"point": [-90, 120, 0]})); + assert_eq!( + (&r["reached"], &r["moved"], &r["stopped_by_limit"], &r["stopped_by_loop"]), + (&json!(false), &json!(true), &Value::Null, &Value::Null), + "{r}" + ); + assert!(r["error_mm"].as_f64().is_some_and(|e| (e - 50.0).abs() < 1e-3), "{r}"); + assert!(got(&r).dist(Vec3::new(-60.0, 80.0, 0.0)) < 1e-2, "{r}"); + assert!(place(&s, "Fore", tip).dist(got(&r)) < 1e-9, "{r}"); + // Out of its plane: the nearest is the point under the target. + let mut s = arm(false, 30.0); + let r = reach(&mut s, "Fore", tip, json!({"point": [50, 50, 30]})); + assert_eq!(r["reached"], false, "{r}"); + assert!(r["error_mm"].as_f64().is_some_and(|e| (e - 30.0).abs() < 1e-6), "{r}"); + assert!(got(&r).dist(Vec3::new(50.0, 50.0, 0.0)) < 1e-6, "{r}"); +} + +#[test] +fn a_limited_elbow_stops_the_reach_and_says_so() { + let tip = Vec3::new(100.0, 0.0, 0.0); + let mut s = arm(false, 10.0); + run(&mut s, "joint.limits", json!({"joint": "Elbow", "min": -30, "max": 30})); + let r = reach(&mut s, "Fore", tip, json!({"point": [50, 50, 0]})); + // The target needs the elbow at 92°: it bends to its limit, and the arm points at the target. + assert_eq!(r["reached"], false, "{r}"); + assert_eq!(r["stopped_by_limit"], json!([{"joint": "Elbow", "value": "angle", "limit": "max"}]), "{r}"); + assert!((value(&mut s, "Elbow") - 30.0).abs() < 1e-9, "{r}"); + let span = (60f64.powi(2) + 40f64.powi(2) + 2.0 * 60.0 * 40.0 * 30f64.to_radians().cos()).sqrt(); + let d = 50.0 * 2f64.sqrt(); + assert!(r["error_mm"].as_f64().is_some_and(|e| (e - (span - d)).abs() < 1e-4), "{r}"); + // Only the shoulder moves when only it is given: the tip swings on its circle. + let mut s = arm(false, 30.0); + let r = reach(&mut s, "Fore", tip, json!({"point": [0, 100, 0], "joints": ["Shoulder"]})); + let span = (60f64.powi(2) + 40f64.powi(2) + 2.0 * 60.0 * 40.0 * 30f64.to_radians().cos()).sqrt(); + assert!(r["error_mm"].as_f64().is_some_and(|e| (e - (100.0 - span)).abs() < 1e-4), "{r}"); + assert!((value(&mut s, "Elbow") - 30.0).abs() < 1e-12, "{r}"); +} + +/// A linked elbow (elbow = −shoulder, so the forearm keeps pointing along +X) follows the +/// shoulder: the tip moves on a circle about (40, 0), and the reach turns the shoulder only. +#[test] +fn a_linked_joint_follows_the_reach() { + let tip = Vec3::new(100.0, 0.0, 0.0); + let mut s = arm(false, 0.0); + run(&mut s, "joint.motion_link", json!({"a": "Shoulder", "b": "Elbow", "ratio": -1})); + let th = 40f64.to_radians(); + let to = json!([40.0 + 60.0 * th.cos(), 60.0 * th.sin(), 0.0]); + let r = reach(&mut s, "Fore", tip, json!({"point": to})); + assert_eq!(r["reached"], true, "{r}"); + assert!((value(&mut s, "Shoulder") - 40.0).abs() < 1e-6 && (value(&mut s, "Elbow") + 40.0).abs() < 1e-6, "{r}"); + // Given only the linked elbow, the reach turns it through its link's source. + run(&mut s, "edit.undo", json!({})); + let r = reach(&mut s, "Fore", tip, json!({"point": to, "joints": ["Elbow"]})); + assert_eq!(r["reached"], true, "{r}"); + assert!((value(&mut s, "Shoulder") - 40.0).abs() < 1e-6, "{r}"); + // The elbow held at its own limit by the link stops the reach, and is named. + run(&mut s, "edit.undo", json!({})); + run(&mut s, "joint.limits", json!({"joint": "Elbow", "min": -30, "max": 30})); + let r = reach(&mut s, "Fore", tip, json!({"point": to})); + assert_eq!(r["reached"], false, "{r}"); + assert!(r["stopped_by_limit"].as_array().is_some_and(|l| l.contains(&json!({"joint": "Elbow", "value": "angle", "limit": "min"}))), "{r}"); +} + +/// A target straight across the pivot from the point (where the misses don't change to first +/// order) is still reached: the arm turns half round, the way its limits leave open. +#[test] +fn a_target_across_the_pivot_is_reached() { + let tip = Vec3::new(100.0, 0.0, 0.0); + for (limits, want) in [(None, None), (Some([-180, 0]), Some(-180.0)), (Some([0, 180]), Some(180.0))] { + let mut s = arm(false, 0.0); + if let Some([min, max]) = limits { + run(&mut s, "joint.limits", json!({"joint": "Shoulder", "min": min, "max": max})); + } + let r = reach(&mut s, "Fore", tip, json!({"point": [-100, 0, 0], "joints": ["Shoulder"]})); + assert_eq!(r["reached"], true, "{limits:?}: {r}"); + let got = value(&mut s, "Shoulder"); + assert!((got.abs() - 180.0).abs() < 1e-6 && want.is_none_or(|w| (got - w).abs() < 1e-6), "{limits:?}: {got}"); + } +} + +/// A loop that only closes again after a full turn of a value geared up from the arm's (a joint +/// between two grounded parts, linked 36:1 to the shoulder) holds the arm: a step of a tenth of a +/// turn would close it at both ends, but not on the way. +#[test] +fn a_loop_geared_up_from_the_chain_holds_it() { + let tip = Vec3::new(100.0, 0.0, 0.0); + let mut s = arm(false, 0.0); + part(&mut s, "Lock", [-1.0, 5.0, -1.0]); + let lock = occ(&s, "Lock"); + run(&mut s, "occurrence.ground", json!({"occurrence": lock, "grounded": true})); + pin(&mut s, "revolute", "Lock", "Base", "Lock", Vec3::ZERO, Vec3::Z); + run(&mut s, "joint.motion_link", json!({"a": "Shoulder", "b": "Lock", "ratio": 36})); + let r = reach(&mut s, "Fore", tip, json!({"point": [0, 1e6, 0], "joints": ["Shoulder"]})); + assert_eq!((&r["reached"], &r["stopped_by_loop"]), (&json!(false), &json!({"joint": "Lock"})), "{r}"); + assert!(value(&mut s, "Shoulder").abs() < 1e-6, "{r}"); + // Geared up so far that even the small kick off a target straight across the pivot would turn + // the loop a whole turn: it holds too. + run(&mut s, "joint.delete", json!({"link": 0})); + run(&mut s, "joint.motion_link", json!({"a": "Shoulder", "b": "Lock", "ratio": 36000})); + let r = reach(&mut s, "Fore", tip, json!({"point": [-100, 0, 0], "joints": ["Shoulder"]})); + assert_eq!(r["reached"], false, "{r}"); + assert!(value(&mut s, "Shoulder").abs() < 1e-6, "{r}"); +} + +/// A planar joint far from the point turns it and slides the long way round, and the answer +/// gives the values as joint.list does. +#[test] +fn a_planar_joint_far_from_the_point_turns_and_slides() { + let mut s = Session::default(); + part(&mut s, "Base", [-1.0, -1.0, -1.0]); + part(&mut s, "Puck", [99.0, -1.0, 2.0]); + let base = occ(&s, "Base"); + run(&mut s, "occurrence.ground", json!({"occurrence": base, "grounded": true})); + pin(&mut s, "planar", "Table", "Base", "Puck", Vec3::ZERO, Vec3::Z); + let r = reach(&mut s, "Puck", Vec3::new(100.0, 0.0, 0.0), json!({"point": [100, 0, 0], "z": [0, 1, 0], "axis": [1, 0, 0]})); + assert_eq!(r["reached"], true, "{r}"); + let l = run(&mut s, "joint.list", json!({"quiet": true})); + let listed = l["joints"].as_array().and_then(|js| js.iter().find(|j| j["name"] == "Table")).map(|j| j["values"].clone()); + assert_eq!(Some(r["values"][0]["values"].clone()), listed, "{r}"); + assert!((r["values"][0]["values"][2]["value"].as_f64().unwrap_or(f64::NAN) - 90.0).abs() < 1e-6, "{r}"); +} + +/// A slider on its own (a chain with no size) still travels the whole way. +#[test] +fn a_lone_slider_travels_all_the_way() { + let mut s = Session::default(); + part(&mut s, "Base", [-1.0, -1.0, -1.0]); + part(&mut s, "Ram", [-1.0, -1.0, 2.0]); + let base = occ(&s, "Base"); + run(&mut s, "occurrence.ground", json!({"occurrence": base, "grounded": true})); + pin(&mut s, "slider", "Rail", "Base", "Ram", Vec3::ZERO, Vec3::Z); + let r = reach(&mut s, "Ram", Vec3::ZERO, json!({"point": [0, 0, 100]})); + assert_eq!(r["reached"], true, "{r}"); + assert!((value(&mut s, "Rail") - 100.0).abs() < 1e-6, "{r}"); + // However far. + let r = reach(&mut s, "Ram", Vec3::ZERO, json!({"point": [0, 0, 3e7]})); + assert_eq!(r["reached"], true, "{r}"); + // A limit on it is named whatever the chain's size. + run(&mut s, "edit.undo", json!({})); + run(&mut s, "joint.limits", json!({"joint": "Rail", "min": -50, "max": 100})); + let r = reach(&mut s, "Ram", Vec3::ZERO, json!({"point": [0, 0, 1e6]})); + assert_eq!(r["stopped_by_limit"], json!([{"joint": "Rail", "value": "distance", "limit": "max"}]), "{r}"); + // And a reach of a thousandth of a millimetre still moves it. + let r = reach(&mut s, "Ram", Vec3::ZERO, json!({"point": [0, 0, 99.999]})); + assert_eq!((&r["reached"], &r["moved"]), (&json!(true), &json!(true)), "{r}"); +} + +/// The four-bar's rocker reaches a point on its swing, the crank and coupler following with the +/// loop closed; past its toggle position it stops there, and the loop is named. +#[test] +fn a_four_bar_reaches_with_its_rocker() { + let (mut s, _, q0) = four_bar(); + let o4 = Vec3::new(45.0, 0.0, 0.0); + let start = q0.y.atan2(q0.x - 45.0); + let swing = |deg: f64| o4 + Vec3::new((start + deg.to_radians()).cos(), (start + deg.to_radians()).sin(), 0.0) * 40.0; + let r = run(&mut s, "joint.reach", json!({"occurrence": "Rocker", "at": pt(q0), "point": pt(swing(-10.0))})); + assert_eq!((&r["reached"], &r["stopped_by_loop"]), (&json!(true), &Value::Null), "{r}"); + assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}"); + assert!(place(&s, "Rocker", q0).dist(swing(-10.0)) < 1e-6 && place(&s, "Coupler", q0).dist(swing(-10.0)) < 1e-6, "{r}"); + // The crank turned to where the loop closes with the rocker there (its pin on the coupler's circle). + let th = (90.0 + value(&mut s, "Crank")).to_radians(); + let crank_pin = Vec3::new(20.0 * th.cos(), 20.0 * th.sin(), 0.0); + assert!((crank_pin.dist(swing(-10.0)) - 50.0).abs() < 1e-6 && place(&s, "Coupler", Vec3::new(0.0, 20.0, 0.0)).dist(crank_pin) < 1e-6, "{r}"); + // Past its toggle position (|Q| = 30) the loop stops it there. + let (mut s, _, q0) = four_bar(); + let r = run(&mut s, "joint.reach", json!({"occurrence": "Rocker", "at": pt(q0), "point": pt(swing(100.0))})); + assert_eq!((&r["reached"], &r["stopped_by_loop"]), (&json!(false), &json!({"joint": "Rocker pin"})), "{r}"); + // Its joints have no limits, so none is named. + assert_eq!(r["stopped_by_limit"], Value::Null, "{r}"); + assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}"); + let limit = ((900.0f64 - 3625.0) / 3600.0).acos().to_degrees() - start.to_degrees(); + let at = value(&mut s, "Rocker"); + assert!(at > limit - 0.5 && at <= limit + 1e-6, "stopped at {at}°, the toggle is at {limit}°: {r}"); + let [m1, m2] = meet(o4, 40.0, Vec3::ZERO, 30.0); + assert!(place(&s, "Rocker", q0).dist(m1).min(place(&s, "Rocker", q0).dist(m2)) < 0.5, "{r}"); +} + +#[test] +fn reach_refuses_bad_params_and_says_why_nothing_moves() { + let mut s = arm(false, 30.0); + for bad in [ + json!({"occurrence": "Fore"}), + json!({"occurrence": "Nope", "point": [0, 0, 0]}), + json!({"occurrence": "Fore", "point": [1e300, 0, 0]}), + json!({"occurrence": "Fore", "point": [0, 0, "x"]}), + json!({"occurrence": "Fore", "point": [0, 0, 0], "z": [0, 0, 0]}), + json!({"occurrence": "Fore", "point": [0, 0, 0], "axis": [1, 0, 0]}), + json!({"occurrence": "Fore", "point": [0, 0, 0], "joints": "Elbow"}), + json!({"occurrence": "Fore", "point": [0, 0, 0], "joints": ["Nope"]}), + json!({"occurrence": "Fore", "point": [0, 0, 0], "at": [f64::MAX, 0, 0]}), + ] { + assert!(s.execute("joint.reach", &bad).is_err(), "{bad}"); + } + // A grounded part, a part no joint places, and joints that don't move it: nothing moves, and + // the answer says why. + part(&mut s, "Loose", [200.0, 0.0, 0.0]); + for (p, why) in [ + (json!({"occurrence": "Base", "point": [5, 5, 5]}), "fixed in place"), + (json!({"occurrence": "Loose", "point": [5, 5, 5]}), "not connected"), + (json!({"occurrence": "Upper", "point": [5, 5, 5], "joints": ["Elbow"]}), "none of the joints given"), + ] { + let r = run(&mut s, "joint.reach", p.clone()); + assert_eq!((&r["reached"], &r["moved"]), (&json!(false), &json!(false)), "{p}: {r}"); + assert!(r["reason"].as_str().is_some_and(|w| w.contains(why)), "{p}: {r}"); + } +} diff --git a/crates/engine/src/cmd/joints_tests.rs b/crates/engine/src/cmd/joints_tests.rs index 1c86e632..66da370e 100644 --- a/crates/engine/src/cmd/joints_tests.rs +++ b/crates/engine/src/cmd/joints_tests.rs @@ -5,19 +5,19 @@ use solidcraft_geom::Vec3; use crate::Session; -fn run(s: &mut Session, id: &str, p: Value) -> Value { +pub(super) fn run(s: &mut Session, id: &str, p: Value) -> Value { match s.execute(id, &p) { Ok(v) => v, Err(e) => panic!("{id} {p}: {e}"), } } -fn occ(s: &Session, component: &str) -> u64 { +pub(super) fn occ(s: &Session, component: &str) -> u64 { let c = s.doc.find_component(component).unwrap_or(0); s.doc.occurrence_of(c).map(|o| o.id).unwrap_or(0) } -fn place(s: &Session, component: &str, p: Vec3) -> Vec3 { +pub(super) fn place(s: &Session, component: &str, p: Vec3) -> Vec3 { solidcraft_doc::apply_point(&solidcraft_doc::joints::occurrence_world(&s.doc, occ(s, component)), p) } @@ -313,21 +313,21 @@ fn interference_box_sweep_matches_every_pair_check() { } /// A small block in a new component: a part of a linkage, where only its joints matter. -fn part(s: &mut Session, name: &str, corner: [f64; 3]) { +pub(super) fn part(s: &mut Session, name: &str, corner: [f64; 3]) { run(s, "component.activate", json!({"component": "root"})); run(s, "component.create", json!({"name": name})); run(s, "solid.box", json!({"length": 2, "width": 2, "height": 2, "corner": corner})); } /// A joint between two parts as they are, at a world point with its z along `z`. -fn pin(s: &mut Session, kind: &str, name: &str, a: &str, b: &str, p: Vec3, z: Vec3) { +pub(super) fn pin(s: &mut Session, kind: &str, name: &str, a: &str, b: &str, p: Vec3, z: Vec3) { let at = |o: &str| json!({"occurrence": o, "point": [p.x, p.y, p.z], "z": [z.x, z.y, z.z]}); let r = run(s, "joint.create", json!({"type": kind, "a": at(a), "b": at(b), "flip": true, "name": name})); assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}"); } /// Where circles about `p` (radius `r`) and `q` (radius `s`) in the XY plane meet. -fn meet(p: Vec3, r: f64, q: Vec3, s: f64) -> [Vec3; 2] { +pub(super) fn meet(p: Vec3, r: f64, q: Vec3, s: f64) -> [Vec3; 2] { let d = q - p; let l = d.len(); let a = (r * r - s * s + l * l) / (2.0 * l); @@ -339,7 +339,7 @@ fn meet(p: Vec3, r: f64, q: Vec3, s: f64) -> [Vec3; 2] { /// A crank-rocker four-bar about +Z: ground pivots O2 = (0, 0) and O4 = (45, 0), crank 20, /// coupler 50, rocker 40, assembled with the crank at 90°. Returns the crank pin P0 and the /// rocker pin Q0 (the upper, open assembly). -fn four_bar() -> (Session, Vec3, Vec3) { +pub(super) fn four_bar() -> (Session, Vec3, 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); diff --git a/crates/ui-egui/src/dialogs_assembly.rs b/crates/ui-egui/src/dialogs_assembly.rs index 3010838e..06fd681a 100644 --- a/crates/ui-egui/src/dialogs_assembly.rs +++ b/crates/ui-egui/src/dialogs_assembly.rs @@ -343,7 +343,7 @@ fn pt(v: Vec3) -> Value { } /// The occurrence placing a body (0: the root design), when the body is its primary instance. -fn occurrence_of_body(s: &Session, body: &str) -> Option { +pub(crate) fn occurrence_of_body(s: &Session, body: &str) -> Option { let st = s.world_state(); let b = st.body(body)?; let comp = s.doc.model_body_component(b); diff --git a/crates/ui-egui/src/joint_drag.rs b/crates/ui-egui/src/joint_drag.rs new file mode 100644 index 00000000..2b2929b1 --- /dev/null +++ b/crates/ui-egui/src/joint_drag.rs @@ -0,0 +1,214 @@ +//! Dragging a part that joints place moves it as far as its joints allow: each frame runs +//! `joint.reach`, taking the point grabbed toward the pointer in the plane through it that faces +//! the view. The whole drag is one undo step. + +use egui::Pos2; +use serde_json::json; +use solidcraft_engine::doc::{apply_point, joints, mat_inverse}; +use solidcraft_engine::geom::Vec3; + +use crate::SolveApp; +use crate::viewport::{Hit, Proj}; + +/// A drag under way. +#[derive(Clone, Debug)] +pub struct Drag { + occurrence: u64, + /// The point grabbed, in the occurrence's own coordinates. + local: Vec3, + /// The plane the pointer moves the point in: through where it was grabbed, facing the view. + origin: Vec3, + normal: Vec3, + /// The drag's undo step is on top of the history (it went in with the first frame that moved). + pushed: bool, +} + +/// Begin a drag on the part under `p` when joints place it and leave it free to move (it is +/// neither grounded nor fixed to what is). True when one began. +pub fn start(app: &mut SolveApp, proj: &Proj, p: Pos2) -> bool { + // The face under the pointer, past the part's own edges and vertices (not past sketches or + // construction geometry in front of it). + let hits = crate::viewport::pick(app, proj, p); + let Some((body, at)) = + hits.into_iter().take_while(|h| matches!(h, Hit::Face { .. } | Hit::Edge { .. } | Hit::Vertex { .. })).find_map(|h| match h { + Hit::Face { body, point, .. } => Some((body, point)), + _ => None, + }) + else { + return false; + }; + let s = &app.session; + let Some(occurrence) = crate::dialogs_assembly::occurrence_of_body(s, &body).filter(|o| *o != 0) else { return false }; + if !s.doc.assembly.joints.iter().any(|j| !j.suppressed && (j.a.occurrence == occurrence || j.b.occurrence == occurrence)) { + return false; + } + let sol = joints::solve(&s.doc, &s.model.state()); + if !sol.world.contains_key(&occurrence) || sol.dof.get(&occurrence).is_none_or(|d| *d == 0) { + return false; + } + let Some(local) = mat_inverse(&joints::occurrence_world(&s.doc, occurrence)).map(|inv| apply_point(&inv, at)) else { return false }; + let normal = proj.ray(p).1; + app.viewport.joint_drag = Some(Drag { occurrence, local, origin: at, normal, pushed: false }); + true +} + +pub fn active(app: &SolveApp) -> bool { + app.viewport.joint_drag.is_some() +} + +/// Reach for the point under `p`. +pub fn to(app: &mut SolveApp, proj: &Proj, p: Pos2) { + let Some(d) = app.viewport.joint_drag.clone() else { return }; + let (o, dir) = proj.ray(p); + let facing = dir.dot(d.normal); + if facing.abs() < 1e-9 { + return; + } + let target = o + dir * ((d.origin - o).dot(d.normal) / facing); + if !target.is_finite() { + return; + } + let at = apply_point(&joints::occurrence_world(&app.session.doc, d.occurrence), d.local); + let params = + json!({"occurrence": d.occurrence, "at": [at.x, at.y, at.z], "point": [target.x, target.y, target.z], "quiet": true, "continuous": true}); + // One undo step for the whole drag: the first frame's, kept on top while the others run (not + // truncated by depth, which a full history would make lose its oldest steps frame by frame). + let own = if d.pushed { app.session.undo.pop() } else { None }; + // A frame that changes the design adds an undo step (a full history drops its oldest for it, + // so its length says nothing). + let before = app.session.doc.clone(); + let pushed = app.run("joint.reach", params).is_ok() && !std::sync::Arc::ptr_eq(&before, &app.session.doc); + match own { + Some(step) => { + if pushed { + app.session.undo.pop(); + } + app.session.undo.push(step); + } + None if pushed => { + if let Some(d) = app.viewport.joint_drag.as_mut() { + d.pushed = true; + } + } + None => {} + } +} + +pub fn end(app: &mut SolveApp) { + app.viewport.joint_drag = None; +} + +#[cfg(test)] +mod tests { + use serde_json::{Value, json}; + use solidcraft_engine::doc::{apply_point, joints}; + use solidcraft_engine::geom::Vec3; + + use crate::scenario::Harness; + + fn run(h: &mut Harness, id: &str, p: Value) -> Value { + h.app.run(id, p.clone()).unwrap_or_else(|e| panic!("{id} {p}: {e}")) + } + + fn screen(h: &mut Harness, p: Vec3) -> egui::Pos2 { + let [x, y] = h.at(&json!({"world": [p.x, p.y, p.z]})).expect("on screen"); + egui::pos2(x as f32, y as f32) + } + + /// Where the arm's point `local` (its own coordinates) is. + fn arm_at(h: &Harness, local: Vec3) -> Vec3 { + let s = &h.app.session; + let c = s.doc.find_component("Arm").unwrap_or(0); + let o = s.doc.occurrence_of(c).map(|o| o.id).unwrap_or(0); + apply_point(&joints::occurrence_world(&s.doc, o), local) + } + + /// An arm hinged about +Z on a grounded base (seen from the top): its far end, and the screen + /// points of that end and of the end turned a quarter round. + fn hinged_arm() -> (Harness, Vec3, egui::Pos2, egui::Pos2) { + let mut h = Harness::new(); + h.app.home.open = false; + for (name, corner, size) in [("Base", [-5, -5, 0], [10, 10, 5]), ("Arm", [-3, -3, 5], [63, 6, 5])] { + run(&mut h, "component.activate", json!({"component": "root"})); + run(&mut h, "component.create", json!({"name": name})); + run(&mut h, "solid.box", json!({"length": size[0], "width": size[1], "height": size[2], "corner": corner})); + } + run(&mut h, "component.activate", json!({"component": "root"})); + let base = h.app.session.doc.find_component("Base").and_then(|c| h.app.session.doc.occurrence_of(c)).map(|o| o.id).unwrap_or(0); + run(&mut h, "occurrence.ground", json!({"occurrence": base, "grounded": true})); + let at = |o: &str| json!({"occurrence": o, "point": [0, 0, 5], "z": [0, 0, 1]}); + run(&mut h, "joint.create", json!({"type": "revolute", "a": at("Base"), "b": at("Arm"), "flip": true, "name": "Hinge"})); + h.call("ui.view", json!({"view": "top"})); + h.app.cam.distance = 200.0; + h.frames(3); + let end = Vec3::new(55.0, 0.0, 10.0); + let (a, b) = (screen(&mut h, end), screen(&mut h, Vec3::new(0.0, 55.0, 10.0))); + (h, end, a, b) + } + + fn play(h: &mut Harness) { + while !h.app.synthetic.is_empty() { + h.frame(); + } + h.frames(2); + } + + /// Press at `a` and move to `b` in 12 frames, holding the button there. + fn press_and_move(h: &mut Harness, a: egui::Pos2, b: egui::Pos2) { + h.app.synthetic.push(egui::Event::PointerMoved(a)); + h.app.synthetic.push(egui::Event::PointerButton { + pos: a, + button: egui::PointerButton::Primary, + pressed: true, + modifiers: Default::default(), + }); + h.app.synthetic.extend((1..=12).map(|k| egui::Event::PointerMoved(a + (b - a) * (k as f32 / 12.0)))); + play(h); + } + + fn release(h: &mut Harness, b: egui::Pos2) { + h.app.synthetic.push(egui::Event::PointerButton { + pos: b, + button: egui::PointerButton::Primary, + pressed: false, + modifiers: Default::default(), + }); + play(h); + } + + /// Dragged by its far end a quarter turn round, the arm turns there in one undo step, and undo + /// turns it back. + #[test] + fn dragging_a_jointed_part_turns_it_about_its_hinge() { + let (mut h, end, a, b) = hinged_arm(); + let undo = h.app.session.undo.len(); + press_and_move(&mut h, a, b); + assert!(h.app.viewport.joint_drag.is_some() && h.app.viewport.boxsel.is_none(), "the drag moves the arm, not a selection box"); + release(&mut h, b); + assert!(h.app.viewport.joint_drag.is_none()); + assert!(arm_at(&h, end).dist(Vec3::new(0.0, 55.0, 10.0)) < 0.5, "{:?}", arm_at(&h, end)); + assert_eq!(h.app.session.undo.len(), undo + 1, "one undo step for the drag"); + assert!(!h.app.session.log.iter().any(|l| l.contains("Reach")), "frames are not echoed: {:?}", h.app.session.log); + run(&mut h, "edit.undo", json!({})); + assert!(arm_at(&h, end).dist(end) < 1e-9, "{:?}", arm_at(&h, end)); + } + + /// With the undo history full, a drag still adds one step: it drops the oldest once, not one + /// per frame. + #[test] + fn a_drag_on_a_full_undo_history_drops_one_step() { + let (mut h, _, a, b) = hinged_arm(); + let step = h.app.session.undo.last().cloned().expect("an undo step"); + while h.app.session.undo.len() < 200 { + let mut older = step.clone(); + older.label = format!("step {}", h.app.session.undo.len()); + h.app.session.undo.insert(0, older); + } + let second = h.app.session.undo.get(1).map(|s| s.label.clone()); + press_and_move(&mut h, a, b); + release(&mut h, b); + assert_eq!(h.app.session.undo.len(), 200); + assert_eq!(h.app.session.undo.first().map(|s| s.label.clone()), second, "only the oldest step went"); + assert_eq!(h.app.session.undo.last().map(|s| s.label.as_str()), Some("Reach Point"), "the drag's step is on top"); + } +} diff --git a/crates/ui-egui/src/lib.rs b/crates/ui-egui/src/lib.rs index a740dd3b..6a5bf6b4 100644 --- a/crates/ui-egui/src/lib.rs +++ b/crates/ui-egui/src/lib.rs @@ -62,6 +62,7 @@ pub mod icons; pub mod inference; pub mod install; pub mod issue_report; +pub mod joint_drag; pub mod keymap; mod material_card_review; #[cfg(test)] @@ -632,7 +633,9 @@ impl SolveApp { // reads the same way the toolbar button it came from does. let language = i18n::Locale::for_language(&self.preferences.language); let label = language.label(command_label(id)); - if !(id == "view.navigate" && v.get("continuous").and_then(Value::as_bool) == Some(true)) { + // Frames of a gesture (a continuous view move, a part dragged by its joints) are + // not echoed one by one. + if v.get("continuous").and_then(Value::as_bool) != Some(true) { self.session.echo(language.format("ui.command_done", &[("command", label)])); } if id == "file.new" { diff --git a/crates/ui-egui/src/viewport.rs b/crates/ui-egui/src/viewport.rs index d02d06e1..2bcc6464 100644 --- a/crates/ui-egui/src/viewport.rs +++ b/crates/ui-egui/src/viewport.rs @@ -118,6 +118,8 @@ pub struct ViewportState { pub point_drag: Option<(String, usize, Option)>, /// Checkpoint and source for a cancellable whole-text drag. pub(crate) text_drag: Option<(u64, String)>, + /// A part that joints place, being dragged. + pub(crate) joint_drag: Option, } /// Something under the cursor. @@ -2493,12 +2495,32 @@ pub fn show(app: &mut SolveApp, ui: &mut egui::Ui) { if crate::sketch_text_handles::dragging(app) && !crate::issue_report::cad_input_blocked() && !ui.input(|i| i.pointer.primary_down()) { crate::sketch_text_handles::drag_end(app); } + // Dragging a part that joints place moves it as far as they allow (outside sketches). + if app.tool.is_none() && app.dialog.is_none() && primary_nav.is_none() && mode.is_none() && app.session.active_sketch.is_none() && !held { + if resp.drag_started_by(egui::PointerButton::Primary) + && let Some(a) = ui.input(|i| i.pointer.press_origin()) + && rect.contains(a) + && !axes.contains(a) + { + crate::joint_drag::start(app, &proj, a); + } + if crate::joint_drag::active(app) + && resp.dragged_by(egui::PointerButton::Primary) + && let Some(p) = hover + { + crate::joint_drag::to(app, &proj, p); + } + } + if crate::joint_drag::active(app) && (resp.drag_stopped() || !ui.input(|i| i.pointer.primary_down())) { + crate::joint_drag::end(app); + } // Box selection: a primary drag on the model when no navigation mode is on. if app.tool.is_none() && !held && primary_nav.is_none() && mode.is_none() && app.viewport.point_drag.is_none() + && !crate::joint_drag::active(app) && !crate::dim_view::dragging() && !crate::sketch3d::dragging() && !crate::sketch_text_handles::dragging(app) diff --git a/docs/book/src/guide/assemblies.md b/docs/book/src/guide/assemblies.md index 9b2ff893..ca29a561 100644 --- a/docs/book/src/guide/assemblies.md +++ b/docs/book/src/guide/assemblies.md @@ -76,6 +76,21 @@ why. While it previews, a row under the values gives the value the joint reached status line repeats it. The value box keeps what you typed. The dialog opens on the joint picked in the browser, or on the first joint that moves when none is picked. +**Drag a part that joints place** and it moves as far as its joints allow: the point you +grabbed follows the pointer in the plane through it that faces the view, the joints on the +part's chain take the values that bring it nearest, within their limits, through motion links, +and with closed loops kept closed, and a four-bar's rocker stops at the end of its swing. The +whole drag is one undo step. A grounded part, or one no joint places, is not moved; a drag on +it draws a selection box as before. Each frame runs `joint.reach`, which scripts and agents use +directly to put a part's point somewhere: `joint.reach {occurrence, point}` moves the joints on +its chain from where they are to the nearest pose (the arm keeps its elbow up or down, and a +linkage stays on its branch). `at` picks the part's point (the centre of its bodies by +default), `z` with `axis` also turns one of its axes, and `joints` names the only joints to +move (the other joints of a closed loop they are in follow). The answer says whether it +`reached` the point and how close it got (`error_mm`, `error_deg`), and when it falls short, +the limits it rests on (`stopped_by_limit`) or the loop that stopped it (`stopped_by_loop`). +Contact sets are not checked by a reach; drive the joints for that. + The solver places what it reaches from the root and from grounded components. A chain of joints that is joined to nothing grounded is never placed, so driving one of its joints moves nothing. The drive says so: `joint.drive` answers `moved: false` with a `reason` and the `unconnected` diff --git a/skills/solidcraft-mechanisms/SKILL.md b/skills/solidcraft-mechanisms/SKILL.md index 56b29217..535bcdc0 100644 --- a/skills/solidcraft-mechanisms/SKILL.md +++ b/skills/solidcraft-mechanisms/SKILL.md @@ -74,6 +74,7 @@ Written against SolidCraft `main` after 0.11.1, which counts the degrees of free |---|---| | "The origins are at the same point, so `flip` doesn't matter" | Pass `flip: true`, or the part turns 180 degrees. | | "The drive answered values, so it moved" | Read `moved`; a chain joined to nothing grounded answers `moved: false` and a `reason`. | +| "Drive the arm joint by joint until the gripper gets there" | `joint.reach {"occurrence":..., "at":..., "point":...}` solves every joint on its chain at once; read `reached` and `error_mm`. | | "Key every joint of the loop in the study" | Key the driving joint only; the loop follows. | | "Check the pose with `component.list`" | `occurrence.properties` on the one occurrence. `component.list` is 140 KB on a 36-part assembly. | | "Open the design with `file.open`" | The `open` tool, or `doc.open`; there is no `file.open`. | diff --git a/skills/solidcraft-mechanisms/reference.md b/skills/solidcraft-mechanisms/reference.md index 99adc387..aee1fa9b 100644 --- a/skills/solidcraft-mechanisms/reference.md +++ b/skills/solidcraft-mechanisms/reference.md @@ -47,6 +47,7 @@ A slider's value grows as b moves along +z of its origin. Give its two origins t {"command":"joint.motion_link","params":{"a":"Hinge","b":"Hinge 2","ratio":-1}} {"command":"joint.drive","params":{"joint":"Hinge","value":20}} {"command":"joint.drive","params":{"joint":"Spindle","values":[30,5]}} +{"command":"joint.reach","params":{"occurrence":"Gripper","at":[120,0,40],"point":[60,80,40],"z":[0,0,-1],"axis":[1,0,0],"quiet":true}} {"command":"joint.limits","params":{"joint":"Hinge","min":0,"max":60}} {"command":"joint.edit","params":{"joint":"Hinge","flip":true}} {"command":"joint.delete","params":{"link":0}} @@ -64,6 +65,7 @@ A slider's value grows as b moves along +z of its origin. Give its two origins t - `joint.motion_link`: b = ratio·a + offset, clamped to b's limits. `ratio` is in b's units per a's: mm per degree when an angle drives a distance (a pinion of pitch radius r moves its rack r·π/180 mm per degree), degrees per mm the other way, and a plain factor between two angles or two distances. `offset` is in b's units: degrees for an angle, mm for a distance, or an expression such as `"10 deg"`. `joint.list` gives each link's `ratio` the same way and its `offset` with its `unit`. `ia`/`ib` pick a value of a multi-value joint (default 0). `joint.delete {"link": n}` removes link n (its index in `joint.list` `links`). - `joint.gear {a, b, ra, rb, internal?, reverse?}`, `joint.rack_pinion {a, b, radius, reverse?}` and `joint.screw {joint | a, b; pitch, reverse?}` make a motion link whose ratio comes from its sizes: gears -ra/rb (+ra/rb internal), a rack r mm per radian of its pinion (either joint may be `a`), a screw pitch per turn. Each size is mm, an expression, or `{occurrence, circle: [x,y,z]}`: the radius of the round face there, read again whenever the design changes. The reply gives `link` (its index) and `ratio` as `joint.list` does; `link: n` in the params replaces link n and keeps its offset. A new coupling's offset keeps b where it is. `joint.list` `links` carry the `coupling` and an `error` when it can't be worked out (a picked face gone: the last ratio stays). A value two links would set, or a coupling that would drive its own driver, is refused. - `joint.drive`: `values` drives the values it lists, in order (`Spindle` above is cylindrical: 30 degrees, then 5 mm), and `value` the first; values left out keep theirs, or follow in a closed loop. Values are clamped to the limits. The reply gives `values` as `joint.list` does (`{name, value, unit, limits}`), and `moved`; when the joint can move nothing, a `reason`. +- `joint.reach` (inverse kinematics): moves the joints on the occurrence's chain so its point `at` (world, where it is now; default: the centre of its bodies) gets to `point`, from the pose it is in to the nearest one, within limits, through motion links, and with closed loops kept closed. `z` with `axis` (world, now; default its own z) also turns that axis. `joints: [...]` moves only those (loop followers still follow). The reply: `reached`, `error_mm` (and `error_deg`), `point` where it ended, `values` of the joints that moved, `moved`, and when short, `stopped_by_limit: [{joint, value, limit}]` or `stopped_by_loop: {joint}`. A part that nothing moves answers `moved: false` with a `reason`. Contact sets are not checked; check with `contact.check` afterwards. - `motion.study {"study":..., ...}` edits a study; `keys` given replace all keys. - `motion.play` sets the joints to the study's values at a step (fractions work) and changes the design. `motion.export` does not change it. @@ -127,6 +129,7 @@ The same pattern moved the solar wings of the SELENE rover (`Rover-v7.solidcraft | A gear, rack or screw turns its partner the wrong way | The joints' axes point opposite ways (or the rack lies on the other side of the pinion). Run the command again with `reverse: true` and `link: n`. | | `already follows a motion link` from `joint.gear` | A joint follows one link at most. Make the joint that already follows the driving one (`a`), as in a gear train G1 → G2 → G3. | | A linked joint stops following | Its value is clamped to its own limits. Hinge 2 in the example has `[[-60, 0]]`, so a drive of Hinge below 0 leaves it at 0. Give a mirrored joint the mirrored limits. | +| `joint.reach` answers `reached: false` | The point is out of reach: `point` and `error_mm` give the nearest pose. `stopped_by_limit` names the limits it rests on, `stopped_by_loop` the loop that can't follow further; with neither, the chain can't get there at all (too far, or out of its plane). | | A drive stops short, `stopped_by_contact` | Two bodies of a contact set touch. `fraction` is how far the move got. | | `occurrence.ground` says there is no occurrence `Frame` | It takes occurrence names only: `Frame:1`. | | `a joint needs two different occurrences` | Both origins found the same part. Give `occurrence` on both. If the parts are bodies of one component (an imported STEP file often brings them so), run `component.from_bodies {"bodies": [...]}` first. |