wangdequan/KDL_WORK
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

接入KDL native IK与链路采样

Browse Source
This commit is contained in:
wangdequan
2026-06-27 09:19:57 -04:00
parent ef5f14aa2a
commit 2817cba164
7 changed files with 504 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

132
kdl-wasm/web/tests/kdl/cAbi.test.ts
View File

@@ -113,7 +113,7 @@ describe("KDL C ABI", () => {
expect(abi.callNumber("kdl_destroy_robot", ["number"], [handle])).toBe(0);
});
it("constructs a native KDL chain and returns real FK and Jacobian data", async () => {
it("constructs a native KDL chain and returns real FK, fkAllLinks, Jacobian, and IK data", async () => {
const native = await loadNativeModule();
const abi = new KdlNativeAbi(native);
const model = loadRobotFromUrdfModel(NATIVE_SOLVER_URDF, {
@@ -135,8 +135,11 @@ describe("KDL C ABI", () => {
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);
const ikSeed = writeFloat64Array(native, [1.4, 0.3]);
const ikOut = native._malloc?.(2 * Float64Array.BYTES_PER_ELEMENT);
expect(pose).toBeTruthy();
expect(jacobian).toBeTruthy();
expect(ikOut).toBeTruthy();
try {
expect(abi.callNumber("kdl_fk", ["number", "number", "number", "number"], [handle, joints, 2, pose])).toBe(0);
@@ -147,6 +150,15 @@ describe("KDL C ABI", () => {
expect(pose7[5]).toBeCloseTo(Math.SQRT1_2);
expect(pose7[6]).toBeCloseTo(Math.SQRT1_2);
const links = abi.readJsonCall<{
ok: boolean;
linkPoses: Array<{ link: string; pose: { position: [number, number, number] } }>;
}>("kdl_fk_all_links", ["number", "number", "number"], [handle, joints, 2]);
expect(links.ok).toBe(true);
expect(links.linkPoses.map((linkPose) => linkPose.link)).toEqual(["base_link", "link_1", "tool0"]);
expect(links.linkPoses[2]?.pose.position[0]).toBeCloseTo(0);
expect(links.linkPoses[2]?.pose.position[1]).toBeCloseTo(0.4);
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);
@@ -154,39 +166,133 @@ describe("KDL C ABI", () => {
expect(jac[2]).toBeCloseTo(0, 4);
expect(jac[3]).toBeCloseTo(1, 4);
expect(jac[10]).toBeCloseTo(1, 4);
expect(
abi.callNumber(
"kdl_ik",
["number", "number", "number", "number", "string", "number"],
[handle, ikSeed, 2, pose, "{}", ikOut]
)
).toBe(0);
const ikJoints = readFloat64Array(native, ikOut!, 2);
expect(ikJoints[0]).toBeCloseTo(Math.PI / 2, 4);
expect(ikJoints[1]).toBeCloseTo(0.4, 4);
} finally {
native._free?.(joints);
native._free?.(ikSeed);
if (pose) {
native._free?.(pose);
}
if (jacobian) {
native._free?.(jacobian);
}
if (ikOut) {
native._free?.(ikOut);
}
abi.callNumber("kdl_destroy_robot", ["number"], [handle]);
}
});
it("normalizes C ABI failures through kdl_last_error", async () => {
const native = await loadNativeModule();
const abi = new KdlNativeAbi(native);
const seed = writeFloat64Array(native, [0, 0]);
const target = writeFloat64Array(native, [0, 0, 0, 0, 0, 0, 1]);
const out = native._malloc?.(2 * Float64Array.BYTES_PER_ELEMENT);
try {
expect(abi.callNumber("kdl_init", ["string"], ["{}"])).toBe(0);
const returnCode = abi.callNumber(
"kdl_ik",
["number", "number", "number", "number", "string", "number"],
[404, seed, 2, target, "{}", out]
);
expect(returnCode).toBe(-1);
expect(abi.lastError()).toMatchObject({
code: "KDL_INVALID_HANDLE",
diagnostics: [
{
severity: "error",
code: "KDL_INVALID_HANDLE"
}
]
});
expect(() => abi.checkReturnCode(returnCode)).toThrowError(
expect.objectContaining({
code: "KDL_INVALID_HANDLE"
})
);
} finally {
native._free?.(seed);
native._free?.(target);
if (out) {
native._free?.(out);
}
}
});
it("returns native trapezoid samples through kdl_sample_trap", 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"
}
const profile = abi.readJsonCall<{
ok: boolean;
type: string;
duration: number;
samples: Array<{ index: number; time: number; s: number; sd: number; sdd: number }>;
diagnostics: Array<{ code: string }>;
}>(
"kdl_sample_trap",
["number", "string"],
[
2,
JSON.stringify({
maxVelocity: 1,
maxAcceleration: 1,
sampleTime: 0.25
})
]
);
expect(profile).toMatchObject({
ok: true,
type: "trapezoid",
duration: 3,
diagnostics: []
});
expect(() => abi.checkReturnCode(returnCode)).toThrowError(
expect(profile.samples[0]).toMatchObject({ index: 0, time: 0, s: 0 });
expect(profile.samples.find((sample) => sample.time === 1)?.s).toBeCloseTo(0.25);
expect(profile.samples.find((sample) => sample.time === 1)?.sd).toBeCloseTo(0.5);
expect(profile.samples.at(-1)).toMatchObject({ s: 1 });
const triangle = abi.readJsonCall<{
type: string;
diagnostics: Array<{ code: string }>;
}>(
"kdl_sample_trap",
["number", "string"],
[
0.5,
JSON.stringify({
maxVelocity: 2,
maxAcceleration: 1,
sampleTime: 0.1
})
]
);
expect(triangle.type).toBe("triangle");
expect(triangle.diagnostics).toContainEqual(
expect.objectContaining({
code: "KDL_NOT_IMPLEMENTED"
code: "KDL_TRAP_TRIANGLE_PROFILE"
})
);
const returnCode = abi.callNumber("kdl_sample_trap", ["number", "string", "number", "number"], [1, "{}", 0, 0]);
expect(returnCode).toBe(-1);
expect(abi.lastError()).toMatchObject({
code: "KDL_INVALID_TRAP_PROFILE"
});
});
it("reports JSON output buffer errors without raw strings", async () => {