Moveit Servo中允许碰撞矩阵对世界碰撞对象的忽略问题分析

问题背景

在Moveit机器人运动控制框架中，Servo模块负责实现实时伺服控制功能。该模块包含一个重要的碰撞检测组件，用于在伺服运动过程中持续监控机器人可能发生的碰撞情况。碰撞检测通常分为两类：机器人自碰撞检测和机器人与环境(世界)碰撞检测。

问题发现

在Moveit Servo的碰撞检测实现中，开发人员发现了一个关键问题：系统在处理世界碰撞对象时，没有考虑预先配置的允许碰撞矩阵(Allowed Collision Matrix, ACM)。这意味着即使某些碰撞对在ACM中被明确允许，系统仍然会将其视为潜在碰撞进行检查。

技术细节分析

碰撞检测的核心代码位于collision_check.cpp文件中。通过代码审查发现：

1. 对于自碰撞检测，代码正确地将ACM传递给检测函数：

getLockedPlanningSceneRO()->getCollisionEnv()->checkSelfCollision(
    collision_request_, collision_result_, *current_state_, acm_);

2. 但对于世界碰撞检测，代码却忽略了ACM参数：

getLockedPlanningSceneRO()->getCollisionEnv()->checkRobotCollision(
    collision_request_, collision_result_, *current_state_);  // 缺少acm_参数

这种不一致的实现会导致以下问题：

• 系统性能下降：不必要的碰撞检查增加了计算负担
• 功能限制：用户无法通过ACM灵活配置允许的碰撞对
• 行为不一致：自碰撞和世界碰撞处理逻辑不统一

解决方案

修复方案非常简单直接 - 在调用checkRobotCollision时添加ACM参数：

getLockedPlanningSceneRO()->getCollisionEnv()->checkRobotCollision(
    collision_request_, collision_result_, *current_state_, acm_);

影响评估

该修复将带来以下改进：

1. 完整支持ACM功能，与世界碰撞对象交互更加灵活
2. 减少不必要的碰撞检查，提高伺服控制性能
3. 使自碰撞和世界碰撞的处理逻辑保持一致
4. 不影响现有正常碰撞检测功能

最佳实践建议

对于Moveit Servo用户，在使用允许碰撞矩阵时应注意：

1. 明确区分自碰撞和世界碰撞的ACM配置
2. 在性能敏感场景，合理配置ACM可以减少计算开销
3. 定期检查ACM配置，确保其与实际应用需求一致

该问题已在最新版本中修复，建议用户更新到包含此修复的Moveit版本以获得更完善的碰撞检测功能。