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

44
working/03-推进台账.md
View File

@@ -1185,3 +1185,47 @@
1. 若继续深化 KDL native 主线，优先接入 native `fkAllLinks` JSON 输出和 native IK。
2. native IK 稳定后，再逐步把 `planMoveL/planMoveC/planPath` 从 TypeScript baseline 切到 native solver。
### 轮次 025
日期：2026-06-27
关联任务：`KW-003`、`KW-004`、`KW-007`、`KW-013`
本轮目标：闭合轮次 024 遗留的 native `fkAllLinks` JSON 输出、native IK 和 native sampleTrap C ABI 基线。
做了什么：
1. C ABI 增加 native `kdl_fk_all_links` JSON 序列化，输出 `{ ok, linkPoses, diagnostics }`，并过滤 KDL 内部 origin 辅助段。
2. CMake 接入 Orocos KDL `ChainIkSolverPos_LMA` 源文件。
3. `RobotRecord` 持有 native IK solver，随 KDL Chain 生命周期稳定构造。
4. 实现 native `kdl_ik`，从 pose7 归一化四元数构造 KDL Frame，并输出 joint buffer。
5. 实现 native `kdl_sample_trap`，输出完整 trapezoid/triangle profile JSON，覆盖 zero length、triangle fallback 和非法输入诊断。
6. 扩展 `cAbi.test.ts`，覆盖 native FK、fkAllLinks、Jacobian、IK、sampleTrap 和结构化失败路径。
7. 更新 `README.md`、任务矩阵和验收证据，修正 native IK/fkAllLinks 的旧待办表述。
改了哪些文件：
1. `/home/meswork/kdl_work/kdl-wasm/CMakeLists.txt`
2. `/home/meswork/kdl_work/kdl-wasm/bindings/kdl_c_api.cpp`
3. `/home/meswork/kdl_work/kdl-wasm/web/tests/kdl/cAbi.test.ts`
4. `/home/meswork/kdl_work/working/README.md`
5. `/home/meswork/kdl_work/working/03-推进台账.md`
6. `/home/meswork/kdl_work/working/04-任务矩阵.md`
7. `/home/meswork/kdl_work/working/05-验收证据.md`
验证了什么：
1. `cmake --build kdl-wasm/build-wasm -j16` 通过。
2. `npm run test -- kdl cAbi trapProfile` 通过，27 个测试文件共 109 个测试通过。
3. `npm run typecheck` 通过。
4. `npm test` 通过，27 个测试文件共 109 个测试通过。
问题和风险：
1. Worker runtime 当前仍由 TypeScript registry 管理高层机器人句柄；native C ABI 已覆盖 FK、fkAllLinks、Jacobian、IK 和 sampleTrap，但尚未成为 Worker 默认计算路径。
2. native `planMoveJ/planMoveL/planMoveC/planPath` 仍保留稳定导出边界，高层轨迹规划继续由 TypeScript runtime 基线承载。
3. C ABI 轻量 JSON 解析仍只覆盖当前 `NormalizedRobotModel` 和 trap options 必需字段，后续 schema 扩展需要同步增强。
下一步：
1. 若继续深化 KDL native 主线，优先设计 Worker 中 TypeScript handle 与 native RobotHandle 的映射关系。
2. 映射稳定后，再逐步把 `planMoveJ/planMoveL/planMoveC/planPath` 从 TypeScript runtime 切到 native solver 或 native-assisted solver。

4
working/04-任务矩阵.md
View File

@@ -72,7 +72,7 @@
| KW-002.1 | URDF XML 解析 | Done | link/joint/origin/axis/limit 可读取。 |
| KW-002.2 | 连通性和单位检查 | Done | base/tip 不连通有诊断。 |
| KW-002.3 | 生成 `NormalizedRobotModel` | Done | activeJointNames 和 limits 稳定。 |
| KW-002.4 | WASM 创建 KDL Chain | Done | C ABI 根据 `NormalizedRobotModel` 构造 native KDL Chain，FK/Jacobian golden 通过。 |
| KW-002.4 | WASM 创建 KDL Chain | Done | C ABI 根据 `NormalizedRobotModel` 构造 native KDL Chain，FK/fkAllLinks/Jacobian/IK golden 通过。 |
| KW-002.5 | RobotHandle 生命周期 | Done | create/getInfo/getLimits/destroy 通过。 |
### KW-003 到 KW-013：KDL P0 计算接口
@@ -96,7 +96,7 @@
| KW-011.1 | `planPath` | Done | 多段轨迹合并正确。 |
| KW-011.2 | `validatePath` | Done | 段报告和诊断正确。 |
| KW-012.1 | `estimateCycleTime/resampleTrajectory` | Done | 节拍和重采样正确。 |
| KW-013.1 | C ABI / Embind 稳定导出 | Done | TypeScript 可包装全部 P0 API。 |
| KW-013.1 | C ABI / Embind 稳定导出 | Done | 稳定导出完整，native FK/fkAllLinks/Jacobian/IK/sampleTrap 已有 C ABI 测试；高层轨迹规划由 TypeScript runtime 包装承载。 |
| KW-013.2 | TypedArray 和批量性能 | Done | 性能指标有记录。 |
## 5. GRL 子任务

