同步KDL工程源码到云仓库

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

@@ -0,0 +1,179 @@
+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"
+          }
+        ]
+      }
+    });
+  });
+});