cmvr-es/cmvr-es/algorithms

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. 定义或复用抽象接口

常用接口：

• IKSolver
• JointMotionPlanner
• CartesianMotionPlanner

接口应明确：

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

2. 增加配置

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

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

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 连续求解时，应使用上一步解更新下一步状态，不能一直使用初始状态。

测试要求

每个新算法至少覆盖：

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

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

新增算法类别

如果现有类别无法承载：

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

提交检查

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