机器人 / 系统课时
定位障碍物或自碰撞误判
定位障碍物或自碰撞误判
Panda 在 RViz 中看似避开桌面却始终规划失败,应先核对 MoveIt 2 实际使用的碰撞场景。本课以 ROS 2 Jazzy 为例,读取障碍物、接触对与允许碰撞关系,定位几何尺寸、坐标变换或自碰撞配置造成的误判。
学习目标
- 在 RViz 中区分可视网格、碰撞几何与场景障碍物。
- 查询
move_group使用的场景和碰撞对,锁定误判来源。 - 修正碰撞对象或 ACM,并用同一规划请求复测。
固定失败现场并查看碰撞几何
先启动包含 Panda、move_group 和 RViz 的 MoveIt 2 演示或项目配置,确认规划组为 panda_arm。记录失败时的起点关节角、目标位姿、规划时间及场景版本;排错期间停止机械臂运动,只点击 Plan,不要点击 Execute。
在 RViz 左侧 Displays 中展开 MotionPlanning 或 PlanningScene 显示项:
- 在
Scene Geometry中开启Show Scene Geometry,检查桌子、箱体等场景物体是否存在。 - 在
Scene Robot中关闭Show Robot Visual、开启Show Robot Collision,看真正参与碰撞检查的机器人外形。 - 检查
Global Options → Fixed Frame能通过 TF 连到panda_link0;如果场景中有world,先确认world与基座的变换,不要仅靠画面位置判断是否相交。
在已加载 ROS 2 Jazzy 和项目工作空间的终端执行:
ros2 service list -t | grep -E 'get_planning_scene|check_state_validity'预期输出(采用默认服务命名的 move_group):
/check_state_validity [moveit_msgs/srv/GetStateValidity]
/get_planning_scene [moveit_msgs/srv/GetPlanningScene]如果服务带命名空间,后面的程序需同步修改服务名称;若查询不到服务,先检查 move_group 是否启动及状态验证能力是否启用。
用服务取出碰撞对象和接触对
只观察 RViz 画面不能证明规划器内部看到的物体相同。以下程序先从 /get_planning_scene 取得世界对象、附着对象与机器人状态,再把该状态交给 /check_state_validity,列出发生碰撞的对象名称。将代码保存为 scene_collision_probe.py。
import rclpy
from rclpy.node import Node
from moveit_msgs.msg import PlanningSceneComponents
from moveit_msgs.srv import GetPlanningScene, GetStateValidity
def call_service(node, srv_type, name, request):
client = node.create_client(srv_type, name)
if not client.wait_for_service(timeout_sec=5.0):
raise RuntimeError(f"服务不存在或未就绪: {name}")
future = client.call_async(request)
rclpy.spin_until_future_complete(node, future, timeout_sec=5.0)
if not future.done() or future.result() is None:
raise RuntimeError(f"服务请求超时或失败: {name}")
return future.result()
def main():
rclpy.init()
node = Node("scene_collision_probe")
try:
scene_req = GetPlanningScene.Request()
c = PlanningSceneComponents
# 同时取出附着物,避免把抓取对象误当成普通世界障碍物
scene_req.components.components = (
c.ROBOT_STATE_ATTACHED_OBJECTS
| c.WORLD_OBJECT_GEOMETRY
| c.ALLOWED_COLLISION_MATRIX
)
scene = call_service(
node, GetPlanningScene, "/get_planning_scene", scene_req
).scene
world = scene.world.collision_objects
attached = scene.robot_state.attached_collision_objects
print(f"world={len(world)} attached={len(attached)}")
for obj in world:
p = obj.pose.position
print(
f"object={obj.id} frame={obj.header.frame_id} "
f"origin=({p.x:.3f},{p.y:.3f},{p.z:.3f})"
)
for primitive, pose in zip(obj.primitives, obj.primitive_poses):
print(
f" shape={primitive.type} dims={list(primitive.dimensions)} "
f"local_z={pose.position.z:.3f}"
)
validity_req = GetStateValidity.Request()
validity_req.robot_state = scene.robot_state
validity_req.group_name = "panda_arm"
verdict = call_service(
node, GetStateValidity, "/check_state_validity", validity_req
)
# 同一物体对可能产生多个接触点,这里按物体对去重
pairs = sorted({
tuple(sorted((hit.contact_body_1, hit.contact_body_2)))
for hit in verdict.contacts
})
print(f"valid={verdict.valid} collision_pairs={len(pairs)}")
for left, right in pairs:
print(f"collision={left} <-> {right}")
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()在已配置环境的终端运行 python3 scene_collision_probe.py。预期输出格式如下,具体计数与名称由现场场景决定:
world=<N> attached=<M>
object=<ID> frame=<FRAME> origin=(<X>,<Y>,<Z>)
shape=<TYPE> dims=[<DIMENSIONS>] local_z=<Z>
valid=<True或False> collision_pairs=<K>
collision=<LINK> <-> <OBJECT_OR_LINK>只有存在世界对象时才显示 object、shape 行,只有碰撞时才显示 collision 行。valid=False 且存在碰撞对时可据此定位;valid=True 只说明这一组关节状态在当前场景与未额外指定约束的条件下有效,不证明整条轨迹无碰撞。若机器人在运动,两次服务调用可能取到不同时间的场景,复现时应保持静止。
将碰撞对映射回几何与坐标
先按名称区分 panda_link5 ↔ table 一类机械臂与世界物体的接触,以及 panda_link2 ↔ panda_link4 一类连杆间自碰撞,再核对真正参与计算的几何。

