同步KDL工程源码到云仓库

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

@@ -0,0 +1,152 @@
+import { describe, expect, it } from "vitest";
+import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
+import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
+import type { Pose, PoseTarget } from "../../src/kdl/types.js";
+
+async function createRuntime() {
+  const runtime = createKdlWorkerRuntime();
+  await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
+  return runtime;
+}
+
+function pose(x: number, y: number, z: number): Pose {
+  return {
+    position: [x, y, z],
+    quaternion: [0, 0, 0, 1]
+  };
+}
+
+describe("pose transform and offset API", () => {
+  it("normalizes pose inputs from rpy and quaternion forms", async () => {
+    const runtime = await createRuntime();
+    const rpyResponse = await dispatchKdlRpcRequest(runtime, {
+      id: 2,
+      method: "normalizePose",
+      payload: [{ xyz: [1, 2, 3], rpy: [0, 0, Math.PI / 2] }]
+    });
+    const quatResponse = await dispatchKdlRpcRequest(runtime, {
+      id: 3,
+      method: "normalizePose",
+      payload: [{ xyz: [0, 0, 0], quat: [0, 0, 0, 2] }]
+    });
+
+    expect(rpyResponse.ok).toBe(true);
+    expect((rpyResponse.result as Pose).position).toEqual([1, 2, 3]);
+    expect((rpyResponse.result as Pose).quaternion[2]).toBeCloseTo(Math.SQRT1_2);
+    expect((rpyResponse.result as Pose).quaternion[3]).toBeCloseTo(Math.SQRT1_2);
+    expect(quatResponse.result).toMatchObject({
+      position: [0, 0, 0],
+      quaternion: [0, 0, 0, 1]
+    });
+  });
+
+  it("composes poses and computes an inverse pose", async () => {
+    const runtime = await createRuntime();
+    const composeResponse = await dispatchKdlRpcRequest(runtime, {
+      id: 4,
+      method: "composePose",
+      payload: [pose(1, 0, 0), pose(0, 2, 0)]
+    });
+    expect(composeResponse.result).toMatchObject({
+      position: [1, 2, 0],
+      quaternion: [0, 0, 0, 1]
+    });
+
+    const inverseResponse = await dispatchKdlRpcRequest(runtime, {
+      id: 5,
+      method: "inversePose",
+      payload: [pose(1, 2, 3)]
+    });
+    expect(inverseResponse.result).toMatchObject({
+      position: [-1, -2, -3],
+      quaternion: [0, 0, 0, 1]
+    });
+
+    const identityResponse = await dispatchKdlRpcRequest(runtime, {
+      id: 6,
+      method: "composePose",
+      payload: [pose(1, 2, 3), inverseResponse.result]
+    });
+    expect((identityResponse.result as Pose).position[0]).toBeCloseTo(0);
+    expect((identityResponse.result as Pose).position[1]).toBeCloseTo(0);
+    expect((identityResponse.result as Pose).position[2]).toBeCloseTo(0);
+  });
+
+  it("applies frame, target, and tool using the same order as FK", async () => {
+    const runtime = await createRuntime();
+    const target: PoseTarget = {
+      id: "pick",
+      pose: pose(0.5, 0.1, 0.2)
+    };
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 7,
+      method: "applyToolAndFrame",
+      payload: [target, pose(0, 0, 0.18), pose(0.8, 0, 0.2)]
+    });
+
+    const result = response.result as Pose;
+    expect(result.position[0]).toBeCloseTo(1.3);
+    expect(result.position[1]).toBeCloseTo(0.1);
+    expect(result.position[2]).toBeCloseTo(0.58);
+    expect(result.quaternion).toEqual([0, 0, 0, 1]);
+  });
+
+  it("applies offset in frame/world by left composition and tool by right composition", async () => {
+    const runtime = await createRuntime();
+    const target: PoseTarget = {
+      id: "pick",
+      pose: {
+        position: [1, 2, 3],
+        quaternion: [0, 0, Math.SQRT1_2, Math.SQRT1_2]
+      },
+      frame: pose(10, 0, 0)
+    };
+
+    const frameOffset = await dispatchKdlRpcRequest(runtime, {
+      id: 8,
+      method: "applyOffset",
+      payload: [target, { mode: "frame", xyz: [0.1, 0, 0] }]
+    });
+    const worldOffset = await dispatchKdlRpcRequest(runtime, {
+      id: 9,
+      method: "applyOffset",
+      payload: [target, { mode: "world", xyz: [0, 0.2, 0] }]
+    });
+    const toolOffset = await dispatchKdlRpcRequest(runtime, {
+      id: 10,
+      method: "applyOffset",
+      payload: [target, { mode: "tool", xyz: [0.1, 0, 0] }]
+    });
+
+    expect((frameOffset.result as PoseTarget).id).toBe("pick");
+    expect((frameOffset.result as PoseTarget).frame).toEqual(target.frame);
+    expect((frameOffset.result as PoseTarget).pose.position[0]).toBeCloseTo(1.1);
+    expect((frameOffset.result as PoseTarget).pose.position[1]).toBeCloseTo(2);
+    expect((worldOffset.result as PoseTarget).pose.position[0]).toBeCloseTo(1);
+    expect((worldOffset.result as PoseTarget).pose.position[1]).toBeCloseTo(2.2);
+    expect((toolOffset.result as PoseTarget).pose.position[0]).toBeCloseTo(1);
+    expect((toolOffset.result as PoseTarget).pose.position[1]).toBeCloseTo(2.1);
+  });
+
+  it("returns structured diagnostics for invalid pose inputs", async () => {
+    const runtime = await createRuntime();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 11,
+      method: "normalizePose",
+      payload: [{ xyz: [1, 2], rpy: [0, 0, 0] }]
+    });
+
+    expect(response).toMatchObject({
+      ok: false,
+      error: {
+        code: "KDL_INVALID_POSE",
+        diagnostics: [
+          {
+            severity: "error",
+            code: "KDL_INVALID_POSE"
+          }
+        ]
+      }
+    });
+  });
+});