Add AC/BC RTCP programs and 3D machine manager

scripts/generate_rtcp_beginner_data.mjs

@@ -0,0 +1,110 @@
+import assert from 'node:assert/strict';
+import { mkdir, readFile, writeFile } from 'node:fs/promises';
+import { createHash } from 'node:crypto';
+import createModule from '../build-wasm/linuxcnc-sim-core.js';
+import { WasmProgramRuntime } from '../web/worker/wasm-program-runtime.mjs';
+import { WasmXyzacKinematics } from '../web/worker/wasm-xyzac-kinematics.mjs';
+import { SimSession, CONTEXT_ID } from '../web/worker/sim-session.mjs';
+import { loadSimulationCase, toWorkerConfiguration } from '../web/workspace/sim-case-loader.mjs';
+import { writeTrtGeometry } from '../web/workspace/trt-geometry.mjs';
+import { MACHINE_DEFINITIONS } from '../web/workspace/machine-definitions.mjs';
+
+const root = new URL('../', import.meta.url), out = new URL('docs/teaching/rtcp-beginners/', root);
+await mkdir(new URL('assets/', out), { recursive: true });
+await mkdir(new URL('verification/', out), { recursive: true });
+const wasm = await readFile(new URL('build-wasm/linuxcnc-sim-core.wasm', root));
+const core = await createModule({ wasmBinary: wasm });
+const limits = Object.fromEntries(['x','y','z','a','b','c'].map((a) => [a,[-100000,100000]]));
+const k = new WasmXyzacKinematics(core, { coordinates: 'XYZBC', limits });
+const interpreter = new WasmProgramRuntime(core);
+interpreter.load('M2\n', 'beginner-reference.ngc');
+const blocks = [
+  'N10 G21 G90 G94 G17',
+  'N20 G10 L2 P1 X40 Y20 Z10',
+  'N30 G54',
+  'N40 M428',
+  'N50 G1 X10 Y0 Z5 B0 C0 F600',
+  'N60 C90',
+  'N70 G91 X10',
+  'N80 G90 X20 Y0 Z5 B30 C90 F300',
+  'N90 X0 Y10',
+  'N100 M429',
+  'N110 M428',
+];
+const rows = [];
+const axes = ['x','y','z','a','b','c','u','v','w'];
+let joints = [0,0,0,0,0], mode = 0;
+const poseOf = (state) => Object.fromEntries(axes.map((a) => [a, state[a] ?? 0]));
+let canonical = poseOf({});
+for (const block of blocks) {
+  const result = interpreter.executeBlock(block);
+  assert.equal(result.status, 'verified', JSON.stringify(result));
+  const code = result.program.control_code;
+  if ([428,429].includes(code)) {
+    mode = code === 428 ? 1 : 0;
+    const selected = k.selectPhysical(mode, joints);
+    assert.equal(selected.status, 'verified');
+    interpreter.synchronizePosition(selected.pose);
+    canonical = { ...selected.pose };
+  } else if (result.motion) {
+    const inverse = k.inverseMode(mode, result.motion.end);
+    assert.equal(inverse.status, 'verified'); joints = inverse.joints;
+    canonical = { ...result.motion.end };
+  }
+  const state = interpreter.snapshot(), origin = state.coordinate_context.offsets[state.coordinate_context.system-1];
+  rows.push({ block, has_motion: Boolean(result.motion), mode, canonical: { ...canonical }, work_xyz: ['x','y','z'].map((a,i)=>canonical[a]-origin[i]), joints: [...joints], origin: origin.slice(0,3), units: state.units, distance_mode: state.distance_mode, feed: state.feed, motion: result.motion });
+}
+const near = (actual, expected) => actual.forEach((v,i) => assert.ok(Math.abs(v-expected[i])<1e-8, `${v} != ${expected[i]}`));
+near(rows[4].joints,[50,20,15,0,0]);
+near(rows[5].joints,[-20,50,15,0,90]);
+near(rows[6].joints,[-20,60,15,0,90]);
+near(rows[7].joints,[-9.820508075688775,60,22.99038105676658,30,90]);
+near(rows[9].joints,rows[8].joints); near(rows[10].joints,rows[8].joints);
+
+function probe(blocks) {
+  const p = new WasmProgramRuntime(core); p.load('M2\n','probe.ngc');
+  let canonical = poseOf({});
+  const rows = blocks.map((block) => {
+    const result=p.executeBlock(block); assert.equal(result.status,'verified',block);
+    const state=p.snapshot();
+    if (result.motion) canonical = { ...result.motion.end };
+    return { block, canonical: { ...canonical }, units:state.units,distance_mode:state.distance_mode,feed:state.feed, coordinates:state.coordinate_context,motion:result.motion };
+  });
+  p.destroy(); return rows;
+}
+const units = probe(['G21 G90 G1 X0 Y0 Z0 F600','G20 G91 X1 F2','G21 G90 X30']);
+near([units[1].canonical.x,units[1].feed],[25.4,50.8]);
+const touch = probe(['G21 G90 G1 X70 Y30 Z20 F600','G10 L20 P1 X0 Y0 Z0','G54 X10']);
+near(touch[1].coordinates.offsets[0].slice(0,3),[70,30,20]); near([touch[2].canonical.x],[80]);
+const switching = probe(['G21 G90 G10 L2 P1 X40 Y20 Z10','G10 L2 P2 X80 Y-10 Z10','G54 G1 X10 Y0 Z5 F600','G55','X10']);
+near([switching[2].canonical.x,switching[3].canonical.x,switching[4].canonical.x],[50,50,90]);
+interpreter.destroy(); k.destroy();
+
+const fetchImpl = async (url) => { try { const text=await readFile(new URL(String(url).replace(/^\//,''),root),'utf8'); return {ok:true,text:async()=>text}; } catch {return {ok:false};} };
+const definition=MACHINE_DEFINITIONS.find((m)=>m.coordinates==='XYZBC');
+const sessionCore=await createModule({wasmBinary:wasm});
+const session=new SimSession(sessionCore,{sessionId:'rtcp-beginner-teaching',planner:'linuxcnc-tp'});let seq=0;
+const send=(kind,payload={})=>{
+ const response=session.accept({schema_version:1,context_id:CONTEXT_ID,session_id:session.sessionId,kind,seq:++seq,operation_id:`lesson-${seq}`,payload});
+ assert.notEqual(response.kind,'ERROR',JSON.stringify(response.payload));return response;
+};
+send('HELLO');send('SESSION_ATTACH');
+const bundle=writeTrtGeometry(await loadSimulationCase({path:definition.entrypoint,fetchImpl}),{x_rot_point:0,y_rot_point:0,z_rot_point:0,x_offset:0,y_offset:0,z_offset:0});
+send('LOAD_WORKSPACE',{root_path:'/workspace',configuration:toWorkerConfiguration(bundle)});
+const program=blocks.slice(0,9).join('\n')+'\nM2\n';
+send('PROGRAM_LOAD',{source:program,path:'beginner-g54-rtcp.ngc'});send('POWER',{active:true});send('HOME',{all:true});
+for(let i=0;i<500&&!session.wasmHomeLimit.read().state.homed;i++)send('SIM_STEP',{cycles:1});
+assert.ok(session.wasmHomeLimit.read().state.homed);send('PROGRAM_RUN');
+let final;const trajectory=[];
+for(let i=0;i<6000;i++){
+ final=send('SIM_STEP',{cycles:5,response:'motion_delta'});
+ const state=session.wasmMotion.read().state;
+ if(i%10===0)trajectory.push({tick:session.tick,pose:state.position,joints:state.joint_commanded});
+ if(final.payload.program?.program_status==='completed'&&state.queue_depth===0)break;
+}
+assert.equal(final.payload.program.program_status,'completed');
+const end=session.wasmMotion.read().state;
+near(end.joint_commanded,rows[8].joints);
+const report={report_id:'rtcp-beginners-gcode-v1',status:'verified',wasm_sha256:createHash('sha256').update(wasm).digest('hex'),assumptions:{coordinates:'XYZBC',rot_point:[0,0,0],offsets:[0,0,0],tool_offset:0,work_origin:[40,20,10],rotation:'Ry(B)*Rz(C)',work_rotation:0},rows,units,touch,switching,executed_program:{source:program,status:final.payload.program.program_status,final_pose:end.position,final_joints:end.joint_commanded,samples:trajectory}};
+await writeFile(new URL('gcode-evidence.json',out),JSON.stringify(report,null,2)+'\n');
+console.log(JSON.stringify({report_id:report.report_id,status:report.status,blocks:rows.length,extra_blocks:units.length+touch.length+switching.length,trajectory_samples:trajectory.length,final:end.joint_commanded}));