Skip to main content

手臂防碰撞功能使用文档

功能包说明​

  • 用于检测机器人手臂的碰撞情况, 仅支持机器人双手臂之间的碰撞、机器人手臂与机器人本体之间的碰撞检测, 无外部环境（如障碍物、操作人员、外部设备等）感知及碰撞防护能力, 使用时需确保周围环境无额外干扰。

  • 当启用 “手臂碰撞后自动归位” 功能时, 检测到上述范围内的碰撞后, 将自动控制手臂移动至碰撞发生前 3 秒的姿态位置, 避免碰撞加剧。

    • 自动归位动作完成后, 系统会自动将手臂控制模式切换为"1-保持姿势", 此时手臂将固定在归位后的姿态, 无法通过外部指令直接驱动。

    • 自动归位动作完成后, 若需重新控制手臂, 需重新将手臂控制模式切换为"2-外部控制", 切换完成后方可接收外部控制指令

注意事项​

  • 使用前建议参考机器人关节标定文档中的"头部和手臂零点自动标定"章节进行标定, 以确保检测效果准确

  • 碰撞检测并无"预判功能", 具有一定滞后性, 因此用户使用时不建议手臂移动速度过快

  • 用户自行使用此功能包时，如果需要启动碰撞归位功能，需要将原本的目标话题 kuavo_arm_traj 和 kuavo_arm_target_poses改为/arm_collision/kuavo_arm_traj和/arm_collision/kuavo_arm_target_poses，否则发送碰撞归位时会发送冲突

启动方式​

step 1:编译功能包​

cd /home/lab/kuavo-ros-opensource
sudo su
catkin build kuavo_arm_collision_check

step 2:修改配置文件​

  • 若您的机器无手腕相机, 可修改配置文件, 以令检测效果更准确
  • 文件位置如下,请根据您的机器版本选择:(若不清楚, 可在下位机终端输入echo $ROBOT_VERSION查询)
    • 45版本:/home/lab/kuavo-ros-opensource/src/kuavo_arm_collision_check/config/s45_collision_config.yaml
    • 49版本:/home/lab/kuavo-ros-opensource/src/kuavo_arm_collision_check/config/s49_collision_config.yaml
  • 编辑文件, 将文件结尾添加如下内容即可
ignore_links:
  - l_hand_tripod
  - r_hand_tripod

step 3:启动手臂碰撞检测程序(并开启碰撞自动归位)​

cd /home/lab/kuavo-ros-opensource
sudo su
source devel/setup.bash
roslaunch kuavo_arm_collision_check arm_collision_check.launch arm_collision_enable_arm_moving:=true

step 4.启动 VR 遥操(并启用碰撞检测归位)​

确保已按step 3启动手臂碰撞检测程序

cd /home/lab/kuavo-ros-opensource
sudo su
source devel/setup.bash
roslaunch noitom_hi5_hand_udp_python launch_quest3_ik.launch use_arm_collision:=true use_cpp_ik:=false

若以此方式启动VR程序, 则:

  • VR 控制手臂发生碰撞时, 会自动将手臂位置恢复到 3 秒前的状态
  • 手臂位置恢复到 3 秒前状态后, 手臂会保持在这个位置, VR 手柄需按下 X+A 才可恢复手臂跟踪。

模型文件说明​

该节点启动后根据 ROBOT_VERSION 环境变量读取 URDF 文件，并加载相应的 STL 模型。模型加载优先级如下：

  1. cache 文件夹：优先使用缓存模型（经过处理的模型）
  2. kuavo_assets：如果上述文件夹中没有 STL 文件，则对 kuavo_assets 内的原始 STL 文件进行膨胀处理, 将处理后的模型文件导入cache 文件夹

注意：当前仅支持 ROBOT_VERSION=45 和 ROBOT_VERSION=49，其他版本可能导致关节不匹配问题。

配置文件说明​

  • 配置文件路径为:$(find kuavo_arm_collision_check)/config/s$(arg robot_version)_collision_config.yaml
    • 45版本:src/kuavo_arm_collision_check/config/s45_collision_config.yaml
    • 49版本:src/kuavo_arm_collision_check/config/s49_collision_config.yaml
  • 如下内容中提到的配置文件均代指这个文件

1. 膨胀系数（Inflation）​

  • 说明:每个关节或 STL 模型的膨胀系数（单位：米）。膨胀系数用于在碰撞检测时，对模型进行微小放大，以提高安全性和鲁棒性。
