同步KDL工程源码到云仓库

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