同步KDL工程源码到云仓库

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

@@ -0,0 +1,193 @@
+import { describe, expect, it } from "vitest";
+import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
+import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
+import type { MoveCRequest, TrajectoryResult } from "../../src/kdl/types.js";
+
+const PLANAR_URDF = `
+<robot name="movec_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="4" acceleration="20"/>
+  </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="2" acceleration="20"/>
+  </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: "movec",
+        baseLink: "base_link",
+        tipLink: "tool0"
+      }
+    ]
+  });
+  expect(response.ok).toBe(true);
+  return { runtime, handle: response.result as number };
+}
+
+function request(overrides: Partial<MoveCRequest>): MoveCRequest {
+  return {
+    startJoints: [0, 0.5],
+    via: {
+      id: "via",
+      pose: {
+        position: [0.5, 0.5, 0],
+        quaternion: [0, 0, 0, 1]
+      }
+    },
+    target: {
+      id: "arc_goal",
+      pose: {
+        position: [0, 0.5, 0],
+        quaternion: [0, 0, 0, 1]
+      }
+    },
+    speed: {
+      kind: "linear",
+      velocity: 0.25,
+      acceleration: 1
+    },
+    zone: {
+      kind: "fine"
+    },
+    sampleTime: 0.05,
+    ...overrides
+  };
+}
+
+describe("planMoveC", () => {
+  it("plans a circular TCP arc with circle metadata", async () => {
+    const { runtime, handle } = await createRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 3,
+      method: "planMoveC",
+      payload: [handle, request({})]
+    });
+
+    expect(response.ok).toBe(true);
+    const trajectory = response.result as TrajectoryResult;
+    expect(trajectory.ok).toBe(true);
+    expect(trajectory.motion).toBe("MOVEC");
+    expect(trajectory.points.length).toBeGreaterThan(2);
+    expect(trajectory.points[0]).toMatchObject({
+      index: 0,
+      time: 0,
+      s: 0,
+      motion: "MOVEC",
+      targetId: "arc_goal"
+    });
+    expect(trajectory.points.at(-1)?.s).toBe(1);
+    expect(trajectory.points.at(-1)?.tcp.position[0]).toBeCloseTo(0, 5);
+    expect(trajectory.points.at(-1)?.tcp.position[1]).toBeCloseTo(0.5, 5);
+
+    const circle = trajectory.meta?.circle as {
+      center: number[];
+      radius: number;
+      angle: number;
+      length: number;
+      direction: "cw" | "ccw";
+      maxArcError: number;
+    };
+    expect(circle.center[0]).toBeCloseTo(0.25);
+    expect(circle.center[1]).toBeCloseTo(0.25);
+    expect(circle.radius).toBeCloseTo(Math.SQRT1_2 / 2);
+    expect(circle.angle).toBeCloseTo(Math.PI);
+    expect(circle.length).toBeCloseTo((Math.SQRT1_2 / 2) * Math.PI);
+    expect(circle.direction).toBe("ccw");
+    expect(circle.maxArcError).toBeLessThan(1e-6);
+  });
+
+  it("returns KDL_ARC_DEGENERATE for collinear points", async () => {
+    const { runtime, handle } = await createRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 4,
+      method: "planMoveC",
+      payload: [
+        handle,
+        request({
+          via: {
+            id: "line_mid",
+            pose: {
+              position: [0.25, 0, 0],
+              quaternion: [0, 0, 0, 1]
+            }
+          },
+          target: {
+            id: "line_end",
+            pose: {
+              position: [0.75, 0, 0],
+              quaternion: [0, 0, 0, 1]
+            }
+          }
+        })
+      ]
+    });
+
+    expect(response.ok).toBe(true);
+    expect(response.result).toMatchObject({
+      ok: false,
+      motion: "MOVEC",
+      points: [],
+      diagnostics: [
+        {
+          severity: "error",
+          code: "KDL_ARC_DEGENERATE"
+        }
+      ]
+    });
+  });
+
+  it("reports zone approximation and joint-speed approximation warnings", async () => {
+    const { runtime, handle } = await createRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 5,
+      method: "planMoveC",
+      payload: [
+        handle,
+        request({
+          speed: {
+            kind: "joint_abs",
+            velocity: 0.25,
+            acceleration: 1
+          },
+          zone: {
+            kind: "distance",
+            value: 0.01
+          }
+        })
+      ]
+    });
+
+    const trajectory = response.result as TrajectoryResult;
+    expect(trajectory.ok).toBe(true);
+    expect(trajectory.diagnostics).toContainEqual(
+      expect.objectContaining({
+        severity: "warning",
+        code: "KDL_MOVEC_JOINT_SPEED_APPROX"
+      })
+    );
+    expect(trajectory.diagnostics).toContainEqual(
+      expect.objectContaining({
+        severity: "warning",
+        code: "KDL_ZONE_APPROX_FINE"
+      })
+    );
+  });
+});