【MoveIt 2】使用 MoveIt 任务构造器（MoveIt Task Constructor）进行拾取和放置

最新推荐文章于 2025-01-18 17:11:06 发布

十年一梦实验室

最新推荐文章于 2025-01-18 17:11:06 发布

阅读量1.4k

点赞数 7

CC 4.0 BY-SA版权

本文链接：https://blog.youkuaiyun.com/cxyhjl/article/details/140861433

本教程将引导您创建一个使用 MoveIt 任务构造器规划抓取和放置操作的包。MoveIt 任务构造器（https://github.com/moveit/moveit_task_constructor/tree/ros2/）提供了一种为包含多个不同子任务（称为阶段）的任务进行规划的方法。如果您只想运行教程，可以按照 Docker 指南 https://moveit.picknik.ai/main/doc/how_to_guides/how_to_setup_docker_containers_in_ubuntu.html 启动一个包含完整教程的容器。

1 基本概念

MTC 的基本思想是复杂的运动规划问题可以分解为一组更简单的子问题。顶层规划问题被指定为任务，而所有子问题由阶段指定。阶段可以按任何任意顺序和层次排列，仅受各个阶段类型的限制。阶段的排列顺序受结果传递方向的限制。与结果流相关的阶段有三种：生成器阶段、传播器阶段和连接器阶段：

生成器独立于其相邻阶段计算结果，并在两个方向上传递，向后和向前。一个例子是几何位姿的 IK 采样器，其中接近和离开运动（相邻阶段）取决于解决方案。

传播器接收一个邻居阶段的结果，解决一个子问题，然后将结果传播到对面邻居。根据实现的不同，传播阶段可以分别向前、向后或双向传递解决方案。一个例子是基于起始状态或目标状态计算笛卡尔路径的阶段。

连接器不会传播任何结果，而是尝试弥合两个相邻状态之间的差距。一个例子是从一个给定状态到另一个状态的自由运动规划的计算。

除了顺序类型外，还有不同的层次类型允许封装从属阶段。没有从属阶段的阶段称为原始阶段，高级阶段称为容器阶段。有三种容器类型：

包装器封装单个从属阶段并修改或过滤结果。例如，仅接受其子阶段满足某个约束条件的解决方案的过滤阶段可以实现为包装器。此类型的另一个标准用法包括 IK 包装器阶段，该阶段基于带有姿态目标属性的规划场景生成逆运动学解决方案。

序列容器包含一系列从属阶段，并且仅将端到端解决方案视为结果。一个例子是由一系列连贯步骤组成的拣选动作。

并行容器结合了一组从属阶段，可用于传递最佳的替代结果、运行后备求解器或合并多个独立的解决方案。示例包括为自由运动计划运行替代规划器、用右手或左手作为后备拾取物体，或同时移动手臂和打开夹爪。

阶段不仅支持解决运动规划问题。它们还可以用于各种状态转换，例如修改规划场景。结合使用类继承的可能性，可以在仅依赖于一组结构良好的原始阶段的情况下构建非常复杂的行为。

有关 MTC 的更多详细信息，请参阅 MoveIt 任务构造器概念页面https://moveit.picknik.ai/main/doc/concepts/moveit_task_constructor/moveit_task_constructor.html

2 入门

如果您还没有这样做，请确保您已完成入门中的步骤。

2.1 下载 MoveIt 任务构造器

进入你的 colcon 工作区并拉取 MoveIt 任务构建器源代码：https://github.com/PickNikRobotics/moveit_task_constructor （手动下载）

cd ~/ws_moveit/src
git clone git@github.com:moveit/moveit_task_constructor.git -b ros2

cxy@cxy-Ubuntu2404:~/moveit_tutorials_ws/src$ cd ..
cxy@cxy-Ubuntu2404:~/moveit_tutorials_ws$ colcon build --packages-select moveit_task_constructor_demo

‍3 试用

MoveIt 任务构建器包包含几个基本示例和一个挑选和放置演示。对于所有演示，您应该启动基本环境：

