Algorithms 模块开发指南

algorithms/ 保存与具体厂商协议无关的运动学、规划、控制和感知算法。算法接受通用类型或显式接口输入，不应直接解析设备报文，也不应承担 gRPC/QUIC 传输职责。

返回项目总览。

当前结构

目录	主要能力	主要 CMake target
`kinematics/ik_solver/`	Pinocchio DLS/QP、SRS、LAWBA 逆运动学	`cmvr_es::ik_solver`
`motion_planner/base_motion/`	TOPPRA、S 曲线、笛卡尔速度限制	`cmvr_es::base_motion`
`motion_planner/arm_motion/`	MoveJ、MoveL、SpeedL 机械臂规划	`cmvr_es::algorithms::arm_motion`
`controllers/`	PID、IBVS、笛卡尔速度控制	`cmvr_es::algorithms::controller`、`cmvr_es::algorithms::arm_control`
`perception/`	AprilTag 和视觉定位	`cmvr_es::perception`

顶层入口是 CMakeLists.txt。

依赖边界

算法层可以依赖 common/、Eigen、Pinocchio、OSQP、TOPPRA、OpenCV、ViSP 等；
不包含串口、CAN、HTTP 或厂商 SDK 协议处理；
不启动 gRPC/QUIC 服务或管理设备生命周期；
不从算法内部读取全局配置文件，构造或 configure 时显式传入配置；
可复用算法不应主动取得 DeviceManager 单例。

当前部分 controller target 仍链接 device_manager，这是现有耦合。新增算法应优先通过参数、回调或窄接口注入设备状态，避免继续扩大该依赖。

扩展已有算法类别

1. 定义或复用抽象接口

常用接口：

接口应明确：

输入输出单位和坐标系；
是否修改内部状态；
是否线程安全；
失败时输出是否保持不变；
是否支持实时循环，以及最大允许耗时。

2. 增加配置

在 ../../protos/README.md 指导下：

为算法增加独立配置 message；
在所属 oneof algorithm 中增加新字段和新 tag；
不复用已发布 tag；
为迭代次数、容差、速度和加速度设置有效范围；
在默认设备配置中给出显式参数。

3. 实现与工厂注册

将实现放在对应类别子目录，并修改实际工厂：

IK：ik_solver_factory.h
MoveJ：joint_motion_planner_factory.h
MoveL / SpeedL：cartesian_motion_planner_factory.h

工厂失败应返回 nullptr 并记录清晰原因，不能静默回退到另一个算法。MoveL 和 SpeedL 的实现必须保持配置组合一致。

4. 更新 CMake

将实现 .cpp 加入对应 library；
使用项目已有 alias target；
通过 target_include_directories 暴露公共头；
将依赖放入使用它的最小 target；
测试源文件不能加入生产共享库；
新增三方依赖时同步根依赖发现逻辑和 request.txt。

数值与机器人语义

算法扩展至少需要明确：

关节位置单位为 rad，速度为 rad/s；
笛卡尔平移为 m，旋转和角速度为 rad；
base、tool、world、user frame 的转换方向；
URDF base frame、tip frame 和关节顺序；
位置、速度、加速度和 jerk 限制；
奇异点、不可达目标和求解超时行为；
measured state 与算法内部 seed 的更新时机。

IK 在求解前应使用真实关节角更新 seed。MoveL 连续求解时，应使用上一步解更新下一步状态，不能一直使用初始状态。

测试要求

每个新算法至少覆盖：

正常输入；
空输入、自由度不匹配和 NaN/Inf；
关节限位与速度限制；
不可达目标和不收敛；
坐标系转换；
确定性和重复调用；
若用于实时控制，统计最坏执行时间；
与一个已知模型或离线参考结果对比。

当前不少算法测试只通过 add_executable() 构建，没有登记到 CTest。新增无设备测试应放在 BUILD_TESTING 条件内，并使用 add_test()；需要图形界面、RealSense 或 MuJoCo 的测试应明确标为集成测试，不得阻塞默认无设备测试。

新增算法类别

如果现有类别无法承载：

在 algorithms/<category>/ 新建目录；
定义协议无关抽象接口；
定义配置 Proto 和工厂；
提供单独 CMake library 与 cmvr_es::... alias；
在 algorithms/CMakeLists.txt 添加子目录；
由设备或任务层注入使用，不让算法反向控制服务层；
添加无设备单元测试和真实设备/仿真集成测试。

提交检查

厂商协议没有进入算法接口
单位、坐标系和关节顺序明确
工厂已注册且配置组合经过校验
不可达、超时和数值异常可观测
测试没有被编入生产共享库
无设备测试已登记到 CTest
实时路径没有日志洪泛和无界内存分配

5.2 KiB Raw Blame History Unescape Escape