同步KDL工程源码到云仓库

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

@@ -0,0 +1,175 @@
+import { describe, expect, it } from "vitest";
+import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
+import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
+import type { MoveLRequest, TrajectoryResult } from "../../src/kdl/types.js";
+
+const PLANAR_URDF = `
+<robot name="movel_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: "movel",
+        baseLink: "base_link",
+        tipLink: "tool0"
+      }
+    ]
+  });
+  expect(response.ok).toBe(true);
+  return { runtime, handle: response.result as number };
+}
+
+function request(overrides: Partial<MoveLRequest>): MoveLRequest {
+  return {
+    startJoints: [Math.PI / 2, 0.2],
+    target: {
+      id: "line_goal",
+      pose: {
+        position: [0, 0.6, 0],
+        quaternion: [0, 0, 0, 1]
+      }
+    },
+    speed: {
+      kind: "linear",
+      velocity: 0.2,
+      acceleration: 1
+    },
+    zone: {
+      kind: "fine"
+    },
+    sampleTime: 0.05,
+    ...overrides
+  };
+}
+
+describe("planMoveL", () => {
+  it("plans a TCP straight-line trajectory with continuous IK seeds", async () => {
+    const { runtime, handle } = await createRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 3,
+      method: "planMoveL",
+      payload: [handle, request({})]
+    });
+
+    expect(response.ok).toBe(true);
+    const trajectory = response.result as TrajectoryResult;
+    expect(trajectory.ok).toBe(true);
+    expect(trajectory.motion).toBe("MOVEL");
+    expect(trajectory.points.length).toBeGreaterThan(2);
+    expect(trajectory.points[0]).toMatchObject({
+      index: 0,
+      time: 0,
+      s: 0,
+      motion: "MOVEL",
+      targetId: "line_goal"
+    });
+    expect(trajectory.points.at(-1)?.s).toBe(1);
+    expect(trajectory.points.at(-1)?.tcp.position[0]).toBeCloseTo(0, 6);
+    expect(trajectory.points.at(-1)?.tcp.position[1]).toBeCloseTo(0.6, 6);
+    expect(trajectory.meta).toMatchObject({
+      targetId: "line_goal",
+      orientationMode: "fixed"
+    });
+    expect(trajectory.meta?.length as number).toBeCloseTo(0.4);
+
+    for (const point of trajectory.points) {
+      expect(point.tcp.position[0]).toBeCloseTo(0, 5);
+      expect(point.tcp.position[2]).toBeCloseTo(0, 5);
+      expect(point.tcp.position[1]).toBeGreaterThanOrEqual(0.2 - 1e-9);
+      expect(point.tcp.position[1]).toBeLessThanOrEqual(0.6 + 1e-9);
+    }
+  });
+
+  it("returns a failed trajectory when a sampled pose is unreachable", async () => {
+    const { runtime, handle } = await createRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 4,
+      method: "planMoveL",
+      payload: [
+        handle,
+        request({
+          target: {
+            id: "far_goal",
+            pose: {
+              position: [0, 2, 0],
+              quaternion: [0, 0, 0, 1]
+            }
+          }
+        })
+      ]
+    });
+
+    const trajectory = response.result as TrajectoryResult;
+    expect(trajectory.ok).toBe(false);
+    expect(trajectory.motion).toBe("MOVEL");
+    expect(trajectory.diagnostics).toContainEqual(
+      expect.objectContaining({
+        severity: "error",
+        code: "KDL_JOINT_LIMIT"
+      })
+    );
+    expect(trajectory.meta).toMatchObject({
+      targetId: "far_goal"
+    });
+  });
+
+  it("reports zone approximation and joint-speed approximation warnings", async () => {
+    const { runtime, handle } = await createRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 5,
+      method: "planMoveL",
+      payload: [
+        handle,
+        request({
+          speed: {
+            kind: "joint_abs",
+            velocity: 0.2,
+            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_MOVEL_JOINT_SPEED_APPROX"
+      })
+    );
+    expect(trajectory.diagnostics).toContainEqual(
+      expect.objectContaining({
+        severity: "warning",
+        code: "KDL_ZONE_APPROX_FINE"
+      })
+    );
+  });
+});