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