Upload project files

kdl-wasm/web/tests/runtime/motionQueue.test.ts

@@ -0,0 +1,157 @@
+import { describe, expect, it } from "vitest";
+import type { SemanticProgramIr } from "../../src/grl/ir/index.js";
+import type { PathPlanRequest, PathPlanResult, TrajectoryPoint } from "../../src/kdl/types.js";
+import { KdlRuntimeBridge, MotionQueue, type MotionPlanner } from "../../src/runtime/index.js";
+
+const POSE = {
+  position: [0, 0, 0] as [number, number, number],
+  quaternion: [0, 0, 0, 1] as [number, number, number, number]
+};
+
+function point(index: number, time: number, joints: number[], segmentId: string): TrajectoryPoint {
+  return {
+    index,
+    time,
+    dt: index === 0 ? 0 : time,
+    s: time,
+    sd: 1,
+    sdd: 0,
+    joints,
+    jointVelocity: joints.map(() => 0),
+    jointAcceleration: joints.map(() => 0),
+    flange: POSE,
+    tcp: POSE,
+    motion: "MOVEJ",
+    segmentId,
+    diagnostics: []
+  };
+}
+
+function pathResult(request: PathPlanRequest): PathPlanResult {
+  return {
+    ok: true,
+    duration: 1,
+    segments: [],
+    points: [point(0, 0, request.startJoints, request.segments[0]?.id ?? "p0"), point(1, 1, [1], request.segments[0]?.id ?? "p0")],
+    diagnostics: []
+  };
+}
+
+function program(): SemanticProgramIr {
+  return {
+    moduleName: "Main",
+    symbols: [],
+    semanticChecks: [],
+    diagnostics: [],
+    sourceMap: [
+      { kind: "path_point", id: "p0", pathId: "pick_path", pointId: "p0", sourceMap: { line: 10 } },
+      { kind: "operation_action", id: "pick_op_start_action_0", operationId: "pick_op", sourceMap: { line: 20 } }
+    ],
+    procedures: [
+      {
+        name: "main",
+        instructions: [
+          { kind: "RUN_OPERATION", operationId: "pick_op", sourceMap: { line: 30 } }
+        ]
+      }
+    ],
+    paths: [
+      {
+        pathId: "pick_path",
+        request: {
+          pathId: "pick_path",
+          startJoints: [0],
+          sampleTime: 0.1,
+          segments: [
+            {
+              id: "p0",
+              motion: "MOVEJ",
+              target: { joints: [1] },
+              speed: { kind: "joint_abs", velocity: 1 },
+              zone: { kind: "fine" },
+              sourceMap: { line: 10 }
+            }
+          ]
+        },
+        motions: [
+          {
+            kind: "MOVEJ",
+            id: "p0",
+            pathId: "pick_path",
+            pointId: "p0",
+            target: { joints: [1] },
+            speed: { kind: "joint_abs", velocity: 1 },
+            zone: { kind: "fine" },
+            sourceMap: { line: 10 },
+            source: { brand: { vendor: "ABB", file: "cell.mod", line: 42 } }
+          }
+        ],
+        events: []
+      }
+    ],
+    operations: [
+      {
+        operationId: "pick_op",
+        kind: "handling",
+        pathId: "pick_path",
+        process: {},
+        startActions: [
+          {
+            kind: "ACTION",
+            actionKind: "start_action",
+            operationId: "pick_op",
+            statement: "io.do[1] = true",
+            sourceMap: { line: 20 }
+          }
+        ],
+        endActions: [
+          {
+            kind: "ACTION",
+            actionKind: "end_action",
+            operationId: "pick_op",
+            statement: "io.do[1] = false",
+            sourceMap: { line: 22 }
+          }
+        ]
+      }
+    ],
+    kdlBridge: {
+      motionRequests: [],
+      pathRequests: []
+    }
+  };
+}
+
+describe("motion queue and KDL runtime bridge", () => {
+  it("preserves operation/path/action sources and samples planned paths by virtual time", async () => {
+    const plannerCalls: PathPlanRequest[] = [];
+    const planner: MotionPlanner = {
+      planPath: (_handle, request) => {
+        plannerCalls.push(request);
+        return pathResult(request);
+      }
+    };
+    const queue = new MotionQueue({ startJoints: [0], sampleTime: 0.1, planner, robotHandle: 7 });
+    const ir = program();
+    const bridge = new KdlRuntimeBridge(ir, queue);
+
+    const items = bridge.enqueueInstruction(ir.procedures[0]!.instructions[0]!);
+    expect(items.map((item) => item.kind)).toEqual(["action", "path", "action"]);
+    expect(items[0]).toMatchObject({ source: { operationId: "pick_op", sourceMap: { line: 20 } } });
+    expect(items[1]).toMatchObject({ source: { pathId: "pick_path", operationId: "pick_op" } });
+
+    await queue.planAll();
+    expect(plannerCalls).toHaveLength(1);
+    expect(plannerCalls[0]).toMatchObject({ pathId: "pick_path", startJoints: [0] });
+
+    expect(queue.advance(0.5)).toMatchObject({
+      itemId: items[1]!.id,
+      localTime: 0.5,
+      point: { joints: [1] },
+      source: { pathId: "pick_path", operationId: "pick_op" }
+    });
+    expect(queue.snapshot().items[1]).toMatchObject({
+      planned: { ok: true, duration: 1 }
+    });
+  });
+});