nx_ik.nx source
↩ module page · 87 lines · 4715 B
1// nx_ik.nx -- CCD INVERSE KINEMATICS on nx_skeleton (Elara bar E5: "grab a wrist, the arm follows"; the same
2// solver = robot-arm reach, per the reuse law). Cyclic Coordinate Descent: per sweep, each bone in the chain
3// nudges its yaw/elevation by a fixed integer step in the direction that closes the effector->target angle --
4// signs come from CROSS PRODUCTS (no arctangent, no float), so the whole solve is integer-deterministic.
5// Yaw sign law: +yaw rotates +x toward -z (CW in the XZ plane seen from +y), so yaw -= sign(crossY)*step.
6// Elevation sign law: es = ty*|eff_h| - effy*|t_h| (elevation-angle comparison without division).
7// license_tier: ORIGINAL
8import "nx_syscalls.nx"
9import "nx_skeleton.nx"
10const K_MAGIC_4096: i64 = 4096
11
12func ik_isqrt(v: i64) -> i64 {
13 if v <= 0 { return 0 }
14 var x: i64 = v
15 var y: i64 = (x + 1) / 2
16 while y < x { x = y; y = (x + v / x) / 2 }
17 return x
18}
19// effector world position: tip bone's world transform applied to a local offset (e.g., (boneLen,0,0))
20func ik_effector(sk: i64, tipBone: i64, ex: i64, ey: i64, ez: i64, out: *i64) -> i64 {
21 let b: *i64 = sk_bone(sk, tipBone)
22 sk_matvec((b as i64 + 48) as *i64, ex, ey, ez, out)
23 out[0] = out[0] + b[15]
24 out[1] = out[1] + b[16]
25 out[2] = out[2] + b[17]
26 return 0
27}
28
29// solve: nudge every chain bone (tip..root walk via parents, cap 8) for `iters` sweeps; returns final distance.
30// PROPORTIONAL + ANNEALED nudge: the yaw/elev step is the actual angle error (sinθ≈crossy/(uh*vh), scaled to
31// rad4096) times an annealing gain that decays over the sweeps -- big early (close fast), tiny late (SETTLE,
32// no oscillation). Fixed ±step CCD oscillated near the target (gate: worstd 892, jitter 1376); proportional
33// annealing converges AND stays smooth. `step` is the STARTING gain cap.
34// rootStop = the LAST bone the solver may adjust (the chain walks tip->parents and stops AFTER it). Without
35// it the solver recruits the whole spine to reach -- charlab gate evidence: torso drift moved the bound
36// regions (residual swing 6 with breathing off), fought convergence (dist 131), dragged flesh from the hand.
37func ik_solve(sk: i64, tipBone: i64, rootStop: i64, ex: i64, ey: i64, ez: i64, tx: i64, ty: i64, tz: i64, iters: i64, step: i64) -> i64 {
38 let eff: *i64 = sk_vs(sk) // in-arena scratch (sys_mmap does not exist in-wasm)
39 var it2: i64 = 0
40 while it2 < iters {
41 // anneal the gain cap from `step` down to step/8 across the sweeps
42 var cap: i64 = step - step * 7 / 8 * it2 / iters
43 if cap < step / 8 { cap = step / 8 }
44 var bi: i64 = tipBone
45 while bi >= 0 {
46 sk_update(sk)
47 ik_effector(sk, tipBone, ex, ey, ez, eff)
48 let b: *i64 = sk_bone(sk, bi)
49 let ux: i64 = eff[0] - b[15]
50 let uy: i64 = eff[1] - b[16]
51 let uz: i64 = eff[2] - b[17]
52 let vx: i64 = tx - b[15]
53 let vy: i64 = ty - b[16]
54 let vz: i64 = tz - b[17]
55 // yaw: proportional to the XZ angle error. sinθ = crossy/(uh*vh); rad4096 ~ sinθ*4096.
56 // +yaw is CW in XZ -> subtract. Clamp the per-sweep move to the annealed cap.
57 let crossy: i64 = ux * vz - uz * vx
58 let uh: i64 = ik_isqrt(ux * ux + uz * uz)
59 let vh: i64 = ik_isqrt(vx * vx + vz * vz)
60 let denom: i64 = uh * vh / K_MAGIC_4096 + 1
61 var dyaw: i64 = crossy / denom
62 if dyaw > cap { dyaw = cap }
63 if dyaw < 0 - cap { dyaw = 0 - cap }
64 b[4] = b[4] - dyaw
65 // elevation is rotation about z: its plane is XY, so the error is the XY CROSS (signed --
66 // the earlier unsigned-horizontal metric FOLDED left/right together: a planar charlab arm could
67 // not tell a target behind it from one in front; gate evidence clear=1052, dist=401).
68 // +elev rotates +x toward +y = CCW in XY: positive cross (v CCW of u) -> elev += step.
69 let crossz: i64 = ux * vy - uy * vx
70 let um: i64 = ik_isqrt(ux * ux + uy * uy + uz * uz)
71 let vm: i64 = ik_isqrt(vx * vx + vy * vy + vz * vz)
72 let edenom: i64 = um * vm / K_MAGIC_4096 + 1
73 var delev: i64 = crossz / edenom
74 if delev > cap { delev = cap }
75 if delev < 0 - cap { delev = 0 - cap }
76 b[5] = b[5] + delev
77 if bi == rootStop { bi = 0 - 1 } else { bi = b[0] }
78 }
79 it2 = it2 + 1
80 }
81 sk_update(sk)
82 ik_effector(sk, tipBone, ex, ey, ez, eff)
83 let dx: i64 = eff[0] - tx
84 let dy: i64 = eff[1] - ty
85 let dz: i64 = eff[2] - tz
86 return ik_isqrt(dx * dx + dy * dy + dz * dz)
87}