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; async function loadNativeModule(): Promise { 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 = ` `; 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); } } }); });