ros2 launch moveit_task_constructor_demo demo.launch.py

随后，您可以运行各个演示：

ros2 launch moveit_task_constructor_demo cartesian.launch.py
ros2 launch moveit_task_constructor_demo modular.launch.py
ros2 launch moveit_task_constructor_demo pickplace.launch.py

cxy@cxy-Ubuntu2404:~/moveit_tutorials_ws$ source install/setup.bash
cxy@cxy-Ubuntu2404:~/moveit_tutorials_ws$ ros2 launch moveit_task_constructor_demo cartesian.launch.py

// 运行前 除之前的任务。rviz左下角 点击 Reset 按钮。
cxy@cxy-Ubuntu2404:~/moveit_tutorials_ws$ ros2 launch moveit_task_constructor_demo modular.launch.py

// 执行错误
cxy@cxy-Ubuntu2404:~/moveit_tutorials_ws$ ros2 launch moveit_task_constructor_demo pickplace.launch.py
……
[pick_place_demo-1] [INFO] [1722493269.128607352] [moveit_task_constructor_demo]: Calling PlanningResponseAdapter 'DisplayMotionPath'
[pick_place_demo-1] [WARN] [1722493269.128675058] [planning_scene_interface_98096004140416.moveit.moveit.ros.planning_pipeline]: The planner plugin did not fill out the 'planner_id' field of the MotionPlanResponse. Setting it to the planner ID name of the MotionPlanRequest assuming that the planner plugin does warn you if it does not use the requested planner.
[pick_place_demo-1] [INFO] [1722493269.128914802] [moveit_task_constructor_demo]: Planning succeded
[pick_place_demo-1] [INFO] [1722493269.128933682] [moveit_task_constructor_demo]: Execution disabled

在右侧，您应该会看到运动规划任务面板，概述任务的分层阶段结构。当您选择特定阶段时，成功和失败的解决方案列表将显示在最右侧的窗口中。选择其中一个解决方案将开始其可视化。

4 使用 MoveIt 任务构造器设置项目

本节介绍了使用 MoveIt 任务构造器构建简单任务所需的步骤。

4.1 创建一个新包

使用以下命令创建一个新包：

ros2 pkg create \
--build-type ament_cmake \
--dependencies moveit_task_constructor_core rclcpp \
--node-name mtc_node mtc_tutorial

这将创建一个名为 mtc_tutorial 的新包和文件夹，并依赖于 moveit_task_constructor_core ，以及在 src/mtc_node 中的 hello world 示例。

4.2 代码

在您选择的编辑器中打开 mtc_node.cpp ，并粘贴以下代码。

#include <rclcpp/rclcpp.hpp> // 引入ROS 2的C++客户端库
#include <moveit/planning_scene/planning_scene.h> // 引入MoveIt的规划场景库
#include <moveit/planning_scene_interface/planning_scene_interface.h> // 引入规划场景接口库
#include <moveit/task_constructor/task.h> // 引入MoveIt任务构造器的task库
#include <moveit/task_constructor/solvers.h> // 引入任务构造器的求解器库
#include <moveit/task_constructor/stages.h> // 引入任务构造器的阶段库
#if __has_include(<tf2_geometry_msgs/tf2_geometry_msgs.hpp>) // 条件编译，检查是否存在tf2_geometry_msgs库
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp> // 引入tf2_geometry_msgs库
#else
#include <tf2_geometry_msgs/tf2_geometry_msgs.h> // 如果不存在，使用.h后缀
#endif
#if __has_include(<tf2_eigen/tf2_eigen.hpp>) // 条件编译，检查是否存在tf2_eigen库
#include <tf2_eigen/tf2_eigen.hpp> // 引入tf2_eigen库
#else
#include <tf2_eigen/tf2_eigen.h> // 如果不存在，使用.h后缀
#endif


