同步KDL工程源码到云仓库

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

@@ -0,0 +1,224 @@
+import { access } from "node:fs/promises";
+import { fileURLToPath } from "node:url";
+import { describe, expect, it } from "vitest";
+import { KDL_C_ABI_EXPORTS, KdlNativeAbi } from "../../src/kdl/nativeAbi.js";
+import type { NativeKdlModule } from "../../src/kdl/nativeModule.js";
+import { loadRobotFromUrdfModel } from "../../src/robot/urdfParser.js";
+
+const BUILD_DIR = new URL("../../../build-wasm/", import.meta.url);
+const WRAPPER_URL = new URL("kdl.js", BUILD_DIR);
+
+type NativeFactory = (options?: {
+  locateFile?: (path: string, prefix: string) => string;
+}) => Promise<NativeKdlModule>;
+
+async function loadNativeModule(): Promise<NativeKdlModule> {
+  await access(fileURLToPath(WRAPPER_URL));
+  const imported = (await import(/* @vite-ignore */ WRAPPER_URL.href)) as {
+    default?: NativeFactory;
+    createKdlModule?: NativeFactory;
+  };
+  const factory = imported.default ?? imported.createKdlModule;
+  if (!factory) {
+    throw new Error("kdl.js did not export createKdlModule");
+  }
+  return factory({
+    locateFile: (path) => fileURLToPath(new URL(path, BUILD_DIR))
+  });
+}
+
+const NATIVE_SOLVER_URDF = `
+<robot name="native_solver">
+  <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="4"/>
+  </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="0.5" acceleration="1"/>
+  </joint>
+</robot>
+`;
+
+function writeFloat64Array(native: NativeKdlModule, values: number[]): number {
+  const bytes = values.length * Float64Array.BYTES_PER_ELEMENT;
+  const ptr = native._malloc?.(bytes);
+  if (!ptr) {
+    throw new Error(`Failed to allocate ${bytes} bytes`);
+  }
+  native.HEAPF64?.set(values, ptr / Float64Array.BYTES_PER_ELEMENT);
+  return ptr;
+}
+
+function readFloat64Array(native: NativeKdlModule, ptr: number, length: number): number[] {
+  return Array.from(native.HEAPF64?.subarray(
+    ptr / Float64Array.BYTES_PER_ELEMENT,
+    ptr / Float64Array.BYTES_PER_ELEMENT + length
+  ) ?? []);
+}
+
+describe("KDL C ABI", () => {
+  it("exports the stable P0 ABI names", async () => {
+    const abi = new KdlNativeAbi(await loadNativeModule());
+
+    expect(() => abi.assertExports()).not.toThrow();
+    expect(KDL_C_ABI_EXPORTS).toEqual([
+      "kdl_init",
+      "kdl_create_robot",
+      "kdl_destroy_robot",
+      "kdl_get_robot_info",
+      "kdl_fk",
+      "kdl_fk_all_links",
+      "kdl_jacobian",
+      "kdl_ik",
+      "kdl_plan_movej",
+      "kdl_plan_movel",
+      "kdl_plan_movec",
+      "kdl_plan_path",
+      "kdl_sample_trap",
+      "kdl_last_error"
+    ]);
+  });
+
+  it("initializes, caches model handles, returns JSON info, and destroys handles", async () => {
+    const abi = new KdlNativeAbi(await loadNativeModule());
+    const model = loadRobotFromUrdfModel(NATIVE_SOLVER_URDF, {
+      robotId: "abi",
+      baseLink: "base_link",
+      tipLink: "tool0"
+    });
+
+    expect(abi.callNumber("kdl_init", ["string"], ["{}"])).toBe(0);
+    const handle = abi.callNumber("kdl_create_robot", ["string"], [JSON.stringify(model)]);
+    expect(handle).toBeGreaterThan(0);
+
+    const info = abi.readJsonCall<{ handle: number; nativeState: string; dof: number }>(
+      "kdl_get_robot_info",
+      ["number"],
+      [handle]
+    );
+    expect(info).toMatchObject({
+      handle,
+      dof: 2,
+      nativeState: "kdl_chain"
+    });
+
+    expect(abi.callNumber("kdl_destroy_robot", ["number"], [handle])).toBe(0);
+  });
+
+  it("constructs a native KDL chain and returns real FK and Jacobian data", async () => {
+    const native = await loadNativeModule();
+    const abi = new KdlNativeAbi(native);
+    const model = loadRobotFromUrdfModel(NATIVE_SOLVER_URDF, {
+      robotId: "native",
+      baseLink: "base_link",
+      tipLink: "tool0"
+    });
+
+    expect(abi.callNumber("kdl_init", ["string"], ["{}"])).toBe(0);
+    const handle = abi.callNumber("kdl_create_robot", ["string"], [JSON.stringify(model)]);
+    expect(handle).toBeGreaterThan(0);
+    expect(abi.readJsonCall("kdl_get_robot_info", ["number"], [handle])).toMatchObject({
+      handle,
+      dof: 2,
+      jointNames: ["joint_1", "joint_2"],
+      nativeState: "kdl_chain"
+    });
+
+    const joints = writeFloat64Array(native, [Math.PI / 2, 0.4]);
+    const pose = native._malloc?.(7 * Float64Array.BYTES_PER_ELEMENT);
+    const jacobian = native._malloc?.(12 * Float64Array.BYTES_PER_ELEMENT);
+    expect(pose).toBeTruthy();
+    expect(jacobian).toBeTruthy();
+
+    try {
+      expect(abi.callNumber("kdl_fk", ["number", "number", "number", "number"], [handle, joints, 2, pose])).toBe(0);
+      const pose7 = readFloat64Array(native, pose!, 7);
+      expect(pose7[0]).toBeCloseTo(0);
+      expect(pose7[1]).toBeCloseTo(0.4);
+      expect(pose7[2]).toBeCloseTo(0);
+      expect(pose7[5]).toBeCloseTo(Math.SQRT1_2);
+      expect(pose7[6]).toBeCloseTo(Math.SQRT1_2);
+
+      expect(abi.callNumber("kdl_jacobian", ["number", "number", "number", "number"], [handle, joints, 2, jacobian])).toBe(0);
+      const jac = readFloat64Array(native, jacobian!, 12);
+      expect(jac[0]).toBeCloseTo(-0.4, 4);
+      expect(jac[1]).toBeCloseTo(0, 4);
+      expect(jac[2]).toBeCloseTo(0, 4);
+      expect(jac[3]).toBeCloseTo(1, 4);
+      expect(jac[10]).toBeCloseTo(1, 4);
+    } finally {
+      native._free?.(joints);
+      if (pose) {
+        native._free?.(pose);
+      }
+      if (jacobian) {
+        native._free?.(jacobian);
+      }
+      abi.callNumber("kdl_destroy_robot", ["number"], [handle]);
+    }
+  });
+
+  it("normalizes C ABI failures through kdl_last_error", async () => {
+    const abi = new KdlNativeAbi(await loadNativeModule());
+
+    expect(abi.callNumber("kdl_init", ["string"], ["{}"])).toBe(0);
+    const returnCode = abi.callNumber("kdl_ik", ["number", "number", "number", "number", "string", "number"], [1, 0, 0, 0, "{}", 0]);
+
+    expect(returnCode).toBe(-1);
+    expect(abi.lastError()).toMatchObject({
+      code: "KDL_NOT_IMPLEMENTED",
+      diagnostics: [
+        {
+          severity: "error",
+          code: "KDL_NOT_IMPLEMENTED"
+        }
+      ]
+    });
+    expect(() => abi.checkReturnCode(returnCode)).toThrowError(
+      expect.objectContaining({
+        code: "KDL_NOT_IMPLEMENTED"
+      })
+    );
+  });
+
+  it("reports JSON output buffer errors without raw strings", async () => {
+    const native = await loadNativeModule();
+    const abi = new KdlNativeAbi(native);
+    const model = loadRobotFromUrdfModel(NATIVE_SOLVER_URDF, {
+      robotId: "abi",
+      baseLink: "base_link",
+      tipLink: "tool0"
+    });
+
+    expect(abi.callNumber("kdl_init", ["string"], ["{}"])).toBe(0);
+    const handle = abi.callNumber("kdl_create_robot", ["string"], [JSON.stringify(model)]);
+    const ptr = native._malloc?.(4);
+    expect(ptr).toBeTruthy();
+
+    try {
+      const returnCode = abi.callNumber("kdl_get_robot_info", ["number", "number", "number"], [handle, ptr, 4]);
+      expect(returnCode).toBe(-1);
+      expect(abi.lastError()).toMatchObject({
+        code: "KDL_BUFFER_TOO_SMALL",
+        diagnostics: [
+          {
+            severity: "error",
+            code: "KDL_BUFFER_TOO_SMALL"
+          }
+        ]
+      });
+    } finally {
+      if (ptr) {
+        native._free?.(ptr);
+      }
+    }
+  });
+});

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

