nx_pose_retarget_gate.nx source

1// nx_pose_retarget_gate.nx -- prove RETARGET (roadmap R3) inverts the FK: known joint ANGLES -> FK -> joint 2// POSITIONS -> retarget -> (T1) positions REPRODUCED + (T2) angles RECOVERED + (T3) a 2-frame track gives 3// distinct poses. This is "a pose track drives our rig" -- the video->pose->MOTION heart. expect_exit:0 ORIGINAL 4import "nx_syscalls.nx" 5import "nx_skeleton.nx" 6import "nx_pose_retarget.nx" 7 8func hw(s: *u8) -> i64 { var n: i64=0; while s[n]!=(0 as u8){n=n+1} sys_write(1,s,n); return 0 } 9func pn(v: i64) -> i64 { let b: *u8=sys_mmap(32) as *u8; var x: i64=v; var ng: i64=0; if x<0{ng=1;x=0-x} var i: i64=31; if x==0{b[i]=48 as u8;i=i-1} while x>0{b[i]=(48+x%10) as u8;x=x/10;i=i-1} if ng==1{b[i]=45 as u8;i=i-1} sys_write(1,(b as i64+i+1) as *u8,31-i); return 0 } 10func iabs(v: i64) -> i64 { if v < 0 { return 0 - v } return v } 11 12const NB: i64 = 5 13 14func build_chain(base: i64) -> i64 { 15 sk_init(base) 16 sk_add_bone(base, 0 - 1, 0, 0, 0) // 0 root (fixed at origin) 17 sk_add_bone(base, 0, 500, 0, 0) // 1 18 sk_add_bone(base, 1, 500, 0, 0) // 2 19 sk_add_bone(base, 2, 500, 0, 0) // 3 20 sk_add_bone(base, 3, 500, 0, 0) // 4 (leaf) 21 return 0 22} 23// read the nbones joint positions into t (nbones*3) 24func read_targets(base: i64, t: *i64) -> i64 { 25 var i: i64 = 0 26 while i < NB { let b: *i64 = sk_bone(base, i); t[i*3]=b[15]; t[i*3+1]=b[16]; t[i*3+2]=b[17]; i = i + 1 } 27 return 0 28} 29 30func main() -> i64 { 31 hw("=== nx_pose_retarget_gate -- positions -> rig angles (R3 retarget inverts the FK) ===\n" as *u8) 32 let base: i64 = sys_mmap(sk_bytes()) as i64 33 build_chain(base) 34 // known angles (moderate -- elev well inside +-pi/2=6434); leaf angle irrelevant 35 let oy: *i64 = sys_mmap(NB * 8) as *i64 36 let oe: *i64 = sys_mmap(NB * 8) as *i64 37 oy[0]=2200; oe[0]=1400 38 oy[1]=0-1800; oe[1]=900 39 oy[2]=1200; oe[2]=0-1100 40 oy[3]=0-700; oe[3]=1600 41 oy[4]=0; oe[4]=0 42 var i: i64 = 0 43 while i < NB { sk_pose(base, i, oy[i], oe[i]); i = i + 1 } 44 sk_update(base) 45 let targets: *i64 = sys_mmap(NB * 3 * 8) as *i64 46 read_targets(base, targets) 47 48 // wipe angles, then retarget from the positions 49 i = 0 50 while i < NB { sk_pose(base, i, 0, 0); i = i + 1 } 51 rt_retarget_chain(base, targets, NB) 52 sk_update(base) 53 let rec: *i64 = sys_mmap(NB * 3 * 8) as *i64 54 read_targets(base, rec) 55 56 var fails: i64 = 0 57 // T1 positions reproduced 58 var maxpe: i64 = 0 59 i = 0 60 while i < NB { var d: i64 = 0; var k: i64 = 0; while k < 3 { let e: i64 = iabs(rec[i*3+k]-targets[i*3+k]); if e>d {d=e} k=k+1 } if d>maxpe {maxpe=d} i=i+1 } 61 if maxpe < 12 { hw("T1 PASS positions REPRODUCED (max joint err "); pn(maxpe); hw(" units on 500-unit bones)\n" as *u8) } 62 else { fails = fails + 1; hw("T1 FAIL max pos err "); pn(maxpe); hw("\n" as *u8) } 63 64 // T2 angles recovered (bones 0..NB-2 have children; leaf free) 65 var maxae: i64 = 0 66 i = 0 67 while i < NB - 1 { 68 let b: *i64 = sk_bone(base, i) 69 let dy2: i64 = iabs(b[4]-oy[i]); let de: i64 = iabs(b[5]-oe[i]) 70 if dy2 > maxae { maxae = dy2 } 71 if de > maxae { maxae = de } 72 hw(" bone"); pn(i); hw(": yaw "); pn(oy[i]); hw("->"); pn(b[4]); hw(" elev "); pn(oe[i]); hw("->"); pn(b[5]); hw("\n" as *u8) 73 i = i + 1 74 } 75 if maxae < 120 { hw("T2 PASS angles RECOVERED (max err "); pn(maxae); hw(" / 25736-per-turn = under 2deg)\n" as *u8) } 76 else { fails = fails + 1; hw("T2 FAIL max angle err "); pn(maxae); hw("\n" as *u8) } 77 78 // T3 a different frame -> a distinct pose (proves it handles a track, not a fixed answer) 79 build_chain(base) 80 sk_pose(base, 0, 0, 3000); sk_pose(base, 1, 2500, 0); sk_pose(base, 2, 0, 2000); sk_pose(base, 3, 1000, 0); sk_pose(base, 4, 0, 0) 81 sk_update(base) 82 let t2: *i64 = sys_mmap(NB * 3 * 8) as *i64 83 read_targets(base, t2) 84 i = 0 85 while i < NB { sk_pose(base, i, 0, 0); i = i + 1 } 86 rt_retarget_chain(base, t2, NB) 87 sk_update(base) 88 let r2: *i64 = sys_mmap(NB * 3 * 8) as *i64 89 read_targets(base, r2) 90 var maxpe2: i64 = 0 91 i = 0 92 while i < NB { var d: i64 = 0; var k: i64 = 0; while k < 3 { let e: i64 = iabs(r2[i*3+k]-t2[i*3+k]); if e>d {d=e} k=k+1 } if d>maxpe2 {maxpe2=d} i=i+1 } 93 var differ: i64 = 0 94 i = 0 95 while i < NB { if iabs(t2[i*3]-targets[i*3])+iabs(t2[i*3+1]-targets[i*3+1])+iabs(t2[i*3+2]-targets[i*3+2]) > 40 { differ = differ + 1 } i = i + 1 } 96 if maxpe2 < 12 { if differ >= 2 { hw("T3 PASS 2nd frame reproduced (err "); pn(maxpe2); hw(") AND distinct from frame 1 ("); pn(differ); hw(" joints moved)\n" as *u8) } else { fails = fails + 1; hw("T3 FAIL frames not distinct\n" as *u8) } } 97 else { fails = fails + 1; hw("T3 FAIL 2nd frame err "); pn(maxpe2); hw("\n" as *u8) } 98 99 if fails == 0 { hw("POSE-RETARGET-GATE 3/3 GREEN -- a pose track drives our rig (R3 retarget inverts the FK)\n" as *u8); sys_exit(0); return 0 } 100 hw("POSE-RETARGET-GATE RED fails="); pn(fails); hw("\n" as *u8) 101 sys_exit(1); return 1 102}