static const rclcpp::Logger LOGGER = rclcpp::get_logger("mtc_tutorial"); // 定义一个ROS日志记录器
namespace mtc = moveit::task_constructor; // 为moveit::task_constructor命名空间创建别名




class MTCTaskNode
{
public:
  // 构造函数，接受节点选项作为参数
  MTCTaskNode(const rclcpp::NodeOptions& options);


  // 获取节点的基本接口
  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();


  // 执行任务
  void doTask();


  // 设置规划场景
  void setupPlanningScene();


private:
  // 创建并配置MTC任务
  mtc::Task createTask(); // 从一系列阶段组成一个MTC任务
  mtc::Task task_; // MTC任务对象
  rclcpp::Node::SharedPtr node_; // ROS 2节点指针
};


// 构造函数实现
MTCTaskNode::MTCTaskNode(const rclcpp::NodeOptions& options)
  : node_{ std::make_shared<rclcpp::Node>("mtc_node", options) }// 初始化节点
{
}


// 获取节点的基本接口实现
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MTCTaskNode::getNodeBaseInterface()
{
  return node_->get_node_base_interface();
}


// 设置规划场景
void MTCTaskNode::setupPlanningScene()
{
  // 创建碰撞对象
  moveit_msgs::msg::CollisionObject object;
  object.id = "object"; // 对象ID
  object.header.frame_id = "world";  // 设置对象的参考坐标系
  object.primitives.resize(1); // 调整图元数组大小
  object.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER; // 设置图元类型为圆柱体
  object.primitives[0].dimensions = { 0.1, 0.02 }; // 圆柱的尺寸（高度0.1米，直径0.02米）


  // 设置碰撞对象的位姿
  geometry_msgs::msg::Pose pose; // 创建位姿对象
  pose.position.x = 0.5; // 设置位姿的x坐标
  pose.position.y = -0.25; // 设置位姿的y坐标
  pose.orientation.w = 1.0; // 设置位姿的四元数w
  object.pose = pose; // 将位姿赋值给碰撞对象


  // 应用碰撞对象到规划场景
  moveit::planning_interface::PlanningSceneInterface psi;// 创建规划场景接口对象
  psi.applyCollisionObject(object); // 将碰撞对象应用到规划场景中
}