inflation:
  torso_STL: 0.020
  camera_STL: 0.020
  ...
  r_hand_pitch_STL: 0.020
  head_yaw_STL: 0.020
  head_pitch_STL: 0.020
  • 注意事项：
    • 为了避免 ROS 参数名称不合法，原文件名中的.或-已经被替换成_。
    • 可以针对不同关节设置不同膨胀系数，例如手臂关节可以略大于腿部关节以增加碰撞安全边界。
    • 修改膨胀系数时，需要保证数值在合理范围（一般 0.01~0.05 米），过大可能导致虚假碰撞，过小可能漏检。
    • 若修改了某个模型的膨胀系数, 需要删掉cache 文件夹中的对应模型文件, 这样才会使用新的膨胀系数重新加载模型
    • 示例:使用sudo用户删除全部缓存文件:sudo rm -rf src/kuavo_arm_collision_check/cache/*

2. 碰撞过滤规则（Collision Filter）​

  • 说明:定义不进行碰撞检测的关节对（joint pairs），用于屏蔽机械臂自身相邻关节或特定关节的碰撞检测。
collision_filter:
  - [r_hand_tripod, zarm_r5_link]
  - [l_hand_tripod, zarm_l5_link]
  • 注意事项：
    • 列表中每一项是一个关节对 [linkA, linkB]。
    • 关节名必须与机器人模型加载时使用的名称一致。
    • 当检测到 linkA 与 linkB 时，将跳过该碰撞对。
    • 可以根据不同机器人版本或手型添加新的屏蔽规则
    • 默认的屏蔽规则是正确的, 除非用户需要在机器人上额外添加物理结构件, 否则无需更改此部分内容
  • 说明： ignore_links 用于指定在碰撞检测中完全忽略的 link。 当某个 link 被加入此列表后，它将不会参与任何碰撞检测，适用于一些非关键结构件、辅助支架、传感器模型等。
ignore_links:
  - l_hand_tripod
  - r_hand_tripod
  • 注意事项：
    • 列表中每一项为一个 link 名称。
    • 被忽略的 link 将不与任何其他 link 进行碰撞检测（包括机器人自身以及环境）。
    • 若 ignore_links: 为空（例如仅写成 ignore_links:），则表示无任何 link 被忽略，不影响正常运行。

功能包详细说明​

此部分为对手臂碰撞检测包的详细说明

缓存机制​

程序首次加载时会使用 OpenMesh 库处理 STL 模型，并将处理后的文件存储在 cache 文件夹内。后续启动将直接使用缓存模型以提高性能。

注意：编译时会清空 cache 文件夹，以防止代码修改导致缓存模型失效。

工作流程​

  1. 加载 STL 文件
  2. 将 STL 文件内的模型添加为 FCL 的碰撞体
  3. 订阅 TF，实时更新碰撞体的位姿
  4. 按照设定频率（默认 5Hz）计算碰撞检测
  5. 发生碰撞时发布碰撞信息
  6. 持续记录传感器数据（默认 3 秒）

发布的话题​

  • /arm_collision/info：碰撞信息，包含涉及碰撞的关节名
    • 消息类型：kuavo_msgs/armCollisionCheckInfo
  • /arm_collision/check_duration：碰撞检测耗时（毫秒）
  • /arm_collision/markers：用于 RViz 显示的碰撞标记（可选，启用会增大耗时）
    • 消息类型：kuavo_msgs/sensorsData
  • /kuavo_arm_target_poses：手臂目标位姿
    • 消息类型：kuavo_msgs/armTargetPoses
  • /kuavo_arm_traj：手臂轨迹数据
    • 消息类型：sensor_msgs/JointState

订阅的话题​

  • /arm_collision/kuavo_arm_traj：手臂轨迹数据
    • 消息类型：sensor_msgs/JointState
  • /arm_collision/kuavo_arm_target_poses：手臂目标位姿
    • 消息类型：kuavo_msgs/armTargetPoses
  • /sensors_data_raw：原始传感器数据
    • 消息类型：kuavo_msgs/sensorsData

服务​

  • /arm_collision/wait_complete：等待碰撞回归完成
    • 服务类型：std_srvs/SetBool
    • 用于同步操作，等待当前操作完成
  • /arm_collision/set_arm_moving_enable：设置手臂移动检测启用状态
    • 服务类型：std_srvs/SetBool
    • 动态启用或禁用手臂发生碰撞时移动归位功能

节点参数​

参数名类型默认值说明
/arm_collision/check_freqdouble5.0碰撞检测频率（Hz）
/arm_collision/publish_collision_markersboolfalse是否发布碰撞标记用于 RViz 显示
/arm_collision/enable_arm_movingboolfalse是否启用手臂碰撞后自动归位
/arm_collision/arm_move_diffdouble0.1手臂移动差异阈值

依赖项​

  • ROS
  • FCL (Flexible Collision Library)
  • OpenMesh
  • kuavo_msgs
  • kuavo_common
  • tf2_ros
  • sensor_msgs
  • visualization_msgs

使用示例​

  1. 启动碰撞检测节点：

    roslaunch kuavo_arm_collision_check arm_collision_check.launch

  2. 查看碰撞信息：

    rostopic echo /arm_collision/info

  3. 在 RViz 中显示碰撞标记：

    roslaunch kuavo_arm_collision_check arm_collision_check.launch arm_collision_publish_collision_markers:=true

    （默认禁用，需要手动启用）

  4. 调整检测频率：

    roslaunch kuavo_arm_collision_check arm_collision_check.launch arm_collision_check_freq:=10.0

  5. 禁用手臂移动检测：

    roslaunch kuavo_arm_collision_check arm_collision_check.launch arm_collision_enable_arm_moving:=false

故障排除​

  • 如果碰撞检测频率过低，可以调整 arm_collision_check_freq 参数
  • 如果模型加载失败，检查 URDF 文件路径和 STL 文件是否存在
  • 如果缓存出现问题，可以手动删除 cache 文件夹重新生成
  • 如果手臂移动检测不工作，检查 enable_arm_moving_check_ 参数是否正确设置
  • 确保 ROBOT_VERSION 环境变量已正确设置（当前支持版本 45 49）
  • 如果传感器数据记录异常，检查 /sensors_data_raw 话题是否正常发布
  • 如果服务调用失败，检查服务是否正常启动和参数是否正确
浏览量 --