Bullet 碰撞检查器

除了 Flexible Collision Library (FCL) 之外，Bullet Collision Detection 也可以用作碰撞检测器。本教程以碰撞可视化教程为基础，进一步展示碰撞检测功能。

此外，本教程还演示了 Bullet 提供的连续碰撞检测（Continuous Collision Detection, CCD）功能。

开始之前

如果尚未完成，请先完成 Getting Started 中的步骤。

运行代码

使用 roslaunch 直接从 moveit_tutorials 运行 launch 文件：

roslaunch moveit_tutorials bullet_collision_checker_tutorial.launch

此时你应该能看到 Panda 机器人和一个箱子，两者都带有可拖动的交互标记。需要注意的是，与 FCL 不同，Bullet 不会计算形状的所有接触点，而只计算穿透最深的那个点。

左侧：FCL 碰撞结果。右侧：Bullet 碰撞结果。

需要注意的是，当前 Bullet 碰撞检测器的实现不是线程安全的，因为其内部的碰撞管理器是可变成员。

连续碰撞检测

Bullet 还具备连续碰撞检测能力。这意味着可以保证机器人在两个离散状态之间的运动过程中不会与环境发生碰撞。要观看 CCD 演示，请点击 RViz 左下角 moveit_visual_tools 面板中的 Next 按钮。此时交互式机器人消失，取而代之的是机器人手部恰好位于箱子后方的位姿。再次按下 Next，机器人跳转到手部恰好位于箱子前方的位姿。在这两种状态下，均未检测到碰撞（参见终端输出）。

左侧：位姿 1 中的机器人。右侧：位姿 2 中的机器人。

再按一次 Next，系统将使用在两个离散位姿之间扫掠的机器人模型执行 CCD，并报告发生了碰撞（详见终端输出）。

再按一次 Next 即可完成本教程。

相关代码

完整代码可以在 moveit_tutorials GitHub 项目的此处查看。为了让本教程聚焦于 Bullet 本身，我们省略了一些理解演示原理所需的基础信息。关于碰撞可视化的代码说明，请参阅碰撞可视化。

代码首先创建了一个交互式机器人和一个新的规划场景。

    InteractiveRobot interactive_robot("robot_description", "bullet_collision_tutorial/interactive_robot_state");
    g_planning_scene = std::make_unique<planning_scene::PlanningScene>(interactive_robot.robotModel());

将碰撞检测器切换为 Bullet

通过 Bullet 专用的碰撞检测器分配器，可以在规划场景中设置当前激活的碰撞检测器。

    g_planning_scene->setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorBullet::create());

关于交互式机器人的更多说明，请参阅碰撞可视化教程。

运行连续碰撞检测

为了演示 CCD，代码再次加载 Panda 机器人并创建一个新的规划场景，同时将 Bullet 设置为当前激活的碰撞检测器。

  robot_model::RobotModelPtr robot_model = moveit::core::loadTestingRobotModel("panda");
  auto planning_scene = std::make_shared<planning_scene::PlanningScene>(robot_model);
  planning_scene->setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorBullet::create());

添加箱子并将机器人移动到指定位置。

  Eigen::Isometry3d box_pose{ Eigen::Isometry3d::Identity() };
  box_pose.translation().x() = 0.43;
  box_pose.translation().y() = 0;
  box_pose.translation().z() = 0.55;

  auto box = std::make_shared<shapes::Box>(BOX_SIZE, BOX_SIZE, BOX_SIZE);
  planning_scene->getWorldNonConst()->addToObject("box", box, box_pose);

  robot_state::RobotState& state = planning_scene->getCurrentStateNonConst();
  state.setToDefaultValues();

  double joint2 = -0.785;
  double joint4 = -2.356;
  double joint6 = 1.571;
  double joint7 = 0.785;
  state.setJointPositions("panda_joint2", &joint2);
  state.setJointPositions("panda_joint4", &joint4);
  state.setJointPositions("panda_joint6", &joint6);
  state.setJointPositions("panda_joint7", &joint7);
  state.update();

  robot_state::RobotState state_before(state);

最后执行一次碰撞检测，并将结果显示在终端上。

  collision_detection::CollisionResult res;
  collision_detection::CollisionRequest req;
  req.contacts = true;
  planning_scene->checkCollision(req, res);
  ROS_INFO_STREAM_NAMED("bullet_tutorial", (res.collision ? "In collision." : "Not in collision."));

该代码会对第二个机器人位姿重复执行同样的检测。

对于 CCD 检测，需要同时显示两个机器人状态。

  moveit_msgs::DisplayRobotState msg_state_before;
  robot_state::robotStateToRobotStateMsg(state_before, msg_state_before.state);
  robot_state_publisher_2.publish(msg_state_before);

现在可以使用两个不同的机器人状态执行连续碰撞检测。由于规划场景目前还没有直接执行 CCD 的接口，我们需要访问碰撞环境并执行检测。

  res.clear();
  planning_scene->getCollisionEnv()->checkRobotCollision(req, res, state, state_before);
  ROS_INFO_STREAM_NAMED("bullet_tutorial", (res.collision ? "In collision." : "Not in collision."));

终端输出将显示 “In collision.”。

Launch 文件

完整的 launch 文件在 GitHub 的此处。本教程中的所有代码都可以从 moveit_tutorials 包中编译并运行。