例如桌面实测顶面为 z=0.75 m,用 BOX 代表厚度 0.04 m 的桌面时,其中心应在 z=0.73 m。若误把顶面高度填成箱体中心高度,碰撞体顶面就会到 0.77 m,平白多占用 0.02 m 的空间。箱体尺寸应按 X、Y、Z 三个方向的完整长度填写,例如 [0.80, 0.60, 0.04],单位是米。
检查时按以下顺序定位:
- 世界物体碰撞:核对对象
id、header.frame_id、pose、primitive_poses与尺寸;物体位姿和形状局部位姿需要组合,不能把两个位置直接当成同一坐标。查询结果可能已归一到规划坐标系,还须检查原始发布代码中的frame_id是否正确并可经 TF 转换。 - 机器人自碰撞:检查 URDF 对应 link 的
<collision>几何与<origin>,不要拿<visual>网格判断安全距离。过大的简化胶囊或错误原点可能让两连杆在未实际接触时相交。 - 抓取状态碰撞:若物体已附着于夹爪,应在
robot_state.attached_collision_objects中查看,并核对link_name与touch_links;同一物体意外留在世界对象中会带来重复几何或错误接触关系。
修正允许碰撞关系并复测
ACM(Allowed Collision Matrix)记录哪些指定对象对可以忽略碰撞;在返回消息中,可通过 entry_names 的索引查看 entry_values[i].enabled[j],若没有显式条目,还须检查 default_entry_names 和 default_entry_values。SRDF 中的 disable_collisions 常用于经过验证的相邻连杆或永不接触的连杆对;对于抓取物体与手指的预期接触,则优先检查附着物的 touch_links。不要为了使 plan() 成功而全局放开自碰撞或把桌面列入允许碰撞对象。
| 发现的问题 | 修正位置 | 复核证据 |
|---|---|---|
桌面箱体高了 0.02 m |
创建 CollisionObject 的尺寸或位姿 |
场景查询的尺寸、位置与实测一致 |
| 自碰撞仅由错误网格造成 | URDF <collision> 几何和原点 |
同一关节角不再报告错误连杆对 |
| 合法抓取接触被禁止 | 附着对象的 touch_links,或经审查的 ACM |
仅预期接触对被放行,其他碰撞仍可检出 |
变更 SRDF 的自碰撞配置后,需要重新加载机器人配置,通常重启相关 MoveIt 节点;修改世界障碍物时,可通过 PlanningSceneInterface::applyCollisionObject() 同步更新,再查询确认新值已进入 move_group,不能只修改 RViz 中的显示状态。
最后保持相同起点、相同目标、相同规划器与超时复测:先用诊断程序确认误报的碰撞对消失,再在 RViz 中只执行 Plan 并检查生成轨迹没有穿过桌面。可将规划时间设为 5 s 并重复 10 次记录成功次数;若当前状态已有效却仍规划失败,应转查目标可达性、路径约束与采样超时,而不继续放宽碰撞检查。