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

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 }
    });
  });
});