Upload project files

kdl-wasm/web/src/controller/virtualController.ts

@@ -0,0 +1,193 @@
+import type { ExecutableInstruction, SemanticProgramIr } from "../grl/ir/index.js";
+import { IoImageRuntime, type IoRuntimeOptions } from "../runtime/ioRuntime.js";
+import { KdlRuntimeBridge } from "../runtime/kdlBridge.js";
+import { MotionQueue, type MotionPlanner } from "../runtime/motionQueue.js";
+import type { RobotHandle } from "../kdl/types.js";
+import { IrExecutionRuntime, type IrExecutionRuntimeSnapshot, type RuntimeStepResult } from "./runtime.js";
+import { VirtualControllerStateMachine } from "./stateMachine.js";
+import type { RuntimeTraceEvent } from "./trace.js";
+
+export interface VirtualControllerLoadOptions {
+  entryProcedure?: string;
+  startJoints: number[];
+  sampleTime: number;
+  planner?: MotionPlanner;
+  robotHandle?: RobotHandle;
+  io?: IoRuntimeOptions;
+}
+
+export interface VirtualControllerSnapshot {
+  state: ReturnType<VirtualControllerStateMachine["snapshot"]>;
+  runtime: IrExecutionRuntimeSnapshot;
+  motion?: ReturnType<MotionQueue["snapshot"]>;
+  io: ReturnType<IoImageRuntime["snapshot"]>;
+  bridge?: ReturnType<KdlRuntimeBridge["snapshot"]>;
+}
+
+export class VirtualController {
+  readonly stateMachine = new VirtualControllerStateMachine();
+  private ioValue = new IoImageRuntime();
+  private runtimeValue: IrExecutionRuntime | undefined;
+  private motionQueueValue: MotionQueue | undefined;
+  private bridgeValue: KdlRuntimeBridge | undefined;
+  private loadedOptions: VirtualControllerLoadOptions | undefined;
+
+  get io(): IoImageRuntime {
+    return this.ioValue;
+  }
+
+  get runtime(): IrExecutionRuntime | undefined {
+    return this.runtimeValue;
+  }
+
+  get motionQueue(): MotionQueue | undefined {
+    return this.motionQueueValue;
+  }
+
+  get bridge(): KdlRuntimeBridge | undefined {
+    return this.bridgeValue;
+  }
+
+  load(program: SemanticProgramIr, options: VirtualControllerLoadOptions): RuntimeTraceEvent | undefined {
+    this.loadedOptions = options;
+    this.ioValue = new IoImageRuntime(options.io);
+    this.motionQueueValue = new MotionQueue({
+      startJoints: options.startJoints,
+      sampleTime: options.sampleTime,
+      ...(options.planner ? { planner: options.planner } : {}),
+      ...(options.robotHandle !== undefined ? { robotHandle: options.robotHandle } : {})
+    });
+    this.runtimeValue = new IrExecutionRuntime({
+      ...(options.entryProcedure ? { entryProcedure: options.entryProcedure } : {}),
+      hooks: {
+        onMotion: (instruction, runtime) => {
+          this.bridgeValue?.enqueueInstruction(instruction);
+          runtime.trace("motion", {
+            instruction,
+            data: { queued: true }
+          });
+        },
+        onIo: (instruction, runtime) => {
+          const result = this.ioValue.executeInstruction(instruction);
+          runtime.trace("io", {
+            instruction,
+            data: { result }
+          });
+        },
+        onWait: (instruction, runtime) => {
+          const waitKey = `$wait:${runtime.currentFrame?.procedure ?? "main"}:${runtime.currentFrame?.pc ?? 0}`;
+          if (runtime.getVariable(waitKey) === undefined) {
+            runtime.setVariable(waitKey, runtime.virtualTime, "global");
+          }
+          const elapsed = runtime.virtualTime - Number(runtime.getVariable(waitKey) ?? runtime.virtualTime);
+          const result = this.ioValue.evaluateWait(instruction, elapsed);
+          runtime.trace("wait", {
+            instruction,
+            data: { status: result.status, elapsed }
+          });
+          if (result.status === "satisfied" || result.status === "timeout") {
+            if (result.onTimeout?.kind === "alarm") {
+              runtime.pushAlarm(result.onTimeout.value, result.onTimeout.value, "warning", runtime.locateInstruction(instruction));
+            }
+            return undefined;
+          }
+          if (result.status === "hold-stop") {
+            this.stateMachine.dispatch("hold");
+          }
+          return {
+            status: "blocked",
+            instruction
+          };
+        }
+      }
+    });
+    this.runtimeValue.load(program, options.entryProcedure);
+    this.bridgeValue = new KdlRuntimeBridge(program, this.motionQueueValue);
+    this.stateMachine.dispatch("load");
+    return this.runtimeValue.snapshot().trace.at(-1);
+  }
+
+  run(maxSteps?: number): RuntimeStepResult {
+    const transition = this.stateMachine.dispatch("run");
+    if (!transition.ok) {
+      return blockedTransitionResult(transition.diagnostic);
+    }
+    const result = this.requireRuntime().run(maxSteps);
+    if (result.status === "completed") {
+      this.stateMachine.dispatch("stop");
+    } else if (result.status === "breakpoint") {
+      this.stateMachine.dispatch("pause");
+    }
+    return result;
+  }
+
+  pause(): void {
+    this.stateMachine.dispatch("pause");
+  }
+
+  stop(): void {
+    this.stateMachine.dispatch("stop");
+  }
+
+  reset(): void {
+    const options = this.loadedOptions;
+    this.stateMachine.dispatch("reset");
+    if (!options || !this.runtimeValue || !this.runtimeValue.program) {
+      return;
+    }
+    this.runtimeValue.reset(options.entryProcedure);
+    this.motionQueueValue?.clear();
+  }
+
+  step(): RuntimeStepResult {
+    const transition = this.stateMachine.dispatch("step");
+    if (!transition.ok) {
+      return blockedTransitionResult(transition.diagnostic);
+    }
+    return this.requireRuntime().step();
+  }
+
+  advance(deltaTime: number): void {
+    this.io.advance(deltaTime);
+    this.motionQueueValue?.advance(deltaTime);
+    if (this.runtimeValue) {
+      this.runtimeValue.virtualTime += deltaTime;
+    }
+  }
+
+  snapshot(): VirtualControllerSnapshot {
+    return {
+      state: this.stateMachine.snapshot(),
+      runtime: this.requireRuntime().snapshot(),
+      ...(this.motionQueueValue ? { motion: this.motionQueueValue.snapshot() } : {}),
+      io: this.ioValue.snapshot(),
+      ...(this.bridgeValue ? { bridge: this.bridgeValue.snapshot() } : {})
+    };
+  }
+
+  private requireRuntime(): IrExecutionRuntime {
+    if (!this.runtimeValue) {
+      throw new Error("Controller has not loaded a program");
+    }
+    return this.runtimeValue;
+  }
+}
+
+function blockedTransitionResult(diagnostic: RuntimeStepResult["diagnostic"]): RuntimeStepResult {
+  return {
+    status: "blocked",
+    ...(diagnostic ? { diagnostic } : {})
+  };
+}
+
+export function isMotionInstruction(
+  instruction: ExecutableInstruction
+): instruction is Extract<ExecutableInstruction, { kind: "MOVEJ" | "MOVEL" | "MOVEC" | "RUN_PATH" | "RUN_OPERATION" }> {
+  return (
+    instruction.kind === "MOVEJ" ||
+    instruction.kind === "MOVEL" ||
+    instruction.kind === "MOVEC" ||
+    instruction.kind === "RUN_PATH" ||
+    instruction.kind === "RUN_OPERATION"
+  );
+}