KDL_WORK/kdl-wasm/web/tests/kdl/trajectoryUtils.test.ts

import { describe, expect, it } from "vitest";
import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
import type { CycleTimeResult, PathPlanResult, TrajectoryResult } from "../../src/kdl/types.js";

function pose(x: number, y: number, z: number) {
  return {
    position: [x, y, z] as [number, number, number],
    quaternion: [0, 0, 0, 1] as [number, number, number, number]
  };
}

function trajectory(overrides: Partial<TrajectoryResult> = {}): TrajectoryResult {
  return {
    ok: true,
    motion: "MOVEJ",
    duration: 1,
    sampleTime: 0.5,
    events: [],
    diagnostics: [],
    points: [
      {
        index: 0,
        time: 0,
        dt: 0,
        s: 0,
        sd: 0,
        sdd: 0,
        joints: [0, 0],
        jointVelocity: [0, 0],
        jointAcceleration: [0, 0],
        flange: pose(0, 0, 0),
        tcp: pose(0, 0, 0),
        motion: "MOVEJ",
        segmentId: "s1",
        diagnostics: []
      },
      {
        index: 1,
        time: 1,
        dt: 1,
        s: 1,
        sd: 0,
        sdd: 0,
        joints: [1, 2],
        jointVelocity: [0, 0],
        jointAcceleration: [0, 0],
        flange: pose(1, 0, 0),
        tcp: pose(1, 2, 0),
        motion: "MOVEJ",
        segmentId: "s1",
        diagnostics: []
      }
    ],
    ...overrides
  };
}

async function createRuntime() {
  const runtime = createKdlWorkerRuntime();
  await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
  return runtime;
}

describe("cycle-time, resample, and diagnostics utilities", () => {
  it("estimates cycle time for a trajectory and a path plan", async () => {
    const runtime = await createRuntime();
    const singleResponse = await dispatchKdlRpcRequest(runtime, {
      id: 2,
      method: "estimateCycleTime",
      payload: [trajectory()]
    });
    expect(singleResponse.result).toMatchObject({
      ok: true,
      motionTime: 1,
      totalTime: 1,
      segmentTimes: [
        {
          segmentId: "s1",
          motion: "MOVEJ",
          duration: 1
        }
      ],
      diagnostics: []
    });

    const path: PathPlanResult = {
      ok: true,
      duration: 3,
      segments: [
        trajectory(),
        trajectory({
          motion: "MOVEL",
          duration: 2,
          points: trajectory().points.map((point) => ({ ...point, motion: "MOVEL", segmentId: "s2" }))
        })
      ],
      points: [],
      diagnostics: []
    };
    const pathResponse = await dispatchKdlRpcRequest(runtime, {
      id: 3,
      method: "estimateCycleTime",
      payload: [path]
    });
    expect(pathResponse.result).toMatchObject({
      ok: true,
      motionTime: 3,
      totalTime: 3,
      segmentTimes: [
        { segmentId: "s1", motion: "MOVEJ", duration: 1 },
        { segmentId: "s2", motion: "MOVEL", duration: 2 }
      ]
    });
  });

  it("resamples a trajectory with stable time and point ordering", async () => {
    const runtime = await createRuntime();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 4,
      method: "resampleTrajectory",
      payload: [trajectory(), 0.25]
    });

    expect(response.ok).toBe(true);
    const result = response.result as TrajectoryResult;
    expect(result.sampleTime).toBe(0.25);
    expect(result.points.map((point) => point.time)).toEqual([0, 0.25, 0.5, 0.75, 1]);
    expect(result.points.map((point) => point.index)).toEqual([0, 1, 2, 3, 4]);
    expect(result.points[2]?.joints).toEqual([0.5, 1]);
    expect(result.points[2]?.tcp.position).toEqual([0.5, 1, 0]);
    expect(result.diagnostics).toContainEqual(
      expect.objectContaining({
        severity: "info",
        code: "KDL_TRAJECTORY_RESAMPLED"
      })
    );
  });

  it("keeps structured diagnostics for warning and error cases", async () => {
    const runtime = await createRuntime();
    const emptyResponse = await dispatchKdlRpcRequest(runtime, {
      id: 5,
      method: "resampleTrajectory",
      payload: [
        trajectory({
          points: []
        }),
        0.1
      ]
    });
    expect(emptyResponse.result).toMatchObject({
      diagnostics: [
        {
          severity: "warning",
          code: "KDL_RESAMPLE_EMPTY_TRAJECTORY"
        }
      ]
    });

    const invalidResponse = await dispatchKdlRpcRequest(runtime, {
      id: 6,
      method: "resampleTrajectory",
      payload: [trajectory(), 0]
    });
    expect(invalidResponse).toMatchObject({
      ok: false,
      error: {
        code: "KDL_INVALID_SAMPLE_TIME",
        diagnostics: [
          {
            severity: "error",
            code: "KDL_INVALID_SAMPLE_TIME"
          }
        ]
      }
    });
  });
});