225 lines
7.3 KiB
TypeScript
225 lines
7.3 KiB
TypeScript
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);
|
|
}
|
|
}
|
|
});
|
|
});
|