@@ -0,0 +1,178 @@
+import { describe, expect, it } from "vitest";
+import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
+import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
+import type { JacobianResult, PoseTarget, ReachabilityResult } from "../../src/kdl/types.js";
+
+const PLANAR_URDF = `
+<robot name="planar_checks">
+  <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="4"/>
+  </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="0.5" acceleration="1"/>
+  </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: "checks",
+        baseLink: "base_link",
+        tipLink: "tool0"
+      }
+    ]
+  });
+  expect(response.ok).toBe(true);
+  return { runtime, handle: response.result as number };
+}
+
+function poseTarget(id: string, x: number, y: number): PoseTarget {
+  return {
+    id,
+    pose: {
+      position: [x, y, 0],
+      quaternion: [0, 0, 0, 1]
+    }
+  };
+}
+
+describe("Jacobian, singularity, limits, and reachability checks", () => {
+  it("computes a 6xdof Jacobian with expected linear components", async () => {
+    const { runtime, handle } = await createRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 3,
+      method: "jacobian",
+      payload: [handle, [Math.PI / 2, 0.4]]
+    });
+
+    expect(response.ok).toBe(true);
+    const jacobian = response.result as JacobianResult;
+    expect(jacobian.rows).toBe(6);
+    expect(jacobian.cols).toBe(2);
+    expect(jacobian.data).toHaveLength(12);
+    expect(jacobian.data[0]).toBeCloseTo(-0.4, 4);
+    expect(jacobian.data[1]).toBeCloseTo(0, 4);
+    expect(jacobian.data[2]).toBeCloseTo(0, 4);
+    expect(jacobian.data[3]).toBeCloseTo(1, 4);
+  });
+
+  it("reports singularity warning for collapsed planar reach", async () => {
+    const { runtime, handle } = await createRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 4,
+      method: "checkSingularity",
+      payload: [handle, [0, 0]]
+    });
+
+    expect(response.result).toMatchObject({
+      ok: true,
+      nearSingularity: true,
+      diagnostics: [
+        {
+          severity: "warning",
+          code: "KDL_SINGULARITY"
+        }
+      ]
+    });
+  });
+
+  it("checks joint and velocity limits with structured diagnostics", async () => {
+    const { runtime, handle } = await createRobot();
+    const jointResponse = await dispatchKdlRpcRequest(runtime, {
+      id: 5,
+      method: "checkJointLimits",
+      payload: [handle, [0, 2]]
+    });
+    expect(jointResponse.result).toMatchObject({
+      ok: false,
+      diagnostics: [
+        {
+          severity: "error",
+          code: "KDL_JOINT_LIMIT"
+        }
+      ]
+    });
+
+    const velocityResponse = await dispatchKdlRpcRequest(runtime, {
+      id: 6,
+      method: "checkVelocityLimits",
+      payload: [
+        handle,
+        {
+          points: [
+            {
+              jointVelocity: [1, 0.75],
+              jointAcceleration: [1, 1.5]
+            }
+          ]
+        }
+      ]
+    });
+    expect(velocityResponse.result).toMatchObject({
+      ok: false,
+      maxJointVelocityRatio: 1.5,
+      maxJointAccelerationRatio: 1.5,
+      diagnostics: [
+        {
+          severity: "error",
+          code: "KDL_VELOCITY_LIMIT",
+          pointIndex: 0
+        },
+        {
+          severity: "error",
+          code: "KDL_ACCEL_LIMIT",
+          pointIndex: 0
+        }
+      ]
+    });
+  });
+
+  it("checks reachability and preserves batch order", async () => {
+    const { runtime, handle } = await createRobot();
+    const reachable = await dispatchKdlRpcRequest(runtime, {
+      id: 7,
+      method: "checkReachability",
+      payload: [handle, poseTarget("ok", 0, 0.3), { positionTolerance: 1e-9 }]
+    });
+    expect(reachable.result).toMatchObject({
+      ok: true,
+      reachable: true,
+      targetId: "ok",
+      joints: [Math.PI / 2, 0.3]
+    });
+
+    const batch = await dispatchKdlRpcRequest(runtime, {
+      id: 8,
+      method: "checkReachabilityBatch",
+      payload: [handle, [poseTarget("a", 0.2, 0), poseTarget("b", 2, 0)], {}]
+    });
+    const results = batch.result as ReachabilityResult[];
+    expect(results.map((result) => result.targetId)).toEqual(["a", "b"]);
+    expect(results[0]?.reachable).toBe(true);
+    expect(results[1]).toMatchObject({
+      reachable: false,
+      diagnostics: [
+        {
+          severity: "error",
+          code: "KDL_JOINT_LIMIT"
+        }
+      ]
+    });
+  });
+});

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"
+          }
+        ]
+      }
+    });
+  });
+});

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"
+        }
+      ]
+    });
+  });
+});

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

@@ -0,0 +1,167 @@
+import { describe, expect, it } from "vitest";
+import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
+import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
+import type { PathPlanRequest, PathPlanResult, PathValidationResult } from "../../src/kdl/types.js";
+
+const PLANAR_URDF = `
+<robot name="path_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: "path",
+        baseLink: "base_link",
+        tipLink: "tool0"
+      }
+    ]
+  });
+  expect(response.ok).toBe(true);
+  return { runtime, handle: response.result as number };
+}
+
+function pathRequest(overrides: Partial<PathPlanRequest> = {}): PathPlanRequest {
+  return {
+    startJoints: [0, 0],
+    sampleTime: 0.05,
+    segments: [
+      {
+        id: "move-home",
+        motion: "MOVEJ",
+        target: {
+          id: "joint_goal",
+          joints: [Math.PI / 2, 0.2]
+        },
+        speed: { kind: "joint_abs", velocity: 1, acceleration: 4 },
+        zone: { kind: "fine" },
+        sourceMap: { line: 10, column: 5 }
+      },
+      {
+        id: "line-out",
+        motion: "MOVEL",
+        target: {
+          id: "line_goal",
+          pose: {
+            position: [0, 0.4, 0],
+            quaternion: [0, 0, 0, 1]
+          }
+        },
+        speed: { kind: "linear", velocity: 0.2, acceleration: 1 },
+        zone: { kind: "fine" },
+        sourceMap: { line: 11, column: 5 }
+      }
+    ],
+    ...overrides
+  };
+}
+
+describe("planPath and validatePath", () => {
+  it("plans multiple motion segments and merges points with segment metadata", async () => {
+    const { runtime, handle } = await createRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 3,
+      method: "planPath",
+      payload: [handle, pathRequest()]
+    });
+
+    expect(response.ok).toBe(true);
+    const result = response.result as PathPlanResult;
+    expect(result.ok).toBe(true);
+    expect(result.segments).toHaveLength(2);
+    expect(result.points.length).toBeGreaterThan(result.segments[0]!.points.length);
+    expect(result.points[0]).toMatchObject({
+      index: 0,
+      time: 0,
+      segmentId: "move-home",
+      targetId: "joint_goal",
+      sourceMap: { line: 10 }
+    });
+    expect(result.points.at(-1)).toMatchObject({
+      segmentId: "line-out",
+      targetId: "line_goal",
+      sourceMap: { line: 11 }
+    });
+    expect(result.points.at(-1)?.tcp.position[0]).toBeCloseTo(0, 5);
+    expect(result.points.at(-1)?.tcp.position[1]).toBeCloseTo(0.4, 5);
+    expect(result.duration).toBeCloseTo(result.segments[0]!.duration + result.segments[1]!.duration);
+    for (let index = 1; index < result.points.length; index += 1) {
+      expect(result.points[index]!.time).toBeGreaterThan(result.points[index - 1]!.time);
+      expect(result.points[index]!.index).toBe(index);
+    }
+  });
+
+  it("validates a path and returns per-segment reports", async () => {
+    const { runtime, handle } = await createRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 4,
+      method: "validatePath",
+      payload: [handle, pathRequest()]
+    });
+
+    expect(response.ok).toBe(true);
+    const result = response.result as PathValidationResult;
+    expect(result.ok).toBe(true);
+    expect(result.reachable).toBe(true);
+    expect(result.cycleTime).toBeGreaterThan(0);
+    expect(result.segmentReports.map((report) => report.segmentId)).toEqual(["move-home", "line-out"]);
+    expect(result.segmentReports[0]).toMatchObject({
+      ok: true,
+      motion: "MOVEJ"
+    });
+    expect(result.segmentReports[1]).toMatchObject({
+      ok: true,
+      motion: "MOVEL",
+      maxCartesianError: 0
+    });
+  });
+
+  it("returns KDL_PATH_EMPTY for empty path requests", async () => {
+    const { runtime, handle } = await createRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 5,
+      method: "planPath",
+      payload: [
+        handle,
+        pathRequest({
+          segments: []
+        })
+      ]
+    });
+
+    expect(response.ok).toBe(true);
+    expect(response.result).toMatchObject({
+      ok: false,
+      duration: 0,
+      segments: [],
+      points: [],
+      diagnostics: [
+        {
+          severity: "error",
+          code: "KDL_PATH_EMPTY"
+        }
+      ]
+    });
+  });
+});

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

