同步KDL工程源码到云仓库

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

@@ -0,0 +1,198 @@
+import { describe, expect, it } from "vitest";
+import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
+import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
+
+const SIMPLE_URDF = `
+<robot name="simple_fk">
+  <link name="base_link"/>
+  <link name="link_1"/>
+  <link name="link_2"/>
+  <link name="tool0"/>
+  <joint name="joint_1" type="revolute">
+    <parent link="base_link"/>
+    <child link="link_1"/>
+    <origin xyz="0 0 0.1" 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="link_2"/>
+    <origin xyz="0 0 0.2" rpy="0 0 0"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="0" upper="0.4" velocity="0.3" acceleration="1.2"/>
+  </joint>
+  <joint name="tool_fixed" type="fixed">
+    <parent link="link_2"/>
+    <child link="tool0"/>
+    <origin xyz="0 0 0.05" rpy="0 0 0"/>
+  </joint>
+</robot>
+`;
+
+async function createRuntimeRobot() {
+  const runtime = createKdlWorkerRuntime();
+  await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
+  const response = await dispatchKdlRpcRequest(runtime, {
+    id: 2,
+    method: "loadRobotFromUrdf",
+    payload: [
+      SIMPLE_URDF,
+      {
+        robotId: "fk",
+        baseLink: "base_link",
+        tipLink: "tool0"
+      }
+    ]
+  });
+  expect(response.ok).toBe(true);
+  return { runtime, handle: response.result as number };
+}
+
+describe("FK and fkAllLinks", () => {
+  it("computes flange and tcp poses for the zero joint state", async () => {
+    const { runtime, handle } = await createRuntimeRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 3,
+      method: "fk",
+      payload: [handle, [0, 0]]
+    });
+
+    expect(response.ok).toBe(true);
+    const result = response.result as {
+      ok: boolean;
+      joints: number[];
+      diagnostics: unknown[];
+      flange: { position: number[]; quaternion: number[] };
+      tcp: { position: number[]; quaternion: number[] };
+    };
+    expect(result.ok).toBe(true);
+    expect(result.joints).toEqual([0, 0]);
+    expect(result.diagnostics).toEqual([]);
+    expect(result.flange.position[0]).toBeCloseTo(0);
+    expect(result.flange.position[1]).toBeCloseTo(0);
+    expect(result.flange.position[2]).toBeCloseTo(0.35);
+    expect(result.flange.quaternion).toEqual([0, 0, 0, 1]);
+    expect(result.tcp.position[0]).toBeCloseTo(0);
+    expect(result.tcp.position[1]).toBeCloseTo(0);
+    expect(result.tcp.position[2]).toBeCloseTo(0.35);
+    expect(result.tcp.quaternion).toEqual([0, 0, 0, 1]);
+  });
+
+  it("applies revolute and prismatic joint motion in chain order", async () => {
+    const { runtime, handle } = await createRuntimeRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 3,
+      method: "fk",
+      payload: [handle, [Math.PI / 2, 0.2]]
+    });
+
+    expect(response.ok).toBe(true);
+    const result = response.result as { flange: { position: number[]; quaternion: number[] } };
+    expect(result.flange.position[0]).toBeCloseTo(0);
+    expect(result.flange.position[1]).toBeCloseTo(0.2);
+    expect(result.flange.position[2]).toBeCloseTo(0.35);
+    expect(result.flange.quaternion[2]).toBeCloseTo(Math.SQRT1_2);
+    expect(result.flange.quaternion[3]).toBeCloseTo(Math.SQRT1_2);
+  });
+
+  it("returns link poses in base-to-tip order", async () => {
+    const { runtime, handle } = await createRuntimeRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 4,
+      method: "fkAllLinks",
+      payload: [handle, [0, 0.1]]
+    });
+
+    expect(response.ok).toBe(true);
+    const result = response.result as {
+      linkPoses: Array<{ link: string; pose: { position: number[] } }>;
+    };
+    expect(result.linkPoses.map((entry) => entry.link)).toEqual(["base_link", "link_1", "link_2", "tool0"]);
+    expect(result.linkPoses[0]?.pose.position).toEqual([0, 0, 0]);
+    expect(result.linkPoses[3]?.pose.position[0]).toBeCloseTo(0.1);
+    expect(result.linkPoses[3]?.pose.position[2]).toBeCloseTo(0.35);
+  });
+
+  it("applies tool offset to tcp without changing flange", async () => {
+    const { runtime, handle } = await createRuntimeRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 5,
+      method: "fk",
+      payload: [
+        handle,
+        [0, 0],
+        {
+          tool: {
+            position: [0, 0, 0.1],
+            quaternion: [0, 0, 0, 1]
+          }
+        }
+      ]
+    });
+
+    expect(response.ok).toBe(true);
+    const result = response.result as {
+      flange: { position: number[] };
+      tcp: { position: number[] };
+    };
+    expect(result.flange.position[2]).toBeCloseTo(0.35);
+    expect(result.tcp.position[2]).toBeCloseTo(0.45);
+  });
+
+  it("writes tcp pose into a reusable Float64Array", async () => {
+    const { runtime, handle } = await createRuntimeRobot();
+    const out = new Float64Array(7);
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 51,
+      method: "fkPose7",
+      payload: [handle, new Float64Array([Math.PI / 2, 0.2]), out]
+    });
+
+    expect(response.ok).toBe(true);
+    expect(response.result).toBe(out);
+    expect(out[0]).toBeCloseTo(0);
+    expect(out[1]).toBeCloseTo(0.2);
+    expect(out[2]).toBeCloseTo(0.35);
+    expect(out[5]).toBeCloseTo(Math.SQRT1_2);
+    expect(out[6]).toBeCloseTo(Math.SQRT1_2);
+  });
+
+  it("returns a structured error for undersized fkPose7 output buffers", async () => {
+    const { runtime, handle } = await createRuntimeRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 52,
+      method: "fkPose7",
+      payload: [handle, [0, 0], new Float64Array(6)]
+    });
+
+    expect(response).toMatchObject({
+      ok: false,
+      error: {
+        code: "KDL_OUTPUT_DIMENSION_MISMATCH"
+      }
+    });
+  });
+
+  it("returns a structured dimension diagnostic", async () => {
+    const { runtime, handle } = await createRuntimeRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 6,
+      method: "fk",
+      payload: [handle, [0]]
+    });
+
+    expect(response).toMatchObject({
+      ok: false,
+      error: {
+        code: "KDL_JOINT_DIMENSION_MISMATCH",
+        diagnostics: [
+          {
+            severity: "error",
+            code: "KDL_JOINT_DIMENSION_MISMATCH"
+          }
+        ]
+      }
+    });
+  });
+});