Joints: joint.reach (inverse kinematics), and a jointed part follows a drag (#1540)

Thanks @SychPL.

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`. |

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. |