同步KDL工程源码到云仓库

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