同步KDL工程源码到云仓库

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

@@ -0,0 +1,167 @@
+import { describe, expect, it } from "vitest";
+import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
+import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
+import type { PathPlanRequest, PathPlanResult, PathValidationResult } from "../../src/kdl/types.js";
+
+const PLANAR_URDF = `
+<robot name="path_planar">
+  <link name="base_link"/>
+  <link name="link_1"/>
+  <link name="tool0"/>
+  <joint name="joint_1" type="revolute">
+    <parent link="base_link"/>
+    <child link="link_1"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-3.141592653589793" upper="3.141592653589793" velocity="2" acceleration="10"/>
+  </joint>
+  <joint name="joint_2" type="prismatic">
+    <parent link="link_1"/>
+    <child link="tool0"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="0" upper="1" velocity="1" acceleration="10"/>
+  </joint>
+</robot>
+`;
+
+async function createRobot() {
+  const runtime = createKdlWorkerRuntime();
+  await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
+  const response = await dispatchKdlRpcRequest(runtime, {
+    id: 2,
+    method: "loadRobotFromUrdf",
+    payload: [
+      PLANAR_URDF,
+      {
+        robotId: "path",
+        baseLink: "base_link",
+        tipLink: "tool0"
+      }
+    ]
+  });
+  expect(response.ok).toBe(true);
+  return { runtime, handle: response.result as number };
+}
+
+function pathRequest(overrides: Partial<PathPlanRequest> = {}): PathPlanRequest {
+  return {
+    startJoints: [0, 0],
+    sampleTime: 0.05,
+    segments: [
+      {
+        id: "move-home",
+        motion: "MOVEJ",
+        target: {
+          id: "joint_goal",
+          joints: [Math.PI / 2, 0.2]
+        },
+        speed: { kind: "joint_abs", velocity: 1, acceleration: 4 },
+        zone: { kind: "fine" },
+        sourceMap: { line: 10, column: 5 }
+      },
+      {
+        id: "line-out",
+        motion: "MOVEL",
+        target: {
+          id: "line_goal",
+          pose: {
+            position: [0, 0.4, 0],
+            quaternion: [0, 0, 0, 1]
+          }
+        },
+        speed: { kind: "linear", velocity: 0.2, acceleration: 1 },
+        zone: { kind: "fine" },
+        sourceMap: { line: 11, column: 5 }
+      }
+    ],
+    ...overrides
+  };
+}
+
+describe("planPath and validatePath", () => {
+  it("plans multiple motion segments and merges points with segment metadata", async () => {
+    const { runtime, handle } = await createRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 3,
+      method: "planPath",
+      payload: [handle, pathRequest()]
+    });
+
+    expect(response.ok).toBe(true);
+    const result = response.result as PathPlanResult;
+    expect(result.ok).toBe(true);
+    expect(result.segments).toHaveLength(2);
+    expect(result.points.length).toBeGreaterThan(result.segments[0]!.points.length);
+    expect(result.points[0]).toMatchObject({
+      index: 0,
+      time: 0,
+      segmentId: "move-home",
+      targetId: "joint_goal",
+      sourceMap: { line: 10 }
+    });
+    expect(result.points.at(-1)).toMatchObject({
+      segmentId: "line-out",
+      targetId: "line_goal",
+      sourceMap: { line: 11 }
+    });
+    expect(result.points.at(-1)?.tcp.position[0]).toBeCloseTo(0, 5);
+    expect(result.points.at(-1)?.tcp.position[1]).toBeCloseTo(0.4, 5);
+    expect(result.duration).toBeCloseTo(result.segments[0]!.duration + result.segments[1]!.duration);
+    for (let index = 1; index < result.points.length; index += 1) {
+      expect(result.points[index]!.time).toBeGreaterThan(result.points[index - 1]!.time);
+      expect(result.points[index]!.index).toBe(index);
+    }
+  });
+
+  it("validates a path and returns per-segment reports", async () => {
+    const { runtime, handle } = await createRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 4,
+      method: "validatePath",
+      payload: [handle, pathRequest()]
+    });
+
+    expect(response.ok).toBe(true);
+    const result = response.result as PathValidationResult;
+    expect(result.ok).toBe(true);
+    expect(result.reachable).toBe(true);
+    expect(result.cycleTime).toBeGreaterThan(0);
+    expect(result.segmentReports.map((report) => report.segmentId)).toEqual(["move-home", "line-out"]);
+    expect(result.segmentReports[0]).toMatchObject({
+      ok: true,
+      motion: "MOVEJ"
+    });
+    expect(result.segmentReports[1]).toMatchObject({
+      ok: true,
+      motion: "MOVEL",
+      maxCartesianError: 0
+    });
+  });
+
+  it("returns KDL_PATH_EMPTY for empty path requests", async () => {
+    const { runtime, handle } = await createRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 5,
+      method: "planPath",
+      payload: [
+        handle,
+        pathRequest({
+          segments: []
+        })
+      ]
+    });
+
+    expect(response.ok).toBe(true);
+    expect(response.result).toMatchObject({
+      ok: false,
+      duration: 0,
+      segments: [],
+      points: [],
+      diagnostics: [
+        {
+          severity: "error",
+          code: "KDL_PATH_EMPTY"
+        }
+      ]
+    });
+  });
+});