同步KDL工程源码到云仓库

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