同步KDL工程源码到云仓库
This commit is contained in:
wangdequan
224
kdl-wasm/web/tests/kdl/cAbi.test.ts
Normal file
224
kdl-wasm/web/tests/kdl/cAbi.test.ts
Normal file
@@ -0,0 +1,224 @@
|
||||
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);
|
||||
}
|
||||
}
|
||||
});
|
||||
});
|
||||
Reference in New Issue
Block a user