// 执行任务
void MTCTaskNode::doTask()
{
  // 创建并初始化任务
  task_ = createTask();// 创建任务


  try
  {
    task_.init();// 初始化任务
  }
  catch (mtc::InitStageException& e)
  {
    // 捕获初始化异常并记录错误
    RCLCPP_ERROR_STREAM(LOGGER, e);// 捕获初始化阶段异常并记录错误
    return;
  }


  // 规划任务
  if (!task_.plan(5)) // 规划任务，最多尝试5次
  {// 记录任务规划失败的错误
    RCLCPP_ERROR_STREAM(LOGGER, "Task planning failed");
    return;
  }


  // 发布解决方案以供调试
  task_.introspection().publishSolution(*task_.solutions().front());


  // 执行任务并检查结果
  auto result = task_.execute(*task_.solutions().front());// 执行任务
  if (result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
  {// 记录任务执行失败的错误
    RCLCPP_ERROR_STREAM(LOGGER, "Task execution failed");
    return;
  }


  return;
}


// 创建任务
mtc::Task MTCTaskNode::createTask()
{
  mtc::Task task; // 创建任务对象
  task.stages()->setName("demo task"); // 设置任务名称
  task.loadRobotModel(node_); // 加载机器人模型


  // 定义机器人运动组的名称
  const auto& arm_group_name = "panda_arm"; // 机械臂组名称
  const auto& hand_group_name = "hand"; // 手部组名称
  const auto& hand_frame = "panda_hand"; // 手部框架名称


  // 设置任务的属性
  task.setProperty("group", arm_group_name);
  task.setProperty("eef", hand_group_name);
  task.setProperty("ik_frame", hand_frame);


// 禁用未使用变量的警告
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-but-set-variable"
  mtc::Stage* current_state_ptr = nullptr;  // 用于传递当前状态给抓取姿势生成器 // 将current_state转发到抓取位姿生成器
#pragma GCC diagnostic pop


  // 添加当前状态阶段
  auto stage_state_current = std::make_unique<mtc::stages::CurrentState>("current"); // 创建当前状态阶段
  current_state_ptr = stage_state_current.get(); // 获取当前状态阶段的指针
  task.add(std::move(stage_state_current)); // 将当前状态阶段添加到任务中


  // 定义不同的规划器
  auto sampling_planner = std::make_shared<mtc::solvers::PipelinePlanner>(node_); // 创建采样规划器
  auto interpolation_planner = std::make_shared<mtc::solvers::JointInterpolationPlanner>(); // 创建插值规划器


  auto cartesian_planner = std::make_shared<mtc::solvers::CartesianPath>(); // 创建笛卡尔路径规划器
  cartesian_planner->setMaxVelocityScalingFactor(1.0); // 设置最大速度缩放因子
  cartesian_planner->setMaxAccelerationScalingFactor(1.0); // 设置最大加速度缩放因子
  cartesian_planner->setStepSize(.01); // 设置步长


  // 创建打开手的阶段
  auto stage_open_hand =
      std::make_unique<mtc::stages::MoveTo>("open hand", interpolation_planner); // 创建打开手部阶段
  stage_open_hand->setGroup(hand_group_name); // 设置手部组
  stage_open_hand->setGoal("open"); // 设置目标为打开
  task.add(std::move(stage_open_hand)); // 将打开手部阶段添加到任务中


  return task; // 返回任务对象
}


// 主函数
int main(int argc, char** argv)
{
  rclcpp::init(argc, argv); // 初始化ROS


  rclcpp::NodeOptions options;// 创建节点选项对象
  options.automatically_declare_parameters_from_overrides(true);// 自动声明覆盖的参数


  auto mtc_task_node = std::make_shared<MTCTaskNode>(options); // 创建MTC任务节点  // 创建MTCTaskNode对象
  rclcpp::executors::MultiThreadedExecutor executor; // 创建多线程执行器


  // 创建线程来运行执行器
  auto spin_thread = std::make_unique<std::thread>([&executor, &mtc_task_node]() {
    executor.add_node(mtc_task_node->getNodeBaseInterface()); // 将节点添加到执行器
    executor.spin(); // 阻塞调用，执行器开始运行
    executor.remove_node(mtc_task_node->getNodeBaseInterface()); // 从执行器中移除节点
  });


  mtc_task_node->setupPlanningScene(); // 设置规划场景
  mtc_task_node->doTask(); // 执行任务


  spin_thread->join(); // 等待线程结束
  rclcpp::shutdown(); // 关闭ROS
  return 0;
}

4.3 代码分解

代码顶部包含此包使用的 ROS 和 MoveIt 库。

rclcpp/rclcpp.hpp 包含核心 ROS2 功能

moveit/planning_scene/planning_scene.h 和 moveit/planning_scene_interface/planning_scene_interface.h 包含与机器人模型和碰撞对象接口的功能

moveit/task_constructor/task.h 、 moveit/task_constructor/solvers.h 和 moveit/task_constructor/stages.h 包含示例中使用的 MoveIt 任务构造器的不同组件

tf2_geometry_msgs/tf2_geometry_msgs.hpp 和 tf2_eigen/tf2_eigen.hpp 不会在这个初始示例中使用，但在我们为 MoveIt 任务构造器任务添加更多阶段时，它们将用于位姿生成。

#include <rclcpp/rclcpp.hpp>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/solvers.h>
#include <moveit/task_constructor/stages.h>
#if __has_include(<tf2_geometry_msgs/tf2_geometry_msgs.hpp>)
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#else
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#endif
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
#include <tf2_eigen/tf2_eigen.hpp>
#else
#include <tf2_eigen/tf2_eigen.h>
#endif

下一行获取我们新节点的记录器。我们还为 moveit::task_constructor 创建了一个命