@@ -0,0 +1,23 @@
+import { describe, expect, it } from "vitest";
+import { runPerformanceBaseline } from "../../src/kdl/performanceBaseline.js";
+
+describe("KDL performance baseline", () => {
+  it("records TypedArray and batch baseline metrics", async () => {
+    const result = await runPerformanceBaseline();
+    const metrics = Object.fromEntries(result.metrics.map((metric) => [metric.name, metric]));
+
+    expect(result.ok).toBe(true);
+    expect(result.diagnostics).toEqual([]);
+    expect(metrics.robot_init_6_axis?.totalMs).toBeLessThanOrEqual(1_000);
+    expect(metrics.fk_pose7_typed_array?.averageMs).toBeLessThanOrEqual(1);
+    expect(metrics.ik_planar_average?.averageMs).toBeLessThanOrEqual(10);
+    expect(metrics.reachability_batch_1000).toMatchObject({
+      points: 1_000,
+      ok: true
+    });
+    expect(metrics.trajectory_10s_4ms).toMatchObject({
+      ok: true
+    });
+    expect(metrics.trajectory_10s_4ms?.points).toBeGreaterThanOrEqual(2_500);
+  });
+});

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"
+      })
+    );
+  });
+});

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

@@ -0,0 +1,192 @@
+import { describe, expect, it } from "vitest";
+import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
+import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
+import type { MoveJRequest, PoseTarget, TrajectoryResult } from "../../src/kdl/types.js";
+
+const PLANAR_URDF = `
+<robot name="movej_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="1" acceleration="2"/>
+  </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="0.5" acceleration="1"/>
+  </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: "movej",
+        baseLink: "base_link",
+        tipLink: "tool0"
+      }
+    ]
+  });
+  expect(response.ok).toBe(true);
+  return { runtime, handle: response.result as number };
+}
+
+function baseRequest(overrides: Partial<MoveJRequest>): MoveJRequest {
+  return {
+    startJoints: [0, 0],
+    target: {
+      id: "joint_goal",
+      joints: [0.5, 0.25]
+    },
+    speed: {
+      kind: "joint_abs",
+      velocity: 0.5,
+      acceleration: 1
+    },
+    zone: {
+      kind: "fine"
+    },
+    sampleTime: 0.1,
+    ...overrides
+  };
+}
+
+describe("planMoveJ", () => {
+  it("plans a synchronized joint trajectory to a joint target", async () => {
+    const { runtime, handle } = await createRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 3,
+      method: "planMoveJ",
+      payload: [handle, baseRequest({})]
+    });
+
+    expect(response.ok).toBe(true);
+    const trajectory = response.result as TrajectoryResult;
+    expect(trajectory.ok).toBe(true);
+    expect(trajectory.motion).toBe("MOVEJ");
+    expect(trajectory.points.length).toBeGreaterThan(2);
+    expect(trajectory.points[0]).toMatchObject({
+      index: 0,
+      time: 0,
+      s: 0,
+      joints: [0, 0],
+      motion: "MOVEJ",
+      targetId: "joint_goal"
+    });
+    expect(trajectory.points.at(-1)?.s).toBe(1);
+    expect(trajectory.points.at(-1)?.joints[0]).toBeCloseTo(0.5);
+    expect(trajectory.points.at(-1)?.joints[1]).toBeCloseTo(0.25);
+    expect(trajectory.points.at(-1)?.jointVelocity[0]).toBeCloseTo(0);
+    expect(trajectory.points.at(-1)?.tcp.position[0]).toBeCloseTo(0.25 * Math.cos(0.5));
+    expect(trajectory.points.at(-1)?.tcp.position[1]).toBeCloseTo(0.25 * Math.sin(0.5));
+    expect(trajectory.meta).toMatchObject({
+      targetType: "joint",
+      qStart: [0, 0],
+      qEnd: [0.5, 0.25]
+    });
+  });
+
+  it("uses IK for pose targets and warns when zone is approximated as fine", async () => {
+    const { runtime, handle } = await createRobot();
+    const target: PoseTarget = {
+      id: "pose_goal",
+      pose: {
+        position: [0, 0.3, 0],
+        quaternion: [0, 0, 0, 1]
+      }
+    };
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 4,
+      method: "planMoveJ",
+      payload: [
+        handle,
+        baseRequest({
+          target,
+          zone: { kind: "distance", value: 0.01 }
+        })
+      ]
+    });
+
+    const trajectory = response.result as TrajectoryResult;
+    expect(trajectory.ok).toBe(true);
+    expect(trajectory.points.at(-1)?.joints[0]).toBeCloseTo(Math.PI / 2);
+    expect(trajectory.points.at(-1)?.joints[1]).toBeCloseTo(0.3);
+    expect(trajectory.diagnostics).toContainEqual(
+      expect.objectContaining({
+        severity: "warning",
+        code: "KDL_ZONE_APPROX_FINE"
+      })
+    );
+    expect(trajectory.meta).toMatchObject({
+      targetType: "pose"
+    });
+  });
+
+  it("returns a failed trajectory result for endpoint joint limit violations", async () => {
+    const { runtime, handle } = await createRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 5,
+      method: "planMoveJ",
+      payload: [
+        handle,
+        baseRequest({
+          target: {
+            id: "bad_goal",
+            joints: [0, 2]
+          }
+        })
+      ]
+    });
+
+    expect(response.ok).toBe(true);
+    expect(response.result).toMatchObject({
+      ok: false,
+      motion: "MOVEJ",
+      points: [],
+      diagnostics: [
+        {
+          severity: "error",
+          code: "KDL_JOINT_LIMIT"
+        }
+      ]
+    });
+  });
+
+  it("keeps velocity and acceleration within joint limits", async () => {
+    const { runtime, handle } = await createRobot();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 6,
+      method: "planMoveJ",
+      payload: [
+        handle,
+        baseRequest({
+          speed: { kind: "joint_percent", value: 1 },
+          target: {
+            id: "limit_goal",
+            joints: [1, 0.5]
+          }
+        })
+      ]
+    });
+
+    const trajectory = response.result as TrajectoryResult;
+    expect(trajectory.ok).toBe(true);
+    for (const point of trajectory.points) {
+      expect(Math.abs(point.jointVelocity[0]!)).toBeLessThanOrEqual(1 + 1e-9);
+      expect(Math.abs(point.jointVelocity[1]!)).toBeLessThanOrEqual(0.5 + 1e-9);
+      expect(Math.abs(point.jointAcceleration[0]!)).toBeLessThanOrEqual(2 + 1e-9);
+      expect(Math.abs(point.jointAcceleration[1]!)).toBeLessThanOrEqual(1 + 1e-9);
+    }
+  });
+});

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"
+      })
+    );
+  });
+});

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"
+          }
+        ]
+      }
+    });
+  });
+});

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

@@ -0,0 +1,206 @@
+import { describe, expect, it } from "vitest";
+import { KdlWorkerClient, type KdlWorkerLike } from "../../src/kdl/kdlClient.js";
+import type { NativeKdlModule } from "../../src/kdl/nativeModule.js";
+import { KdlStructuredError, type KdlRpcRequest, type KdlRpcResponse } from "../../src/kdl/rpc.js";
+import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
+import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
+
+class FakeWorker implements KdlWorkerLike {
+  readonly sent: Array<KdlRpcRequest<unknown[]>> = [];
+  terminated = false;
+  private messageListeners = new Set<(event: MessageEvent<KdlRpcResponse>) => void>();
+  private errorListeners = new Set<(event: ErrorEvent) => void>();
+
+  postMessage(message: KdlRpcRequest<unknown[]>): void {
+    this.sent.push(message);
+  }
+
+  terminate(): void {
+    this.terminated = true;
+  }
+
+  addEventListener(type: "message", listener: (event: MessageEvent<KdlRpcResponse>) => void): void;
+  addEventListener(type: "error", listener: (event: ErrorEvent) => void): void;
+  addEventListener(type: "message" | "error", listener: unknown): void {
+    if (type === "message") {
+      this.messageListeners.add(listener as (event: MessageEvent<KdlRpcResponse>) => void);
+      return;
+    }
+    this.errorListeners.add(listener as (event: ErrorEvent) => void);
+  }
+
+  removeEventListener(type: "message", listener: (event: MessageEvent<KdlRpcResponse>) => void): void;
+  removeEventListener(type: "error", listener: (event: ErrorEvent) => void): void;
+  removeEventListener(type: "message" | "error", listener: unknown): void {
+    if (type === "message") {
+      this.messageListeners.delete(listener as (event: MessageEvent<KdlRpcResponse>) => void);
+      return;
+    }
+    this.errorListeners.delete(listener as (event: ErrorEvent) => void);
+  }
+
+  emitResponse(response: KdlRpcResponse): void {
+    const event = { data: response } as MessageEvent<KdlRpcResponse>;
+    for (const listener of this.messageListeners) {
+      listener(event);
+    }
+  }
+
+  emitError(message: string): void {
+    const event = { message, error: new Error(message) } as ErrorEvent;
+    for (const listener of this.errorListeners) {
+      listener(event);
+    }
+  }
+}
+
+describe("KDL Worker RPC", () => {
+  it("loads the native WASM module during init when a loader is configured", async () => {
+    const calls: unknown[][] = [];
+    const native: NativeKdlModule = {
+      ccall: (...args) => {
+        calls.push(args);
+        return 0;
+      }
+    };
+    const runtime = createKdlWorkerRuntime(async () => native);
+
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 1,
+      method: "init",
+      payload: [{ wasmBuild: "native-test" }]
+    });
+
+    expect(response.ok).toBe(true);
+    expect(response.result).toMatchObject({ wasmBuild: "native-test" });
+    expect(calls).toEqual([["kdl_init", "number", ["string"], ['{"wasmBuild":"native-test"}']]]);
+  });
+
+  it("normalizes native WASM initialization failures", async () => {
+    const runtime = createKdlWorkerRuntime(async () => {
+      throw new Error("cannot load kdl.js");
+    });
+
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 11,
+      method: "init",
+      payload: [{}]
+    });
+
+    expect(response.ok).toBe(false);
+    expect(response.error).toMatchObject({
+      code: "KDL_WASM_INIT_FAILED",
+      diagnostics: [
+        {
+          severity: "error",
+          code: "KDL_WASM_INIT_FAILED"
+        }
+      ]
+    });
+  });
+
+  it("dispatches init and dispose through structured responses", async () => {
+    const runtime = createKdlWorkerRuntime();
+    const initResponse = await dispatchKdlRpcRequest(runtime, {
+      id: 1,
+      method: "init",
+      payload: [{ wasmBuild: "test", useThreads: true }]
+    });
+
+    expect(initResponse.ok).toBe(true);
+    expect(initResponse.result).toMatchObject({
+      version: "0.1.0",
+      wasmBuild: "test",
+      supportsThreads: true
+    });
+
+    const disposeResponse = await dispatchKdlRpcRequest(runtime, {
+      id: 2,
+      method: "dispose",
+      payload: []
+    });
+
+    expect(disposeResponse).toMatchObject({ id: 2, ok: true });
+  });
+
+  it("returns a structured error when an implemented method is called before init", async () => {
+    const response = await dispatchKdlRpcRequest(createKdlWorkerRuntime(), {
+      id: 7,
+      method: "fk",
+      payload: [1, new Float64Array([0])]
+    });
+
+    expect(response.ok).toBe(false);
+    expect(response.error).toMatchObject({
+      code: "KDL_NOT_INITIALIZED",
+      diagnostics: [
+        {
+          severity: "error",
+          code: "KDL_NOT_INITIALIZED"
+        }
+      ]
+    });
+  });
+
+  it("uses unique request ids and resolves responses by id", async () => {
+    const workers: FakeWorker[] = [];
+    const client = new KdlWorkerClient(() => {
+      const worker = new FakeWorker();
+      workers.push(worker);
+      return worker;
+    });
+
+    const first = client.call("init", { wasmBuild: "a" });
+    const second = client.call("dispose");
+
+    expect(workers).toHaveLength(1);
+    expect(workers[0]?.sent.map((request) => request.id)).toEqual([1, 2]);
+
+    workers[0]?.emitResponse({ id: 2, ok: true });
+    workers[0]?.emitResponse({
+      id: 1,
+      ok: true,
+      result: {
+        version: "0.1.0",
+        wasmBuild: "a",
+        supportsThreads: false,
+        supportsWasmFs: false
+      }
+    });
+
+    await expect(second).resolves.toBeUndefined();
+    await expect(first).resolves.toMatchObject({ wasmBuild: "a" });
+  });
+
+  it("rejects pending requests on worker failure and can create a fresh worker", async () => {
+    const workers: FakeWorker[] = [];
+    const client = new KdlWorkerClient(() => {
+      const worker = new FakeWorker();
+      workers.push(worker);
+      return worker;
+    });
+
+    const pending = client.init();
+    workers[0]?.emitError("boom");
+
+    await expect(pending).rejects.toMatchObject({
+      code: "KDL_WORKER_CRASHED"
+    });
+    expect(workers[0]?.terminated).toBe(true);
+
+    const restarted = client.init({ wasmBuild: "restart" });
+    expect(workers).toHaveLength(2);
+    workers[1]?.emitResponse({
+      id: 2,
+      ok: true,
+      result: {
+        version: "0.1.0",
+        wasmBuild: "restart",
+        supportsThreads: false,
+        supportsWasmFs: false
+      }
+    });
+
+    await expect(restarted).resolves.toMatchObject({ wasmBuild: "restart" });
+  });
+});

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

@@ -0,0 +1,179 @@
+import { describe, expect, it } from "vitest";
+import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
+import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
+import type { CycleTimeResult, PathPlanResult, TrajectoryResult } from "../../src/kdl/types.js";
+
+function pose(x: number, y: number, z: number) {
+  return {
+    position: [x, y, z] as [number, number, number],
+    quaternion: [0, 0, 0, 1] as [number, number, number, number]
+  };
+}
+
+function trajectory(overrides: Partial<TrajectoryResult> = {}): TrajectoryResult {
+  return {
+    ok: true,
+    motion: "MOVEJ",
+    duration: 1,
+    sampleTime: 0.5,
+    events: [],
+    diagnostics: [],
+    points: [
+      {
+        index: 0,
+        time: 0,
+        dt: 0,
+        s: 0,
+        sd: 0,
+        sdd: 0,
+        joints: [0, 0],
+        jointVelocity: [0, 0],
+        jointAcceleration: [0, 0],
+        flange: pose(0, 0, 0),
+        tcp: pose(0, 0, 0),
+        motion: "MOVEJ",
+        segmentId: "s1",
+        diagnostics: []
+      },
+      {
+        index: 1,
+        time: 1,
+        dt: 1,
+        s: 1,
+        sd: 0,
+        sdd: 0,
+        joints: [1, 2],
+        jointVelocity: [0, 0],
+        jointAcceleration: [0, 0],
+        flange: pose(1, 0, 0),
+        tcp: pose(1, 2, 0),
+        motion: "MOVEJ",
+        segmentId: "s1",
+        diagnostics: []
+      }
+    ],
+    ...overrides
+  };
+}
+
+async function createRuntime() {
+  const runtime = createKdlWorkerRuntime();
+  await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
+  return runtime;
+}
+
+describe("cycle-time, resample, and diagnostics utilities", () => {
+  it("estimates cycle time for a trajectory and a path plan", async () => {
+    const runtime = await createRuntime();
+    const singleResponse = await dispatchKdlRpcRequest(runtime, {
+      id: 2,
+      method: "estimateCycleTime",
+      payload: [trajectory()]
+    });
+    expect(singleResponse.result).toMatchObject({
+      ok: true,
+      motionTime: 1,
+      totalTime: 1,
+      segmentTimes: [
+        {
+          segmentId: "s1",
+          motion: "MOVEJ",
+          duration: 1
+        }
+      ],
+      diagnostics: []
+    });
+
+    const path: PathPlanResult = {
+      ok: true,
+      duration: 3,
+      segments: [
+        trajectory(),
+        trajectory({
+          motion: "MOVEL",
+          duration: 2,
+          points: trajectory().points.map((point) => ({ ...point, motion: "MOVEL", segmentId: "s2" }))
+        })
+      ],
+      points: [],
+      diagnostics: []
+    };
+    const pathResponse = await dispatchKdlRpcRequest(runtime, {
+      id: 3,
+      method: "estimateCycleTime",
+      payload: [path]
+    });
+    expect(pathResponse.result).toMatchObject({
+      ok: true,
+      motionTime: 3,
+      totalTime: 3,
+      segmentTimes: [
+        { segmentId: "s1", motion: "MOVEJ", duration: 1 },
+        { segmentId: "s2", motion: "MOVEL", duration: 2 }
+      ]
+    });
+  });
+
+  it("resamples a trajectory with stable time and point ordering", async () => {
+    const runtime = await createRuntime();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 4,
+      method: "resampleTrajectory",
+      payload: [trajectory(), 0.25]
+    });
+
+    expect(response.ok).toBe(true);
+    const result = response.result as TrajectoryResult;
+    expect(result.sampleTime).toBe(0.25);
+    expect(result.points.map((point) => point.time)).toEqual([0, 0.25, 0.5, 0.75, 1]);
+    expect(result.points.map((point) => point.index)).toEqual([0, 1, 2, 3, 4]);
+    expect(result.points[2]?.joints).toEqual([0.5, 1]);
+    expect(result.points[2]?.tcp.position).toEqual([0.5, 1, 0]);
+    expect(result.diagnostics).toContainEqual(
+      expect.objectContaining({
+        severity: "info",
+        code: "KDL_TRAJECTORY_RESAMPLED"
+      })
+    );
+  });
+
+  it("keeps structured diagnostics for warning and error cases", async () => {
+    const runtime = await createRuntime();
+    const emptyResponse = await dispatchKdlRpcRequest(runtime, {
+      id: 5,
+      method: "resampleTrajectory",
+      payload: [
+        trajectory({
+          points: []
+        }),
+        0.1
+      ]
+    });
+    expect(emptyResponse.result).toMatchObject({
+      diagnostics: [
+        {
+          severity: "warning",
+          code: "KDL_RESAMPLE_EMPTY_TRAJECTORY"
+        }
+      ]
+    });
+
+    const invalidResponse = await dispatchKdlRpcRequest(runtime, {
+      id: 6,
+      method: "resampleTrajectory",
+      payload: [trajectory(), 0]
+    });
+    expect(invalidResponse).toMatchObject({
+      ok: false,
+      error: {
+        code: "KDL_INVALID_SAMPLE_TIME",
+        diagnostics: [
+          {
+            severity: "error",
+            code: "KDL_INVALID_SAMPLE_TIME"
+          }
+        ]
+      }
+    });
+  });
+});

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

@@ -0,0 +1,138 @@
+import { describe, expect, it } from "vitest";
+import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
+import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
+import type { TrapProfileResult, TrapSample } from "../../src/kdl/types.js";
+
+async function createRuntime() {
+  const runtime = createKdlWorkerRuntime();
+  await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
+  return runtime;
+}
+
+function expectMonotonic(samples: TrapSample[]) {
+  for (let index = 1; index < samples.length; index += 1) {
+    expect(samples[index]!.time).toBeGreaterThan(samples[index - 1]!.time);
+    expect(samples[index]!.s).toBeGreaterThanOrEqual(samples[index - 1]!.s);
+  }
+}
+
+describe("trapezoid velocity profile API", () => {
+  it("creates a trapezoid profile when the path can reach max velocity", async () => {
+    const runtime = await createRuntime();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 2,
+      method: "makeTrapProfile",
+      payload: [
+        2,
+        {
+          maxVelocity: 1,
+          maxAcceleration: 1,
+          sampleTime: 0.25
+        }
+      ]
+    });
+
+    expect(response.ok).toBe(true);
+    const profile = response.result as TrapProfileResult;
+    expect(profile).toMatchObject({
+      ok: true,
+      type: "trapezoid",
+      length: 2,
+      duration: 3,
+      tAccel: 1,
+      tConst: 1,
+      tDecel: 1,
+      vPeak: 1,
+      diagnostics: []
+    });
+    expect(profile.samples[0]).toMatchObject({ index: 0, time: 0, s: 0 });
+    expect(profile.samples.at(-1)).toMatchObject({ time: 3, s: 1 });
+    expect(profile.samples.find((sample) => sample.time === 1)?.s).toBeCloseTo(0.25);
+    expect(profile.samples.find((sample) => sample.time === 1)?.sd).toBeCloseTo(0.5);
+    expectMonotonic(profile.samples);
+  });
+
+  it("falls back to a triangle profile for short paths", async () => {
+    const runtime = await createRuntime();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 3,
+      method: "makeTrapProfile",
+      payload: [
+        0.5,
+        {
+          maxVelocity: 2,
+          maxAcceleration: 1,
+          sampleTime: 0.1
+        }
+      ]
+    });
+
+    expect(response.ok).toBe(true);
+    const profile = response.result as TrapProfileResult;
+    expect(profile.type).toBe("triangle");
+    expect(profile.tConst).toBe(0);
+    expect(profile.vPeak).toBeCloseTo(Math.sqrt(0.5));
+    expect(profile.duration).toBeCloseTo(2 * Math.sqrt(0.5));
+    expect(profile.samples[0]?.s).toBe(0);
+    expect(profile.samples.at(-1)?.s).toBe(1);
+    expect(profile.diagnostics).toMatchObject([
+      {
+        severity: "info",
+        code: "KDL_TRAP_TRIANGLE_PROFILE"
+      }
+    ]);
+    expectMonotonic(profile.samples);
+  });
+
+  it("returns samples from sampleTrapProfile with strict endpoint samples", async () => {
+    const runtime = await createRuntime();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 4,
+      method: "sampleTrapProfile",
+      payload: [
+        1,
+        {
+          maxVelocity: 1,
+          maxAcceleration: 2,
+          sampleTime: 0.2
+        }
+      ]
+    });
+
+    expect(response.ok).toBe(true);
+    const samples = response.result as TrapSample[];
+    expect(samples[0]).toMatchObject({ index: 0, time: 0, s: 0 });
+    expect(samples.at(-1)?.s).toBe(1);
+    expect(samples.at(-1)?.time).toBeCloseTo(1.5);
+    expectMonotonic(samples);
+  });
+
+  it("returns a structured diagnostic for invalid trap profile inputs", async () => {
+    const runtime = await createRuntime();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 5,
+      method: "makeTrapProfile",
+      payload: [
+        1,
+        {
+          maxVelocity: 0,
+          maxAcceleration: 1,
+          sampleTime: 0.01
+        }
+      ]
+    });
+
+    expect(response).toMatchObject({
+      ok: false,
+      error: {
+        code: "KDL_INVALID_TRAP_PROFILE",
+        diagnostics: [
+          {
+            severity: "error",
+            code: "KDL_INVALID_TRAP_PROFILE"
+          }
+        ]
+      }
+    });
+  });
+});

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

@@ -0,0 +1,182 @@
+import { describe, expect, it } from "vitest";
+import { KdlStructuredError } from "../../src/kdl/rpc.js";
+import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
+import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
+import { loadRobotFromUrdfModel } from "../../src/robot/urdfParser.js";
+
+const SIMPLE_URDF = `
+<robot name="simple6">
+  <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.14" upper="3.14" 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 1.57"/>
+    <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>
+`;
+
+describe("URDF to NormalizedRobotModel", () => {
+  it("parses links, joints, origins, axes, limits, stable active joint names, and source hash", () => {
+    const model = loadRobotFromUrdfModel(SIMPLE_URDF, {
+      robotId: "r1",
+      baseLink: "base_link",
+      tipLink: "tool0"
+    });
+
+    expect(model).toMatchObject({
+      robotId: "r1",
+      name: "simple6",
+      baseLink: "base_link",
+      tipLink: "tool0",
+      activeJointNames: ["joint_1", "joint_2"],
+      source: { type: "urdf" }
+    });
+    expect(model.source.urdfHash).toHaveLength(64);
+    expect(model.links.map((link) => link.name)).toEqual(["base_link", "link_1", "link_2", "tool0"]);
+    expect(model.joints[0]).toMatchObject({
+      name: "joint_1",
+      type: "revolute",
+      parent: "base_link",
+      child: "link_1",
+      origin: { xyz: [0, 0, 0.1], rpy: [0, 0, 0] },
+      axis: [0, 0, 1]
+    });
+    expect(model.limits).toEqual([
+      { name: "joint_1", lower: -3.14, upper: 3.14, velocity: 2.5, acceleration: 5 },
+      { name: "joint_2", lower: 0, upper: 0.4, velocity: 0.3, acceleration: 1.2 }
+    ]);
+  });
+
+  it("applies joint order and limit overrides", () => {
+    const model = loadRobotFromUrdfModel(SIMPLE_URDF, {
+      robotId: "r1",
+      baseLink: "base_link",
+      tipLink: "tool0",
+      jointOrder: ["joint_2", "joint_1"],
+      overrideLimits: [{ name: "joint_2", velocity: 0.2 }]
+    });
+
+    expect(model.activeJointNames).toEqual(["joint_2", "joint_1"]);
+    expect(model.limits[0]).toMatchObject({ name: "joint_2", velocity: 0.2 });
+  });
+
+  it("returns structured diagnostics for disconnected base and tip links", () => {
+    let thrown: unknown;
+    try {
+      loadRobotFromUrdfModel(SIMPLE_URDF, {
+        robotId: "r1",
+        baseLink: "tool0",
+        tipLink: "base_link"
+      });
+    } catch (error) {
+      thrown = error;
+    }
+
+    expect(thrown).toBeInstanceOf(KdlStructuredError);
+    expect(thrown).toMatchObject({
+      code: "KDL_INVALID_MODEL",
+      diagnostics: [
+        {
+          severity: "error",
+          code: "KDL_INVALID_MODEL"
+        }
+      ]
+    });
+  });
+
+  it("rejects unsupported joint types", () => {
+    const urdf = SIMPLE_URDF.replace('type="prismatic"', 'type="floating"');
+    let thrown: unknown;
+    try {
+      loadRobotFromUrdfModel(urdf, {
+        robotId: "r1",
+        baseLink: "base_link",
+        tipLink: "tool0"
+      });
+    } catch (error) {
+      thrown = error;
+    }
+
+    expect(thrown).toBeInstanceOf(KdlStructuredError);
+    expect(thrown).toMatchObject({
+      code: "KDL_INVALID_MODEL",
+      message: expect.stringContaining("Unsupported joint type")
+    });
+  });
+
+  it("supports RobotHandle lifecycle through the worker runtime", async () => {
+    const runtime = createKdlWorkerRuntime();
+    await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
+    const createResponse = await dispatchKdlRpcRequest(runtime, {
+      id: 2,
+      method: "loadRobotFromUrdf",
+      payload: [
+        SIMPLE_URDF,
+        {
+          robotId: "r1",
+          baseLink: "base_link",
+          tipLink: "tool0"
+        }
+      ]
+    });
+
+    expect(createResponse).toMatchObject({ ok: true, result: 1 });
+
+    const infoResponse = await dispatchKdlRpcRequest(runtime, {
+      id: 3,
+      method: "getRobotInfo",
+      payload: [1]
+    });
+    expect(infoResponse.result).toMatchObject({
+      handle: 1,
+      robotId: "r1",
+      name: "simple6",
+      dof: 2,
+      jointNames: ["joint_1", "joint_2"]
+    });
+
+    const limitsResponse = await dispatchKdlRpcRequest(runtime, {
+      id: 4,
+      method: "getJointLimits",
+      payload: [1]
+    });
+    expect(limitsResponse.result).toEqual([
+      { name: "joint_1", lower: -3.14, upper: 3.14, velocity: 2.5, acceleration: 5 },
+      { name: "joint_2", lower: 0, upper: 0.4, velocity: 0.3, acceleration: 1.2 }
+    ]);
+
+    const destroyResponse = await dispatchKdlRpcRequest(runtime, {
+      id: 5,
+      method: "destroyRobot",
+      payload: [1]
+    });
+    expect(destroyResponse.ok).toBe(true);
+
+    const afterDestroy = await dispatchKdlRpcRequest(runtime, {
+      id: 6,
+      method: "getRobotInfo",
+      payload: [1]
+    });
+    expect(afterDestroy).toMatchObject({
+      ok: false,
+      error: { code: "KDL_INVALID_HANDLE" }
+    });
+  });
+});