同步KDL工程源码到云仓库

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

@@ -0,0 +1,166 @@
+import { describe, expect, it } from "vitest";
+import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
+import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
+import type { IkResult, Pose } from "../../src/kdl/types.js";
+
+const PLANAR_URDF = `
+<robot name="planar_ik">
+  <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"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-3.141592653589793" upper="3.141592653589793" velocity="2.5" acceleration="5"/>
+  </joint>
+  <joint name="joint_2" type="prismatic">
+    <parent link="link_1"/>
+    <child link="tool0"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="0" upper="1" velocity="0.3" acceleration="1.2"/>
+  </joint>
+</robot>
+`;
+
+const UNSUPPORTED_URDF = `
+<robot name="unsupported_ik">
+  <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 1 0"/>
+    <limit lower="-3.14" upper="3.14" velocity="2.5" acceleration="5"/>
+  </joint>
+  <joint name="joint_2" type="revolute">
+    <parent link="link_1"/>
+    <child link="tool0"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-3.14" upper="3.14" velocity="2.5" acceleration="5"/>
+  </joint>
+</robot>
+`;
+
+function pose(x: number, y: number, z = 0): Pose {
+  return {
+    position: [x, y, z],
+    quaternion: [0, 0, 0, 1]
+  };
+}
+
+async function createRobot(urdf = PLANAR_URDF) {
+  const runtime = createKdlWorkerRuntime();
+  await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
+  const response = await dispatchKdlRpcRequest(runtime, {
+    id: 2,
+    method: "loadRobotFromUrdf",
+    payload: [
+      urdf,
+      {
+        robotId: "ik",
+        baseLink: "base_link",
+        tipLink: "tool0"
+      }
+    ]
+  });
+  expect(response.ok).toBe(true);
+  return { runtime, handle: response.result as number };
+}
+
+describe("IK and ikBatch", () => {
+  it("solves a reachable planar target and FK back-substitution is within tolerance", async () => {
+    const { runtime, handle } = await createRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 3,
+      method: "ik",
+      payload: [handle, [0, 0], pose(0, 0.4), { positionTolerance: 1e-9 }]
+    });
+
+    expect(response.ok).toBe(true);
+    const result = response.result as IkResult;
+    expect(result.ok).toBe(true);
+    expect(result.joints?.[0]).toBeCloseTo(Math.PI / 2);
+    expect(result.joints?.[1]).toBeCloseTo(0.4);
+    expect(result.residualPosition).toBeLessThan(1e-9);
+
+    const fk = await dispatchKdlRpcRequest(runtime, {
+      id: 4,
+      method: "fk",
+      payload: [handle, result.joints]
+    });
+    const fkResult = fk.result as { tcp: { position: number[] } };
+    expect(fkResult.tcp.position[0]).toBeCloseTo(0);
+    expect(fkResult.tcp.position[1]).toBeCloseTo(0.4);
+  });
+
+  it("keeps ikBatch results in input order", async () => {
+    const { runtime, handle } = await createRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 5,
+      method: "ikBatch",
+      payload: [
+        handle,
+        [
+          [0, 0],
+          [0, 0]
+        ],
+        [pose(0.2, 0), pose(0, 0.3)],
+        {}
+      ]
+    });
+
+    expect(response.ok).toBe(true);
+    const results = response.result as IkResult[];
+    expect(results).toHaveLength(2);
+    expect(results[0]?.joints?.[0]).toBeCloseTo(0);
+    expect(results[0]?.joints?.[1]).toBeCloseTo(0.2);
+    expect(results[1]?.joints?.[0]).toBeCloseTo(Math.PI / 2);
+    expect(results[1]?.joints?.[1]).toBeCloseTo(0.3);
+  });
+
+  it("returns joint_limit reason when the candidate exceeds limits", async () => {
+    const { runtime, handle } = await createRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 6,
+      method: "ik",
+      payload: [handle, [0, 0], pose(2, 0), {}]
+    });
+
+    expect(response.ok).toBe(true);
+    expect(response.result).toMatchObject({
+      ok: false,
+      reason: "joint_limit",
+      diagnostics: [
+        {
+          severity: "error",
+          code: "KDL_JOINT_LIMIT"
+        }
+      ]
+    });
+  });
+
+  it("returns invalid_model reason for unsupported IK chains", async () => {
+    const { runtime, handle } = await createRobot(UNSUPPORTED_URDF);
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 7,
+      method: "ik",
+      payload: [handle, [0, 0], pose(0.2, 0), {}]
+    });
+
+    expect(response.ok).toBe(true);
+    expect(response.result).toMatchObject({
+      ok: false,
+      reason: "invalid_model",
+      diagnostics: [
+        {
+          severity: "error",
+          code: "KDL_IK_UNSUPPORTED_MODEL"
+        }
+      ]
+    });
+  });
+});