31
working/05-验收证据.md
View File

@@ -109,7 +109,7 @@ emcmake/emcc: /home/meswork/emsdk/upstream/emscripten
3. 产物存在：`kdl-wasm/build-wasm/kdl.js`、`kdl-wasm/build-wasm/kdl.wasm`、`kdl-wasm/build-wasm/kdl.d.ts`。
4. `npm run test -- kdl rpc` 通过，2 个测试文件共 11 个测试通过。
5. Node 动态加载 `kdl.js` 并调用 `kdl_init`，输出 `{"rc":0,"hasCwrap":true}`。
6. 当前 C ABI 在 `KW-013` 已补稳定导出、RobotHandle 生命周期和结构化错误边界；轮次 024 已进一步接入 native KDL Chain、FK 和 Jacobian，native IK 和轨迹计算仍由 TypeScript runtime 基线承载。
6. 当前 C ABI 在 `KW-013` 已补稳定导出、RobotHandle 生命周期和结构化错误边界；轮次 024/025 已进一步接入 native KDL Chain、FK、fkAllLinks、Jacobian、IK 和 sampleTrap，高层轨迹规划仍由 TypeScript runtime 基线承载。
是否通过：通过。
@@ -148,7 +148,7 @@ Vitest 3.2.6
4. `NormalizedRobotModel` 生成 `activeJointNames`、`limits`、`source.urdfHash`。
5. base/tip 不连通、unsupported joint type 返回 `KDL_INVALID_MODEL` 结构化诊断。
6. Worker runtime 支持 `loadRobotFromUrdf/createRobotFromModel/getRobotInfo/getJointLimits/destroyRobot` 的 RobotHandle 生命周期。
7. Worker runtime 当前仍由 TypeScript registry 管理 RobotHandle；C ABI 已可根据同一 `NormalizedRobotModel` 构造 native KDL Chain 并执行 FK/Jacobian。
7. Worker runtime 当前仍由 TypeScript registry 管理 RobotHandle；C ABI 已可根据同一 `NormalizedRobotModel` 构造 native KDL Chain 并执行 FK、fkAllLinks、Jacobian、IK 和 sampleTrap。
是否通过：通过。
@@ -187,7 +187,7 @@ Vitest 3.2.6
4. `fk` 返回 `ok/flange/tcp/joints/diagnostics`。
5. `fkAllLinks` 返回 base-to-tip link pose 顺序。
6. 覆盖零位 FK、revolute + prismatic 关节、tool TCP 偏移、link 顺序和关节维度结构化诊断。
7. Worker `fk/fkAllLinks` 当前为 TypeScript 标准模型数值基线；C ABI 已补 native `kdl_fk` golden 测试，`fkAllLinks` native JSON 序列化仍待后续接入。
7. Worker `fk/fkAllLinks` 当前为 TypeScript 标准模型数值基线；C ABI 已补 native `kdl_fk` pose7 golden 测试和 `kdl_fk_all_links` JSON link pose 顺序测试。
是否通过：通过。
@@ -224,7 +224,7 @@ Vitest 3.2.6
2. `npm run test -- kdl ik` 通过，8 个测试文件共 34 个测试通过。
3. `ik` 返回 `ok/joints/iterations/residualPosition/residualOrientation/reason/diagnostics`。
4. 覆盖可达目标 IK、FK 回代误差、`ikBatch` 顺序保持、关节限位失败、unsupported model 的 `invalid_model` reason。
5. 当前 IK 为 TypeScript 基线几何求解器，支持 single-prismatic 和 Rz+Px 链；native 通用 6 轴迭代 IK 与 Orocos KDL solver 接入后继续扩展。
5. Worker IK 当前为 TypeScript 基线几何求解器，支持 single-prismatic 和 Rz+Px 链；C ABI 已补 native Orocos KDL `ChainIkSolverPos_LMA` IK 回代测试，后续可继续把 Worker IK 路由切到 native solver。
是否通过：通过。
@@ -611,20 +611,23 @@ Emscripten emcc 6.0.0
1. `npm run typecheck` 通过。
2. `cmake --build kdl-wasm/build-wasm -j16` 通过。
3. `npm test -- kdl-wasm/web/tests/kdl/cAbi.test.ts` 通过，1 个测试文件共 5 个测试通过。
4. `npm run test -- kdl fk jacobian checks c-abi` 通过，27 个测试文件共 108 个测试通过。
5. `npm test` 通过，27 个测试文件共 108 个测试通过。
3. `npm test -- kdl-wasm/web/tests/kdl/cAbi.test.ts` 通过，1 个测试文件共 6 个测试通过。
4. `npm run test -- kdl cAbi trapProfile` 通过，27 个测试文件共 109 个测试通过。
5. `npm test` 通过，27 个测试文件共 109 个测试通过。
6. C ABI 稳定导出 `kdl_init/create_robot/destroy_robot/get_robot_info/fk/fk_all_links/jacobian/ik/plan_movej/plan_movel/plan_movec/plan_path/sample_trap/last_error`。
7. C++ ABI 现支持 `kdl_create_robot/kdl_destroy_robot/kdl_get_robot_info` 的 handle 生命周期和 JSON 输出。
8. WASM wrapper 链接 Orocos KDL 最小源码集，C ABI 可从 `NormalizedRobotModel` 构造 native KDL Chain。
9. `kdl_fk` 通过 `ChainFkSolverPos_recursive` 输出 pose7，测试覆盖 Rz+Px 链 `[pi/2, 0.4]` 的位置和四元数 golden。
10. `kdl_jacobian` 通过 `ChainJntToJacSolver` 输出 6 x dof 行主序矩阵，测试覆盖线速度和角速度分量。
11. 修复 native solver 生命周期：FK/Jacobian solver 在 map 内稳定 `RobotRecord` 上构造，避免持有 move 前 Chain 引用。
12. C ABI 失败统一通过 `kdl_last_error` 返回结构化 `code/message/diagnostics`，覆盖 `KDL_NOT_IMPLEMENTED` 和 `KDL_BUFFER_TOO_SMALL`。
13. 新增 `KdlNativeAbi` 封装 C ABI 导出检查、缓冲区读取、返回码检查和错误归一化。
14. 新增 `fkPose7` TypedArray 高频 FK 接口，支持复用输出缓冲区并覆盖输出维度错误。
15. 性能基线已记录 6 轴初始化、TypedArray FK、平面 IK、1000 点批量可达性和 10 秒 4 ms 轨迹采样。
16. native IK、fkAllLinks JSON 序列化和 native 轨迹规划仍未接入；这些能力继续由 TypeScript baseline 覆盖。
10. `kdl_fk_all_links` 输出 JSON link pose 列表，测试覆盖 `base_link/link_1/tool0` 顺序和 tip 位姿。
11. `kdl_jacobian` 通过 `ChainJntToJacSolver` 输出 6 x dof 行主序矩阵，测试覆盖线速度和角速度分量。
12. `kdl_ik` 通过 `ChainIkSolverPos_LMA` 求解，测试覆盖 native IK 后 FK 回代到 `[pi/2, 0.4]`。
13. `kdl_sample_trap` 输出完整梯形/三角速度曲线 JSON，测试覆盖 trapezoid、triangle 和非法输入结构化错误。
14. 修复 native solver 生命周期：FK/Jacobian/IK solver 在 map 内稳定 `RobotRecord` 上构造，避免持有 move 前 Chain 引用。
15. C ABI 失败统一通过 `kdl_last_error` 返回结构化 `code/message/diagnostics`，覆盖 `KDL_INVALID_HANDLE`、`KDL_INVALID_TRAP_PROFILE` 和 `KDL_BUFFER_TOO_SMALL`。
16. 新增 `KdlNativeAbi` 封装 C ABI 导出检查、缓冲区读取、返回码检查和错误归一化。
17. 新增 `fkPose7` TypedArray 高频 FK 接口，支持复用输出缓冲区并覆盖输出维度错误。
18. 性能基线已记录 6 轴初始化、TypedArray FK、平面 IK、1000 点批量可达性和 10 秒 4 ms 轨迹采样。
19. native `planMoveJ/planMoveL/planMoveC/planPath` 仍由 TypeScript runtime 基线承载；底层 C ABI 已保留稳定导出，后续可逐步切到 native solver。
是否通过：通过。

2
working/README.md
View File

@@ -66,4 +66,4 @@ OPFS、虚拟控制器、UI、报告等内容只在上述两份文档明确要
## 5. 当前首要任务
`KW-001` 到 `KW-013`、`KW-100` 到 `KW-112` 的文档对标任务已完成并补充验收证据。C ABI 已能从 `NormalizedRobotModel` 构造 native Orocos KDL Chain，并完成 native FK/Jacobian golden 对比；后续首要关注点是继续接入 native IK、fkAllLinks 序列化和轨迹规划。
`KW-001` 到 `KW-013`、`KW-100` 到 `KW-112` 的文档对标任务已完成并补充验收证据。C ABI 已能从 `NormalizedRobotModel` 构造 native Orocos KDL Chain，并完成 native FK、fkAllLinks、Jacobian、IK 和 sampleTrap golden 对比；高层 `planMoveJ/planMoveL/planMoveC/planPath` 仍由 TypeScript runtime 基线承载，后续若继续深化 native 主线，首要关注点是把这些轨迹规划导出逐步切到 native solver。