nx_robot_kinematics_gate.nx source
↩ module page · 123 lines · 6093 B
1// nx_robot_kinematics_gate.nx -- R-ROBO-3 GATE: robot kinematics + the full motion pipeline.
2// VERIFIES: (1) CoreXY FK<->IK round-trips EXACTLY (integer, no drift) on several poses;
3// (2) 2-link arm FORWARD kinematics hits exact KATs at axis angles (0/90/180 deg) and a 45-deg
4// case within Q10 tolerance with x==y symmetry; (3) CAPSTONE -- a Cartesian move is converted to
5// CoreXY motor deltas and fed through nx_robot_step, and the per-motor pulse counts EXACTLY equal
6// the IK motor deltas (pose -> motor-space -> synchronized stepper pulses, the whole "command a
7// robot to move" chain composes); (4) NEGATIVE CONTROL -- a corrupted motor value breaks the
8// round-trip, proving the checks have teeth. 100% sovereign, integer-only.
9// license_tier: ORIGINAL expect_exit: 0
10import "nx_syscalls.nx"
11import "nx_robot_kinematics.nx"
12import "nx_robot_step.nx"
13
14func sw(s: *u8) -> i64 { var n: i64 = 0; while s[n] != (0 as u8) { n = n + 1 } sys_write(1, s, n); return 0 }
15func sn(v: i64) -> i64 {
16 if v == 0 { sys_write(1, "0" as *u8, 1); return 0 }
17 var m: i64 = v
18 if m < 0 { sys_write(1, "-" as *u8, 1); m = 0 - m }
19 let d: *u8 = sys_mmap(24); var k: i64 = 0
20 while m > 0 { d[k] = (48 + (m % 10)) as u8; m = m / 10; k = k + 1 }
21 let o: *u8 = sys_mmap(24); var i: i64 = 0
22 while i < k { o[i] = d[k - 1 - i]; i = i + 1 }
23 sys_write(1, o, k); return 0
24}
25func check(name: *u8, cond: i64, tot: *i64) -> i64 {
26 if cond == 1 { sw(" ok " as *u8); tot[0] = tot[0] + 1 }
27 else { sw(" FAIL " as *u8); tot[1] = tot[1] + 1 }
28 sw(name); sw("\n" as *u8)
29 return 0
30}
31func abs_i(v: i64) -> i64 { if v < 0 { return 0 - v } return v }
32
33// one CoreXY round-trip: ik(x,y)->(A,B)->fk-> must return (x,y) exactly
34func corexy_roundtrip_ok(x: i64, y: i64) -> i64 {
35 let a: *i64 = sys_mmap(8) as *i64
36 let b: *i64 = sys_mmap(8) as *i64
37 corexy_ik(x, y, a, b)
38 let rx: *i64 = sys_mmap(8) as *i64
39 let ry: *i64 = sys_mmap(8) as *i64
40 corexy_fk(a[0], b[0], rx, ry)
41 if rx[0] == x { if ry[0] == y { return 1 } }
42 return 0
43}
44
45func main() -> i64 {
46 let tot: *i64 = sys_mmap(16) as *i64
47 tot[0] = 0; tot[1] = 0
48 sw("=== nx_robot_kinematics_gate R-ROBO-3 -- kinematics + pose->motor->pulses pipeline ===\n" as *u8)
49
50 // (1) CoreXY round-trip exact
51 var ok_rt: i64 = 1
52 if corexy_roundtrip_ok(1000, 1000) == 0 { ok_rt = 0 }
53 if corexy_roundtrip_ok(9000, 1000) == 0 { ok_rt = 0 }
54 if corexy_roundtrip_ok(5000, 3000) == 0 { ok_rt = 0 }
55 if corexy_roundtrip_ok(0, 0) == 0 { ok_rt = 0 }
56 check("CoreXY FK<->IK round-trips EXACTLY (integer, no drift)" as *u8, ok_rt, tot)
57
58 // (2) arm forward kinematics, exact axis-angle KATs (L1=100, L2=50)
59 let x: *i64 = sys_mmap(8) as *i64
60 let y: *i64 = sys_mmap(8) as *i64
61 var ok_arm: i64 = 1
62 arm2_fk(0, 0, 100, 50, x, y) // straight out +X -> (150, 0)
63 if x[0] != 150 { ok_arm = 0 }
64 if y[0] != 0 { ok_arm = 0 }
65 sw(" arm(0,0)=(" as *u8); sn(x[0]); sw("," as *u8); sn(y[0]); sw(") expect (150,0)\n" as *u8)
66 arm2_fk(256, 0, 100, 50, x, y) // shoulder 90deg -> (0, 150)
67 if x[0] != 0 { ok_arm = 0 }
68 if y[0] != 150 { ok_arm = 0 }
69 sw(" arm(90,0)=(" as *u8); sn(x[0]); sw("," as *u8); sn(y[0]); sw(") expect (0,150)\n" as *u8)
70 arm2_fk(0, 256, 100, 50, x, y) // elbow 90deg -> (100, 50)
71 if x[0] != 100 { ok_arm = 0 }
72 if y[0] != 50 { ok_arm = 0 }
73 sw(" arm(0,90)=(" as *u8); sn(x[0]); sw("," as *u8); sn(y[0]); sw(") expect (100,50)\n" as *u8)
74 arm2_fk(256, 256, 100, 50, x, y) // shoulder 90 + elbow 90 -> (-50, 100)
75 if x[0] != (0 - 50) { ok_arm = 0 }
76 if y[0] != 100 { ok_arm = 0 }
77 sw(" arm(90,90)=(" as *u8); sn(x[0]); sw("," as *u8); sn(y[0]); sw(") expect (-50,100)\n" as *u8)
78 check("2-link arm FORWARD kinematics: exact axis-angle KATs" as *u8, ok_arm, tot)
79
80 // (2b) arm at 45deg: x==y by symmetry, near 150*0.7071=106
81 arm2_fk(128, 0, 100, 50, x, y) // 45deg, elbow straight
82 sw(" arm(45,0)=(" as *u8); sn(x[0]); sw("," as *u8); sn(y[0]); sw(") expect ~ (106,106)\n" as *u8)
83 var ok45: i64 = 0
84 if x[0] == y[0] { if abs_i(x[0] - 106) <= 2 { ok45 = 1 } }
85 check("arm 45deg: x==y symmetry + within Q10 tolerance of 106" as *u8, ok45, tot)
86
87 // (3) CAPSTONE: Cartesian move -> CoreXY motor deltas -> step-gen -> pulse counts == motor deltas
88 let dx: i64 = 20
89 let dy: i64 = 8
90 let dA: *i64 = sys_mmap(8) as *i64
91 let dB: *i64 = sys_mmap(8) as *i64
92 corexy_ik(dx, dy, dA, dB) // motor-space delta of the move: dA=28, dB=12
93 sw(" cartesian move (dx=20,dy=8) -> motor delta (A=" as *u8); sn(dA[0]); sw(", B=" as *u8); sn(dB[0]); sw(")\n" as *u8)
94 let naxes: i64 = 2
95 let delta: *i64 = sys_mmap(64) as *i64
96 delta[0] = dA[0]; delta[1] = dB[0]
97 let dir: *i64 = sys_mmap(64) as *i64
98 let maxt: i64 = 256
99 let puls: *i64 = sys_mmap(maxt * naxes * 8) as *i64
100 let nt: i64 = robot_step_plan(delta, naxes, dir, puls, maxt)
101 let cA: i64 = robot_step_count(puls, naxes, nt, 0)
102 let cB: i64 = robot_step_count(puls, naxes, nt, 1)
103 sw(" step-gen pulses: motor A=" as *u8); sn(cA); sw(" motor B=" as *u8); sn(cB); sw("\n" as *u8)
104 var ok_cap: i64 = 0
105 if cA == dA[0] { if cB == dB[0] { ok_cap = 1 } }
106 check("CAPSTONE: pose->motor-delta->step-gen pulses match EXACTLY (full pipeline)" as *u8, ok_cap, tot)
107
108 // (4) negative control: corrupt a motor value -> round-trip must FAIL
109 let a2: *i64 = sys_mmap(8) as *i64
110 let b2: *i64 = sys_mmap(8) as *i64
111 corexy_ik(5000, 3000, a2, b2)
112 let rx: *i64 = sys_mmap(8) as *i64
113 let ry: *i64 = sys_mmap(8) as *i64
114 corexy_fk(a2[0] + 2, b2[0], rx, ry) // corrupt motor A by +2
115 var neg_ok: i64 = 0
116 if rx[0] != 5000 { neg_ok = 1 } // corruption MUST be detectable
117 check("NEG-CONTROL: corrupted motor value breaks round-trip (checks have teeth)" as *u8, neg_ok, tot)
118
119 sw("=== VERDICT pass=" as *u8); sn(tot[0]); sw(" fail=" as *u8); sn(tot[1]); sw(" ===\n" as *u8)
120 if tot[1] == 0 { sys_exit(0) }
121 sys_exit(1)
122 return 1
123}