Bootstrap

三、使用MoveIt Task Constructor完成机械臂拾取与放置

目录

1.MoveIt Task Constructor基本概念

        1.1 生成器（Generator Stage）

        1.2 传播器（Propagator Stage）

        1.3 连接器（Connector Stage）

2.拉取MTC函数源

3.创建功能包

4.配置文件设置

5.编写功能代码

        5.1 定义MTC类

        5.2 创建阶段

         5.3 编写主程序

6.添加阶段

7.编写启动文件

8.结果

1.MoveIt Task Constructor基本概念

         MoveIt Task Constructor即Moveit任务构造器，核心思想是将复杂任务分解为可管理的子任务，每个子任务由一个阶段来表示，这些阶段可以是生成器（generators）、传播器（propagators）或连接器（connectors），可以按照任意顺序和层次排列，但受到结果传递方向的限制。

        1.1 生成器（Generator Stage）

        概念：生成器阶段独立于相邻阶段计算结果，它可以在两个方向上传递结果，即向前和向后。

        作用：通常用于创建可能的解决方案，例如位姿采样和运动规划的初始点。

        应用：例如，逆运动学（IK）采样器就是一个生成器阶段，它生成末端执行器可能的位姿，这些位姿可以作为后续阶段的输入。

        1.2 传播器（Propagator Stage）

        概念：传播器阶段接收一个来自相邻阶段的结果，解决一个子问题，然后将结果传递到相邻阶段。

        作用：基于起始状态或目标状态计算路径。

        应用：例如，计算从当前机器人状态到目标状态的笛卡尔路径，或者根据抓取姿势计算接近和放置姿势。

        1.3 连接器（Connector Stage）

        概念：连接器阶段不传播任何结果，而是尝试弥合两个相邻状态之间的差距。

        作用：连接器阶段用于解决从一个给定状态到另一个状态的过渡问题，通常涉及到运动规划。

        应用：例如，计算从一个抓取姿势到放置姿势的平滑运动路径，或者在避免碰撞的同时从初始位置移动到目标位置。

        1.4 MTC实现抓取任务阶段连接

2.拉取MTC函数源

cd ~/ws_moveit/src
git clone https://github.com/ros-planning/moveit_task_constructor.git -b ros2

3.创建功能包

ros2 pkg create --build-type ament_cmake --node-name mtc_tutorial mtc_tutorial

4.配置文件设置

在功能包的package.xml文件中添加

  <depend>moveit_task_constructor_core</depend>
  <depend>rclcpp</depend>
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

在功能包的CMakeLists.txt文件中添加

find_package(moveit_task_constructor_core REQUIRED)
find_package(rclcpp REQUIRED)


add_executable(mtc_tutorial src/mtc_tutorial.cpp)
ament_target_dependencies(mtc_tutorial moveit_task_constructor_core rclcpp)


install(TARGETS mtc_tutorial
DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})

5.编写功能代码

#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

static const rclcpp::Logger LOGGER = rclcpp::get_logger("mtc_tutorial");
namespace mtc = 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::Task task_;
  rclcpp::Node::SharedPtr node_;
};

rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MTCTaskNode::getNodeBaseInterface()
{
  return node_->get_node_base_interface();
}

MTCTaskNode::MTCTaskNode(const rclcpp::NodeOptions& options)
  : node_{ std::make_shared<rclcpp::Node>("mtc_node", options) }
{
}

void MTCTaskNode::setupPlanningScene()
{
  moveit_msgs::msg::CollisionObject object;
  object.id = "object";
  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 };

  geometry_msgs::msg::Pose pose;
  pose.position.x = 0.5;
  pose.position.y = -0.25;
  pose.orientation.w = 1.0;
  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))
  {
    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);


  mtc::Stage* current_state_ptr = nullptr;  // 将当前状态转发给姿势生成器

  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);

  rclcpp::NodeOptions options;
  options.automatically_declare_parameters_from_overrides(true);

  auto mtc_task_node = std::make_shared<MTCTaskNode>(options);
  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();
  return 0;
}

代码分解阐述：

        5.1 定义MTC类

        定义MTCTaskNode类，并在类中声明成员函数包括构造函数

class MTCTaskNode
{
public:
  MTCTaskNode(const rclcpp::NodeOptions& options);

  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();

  void doTask();

  void setupPlanningScene();

private:
  // Compose an MTC task from a series of stages.
  mtc::Task createTask();
  mtc::Task task_;
  rclcpp::Node::SharedPtr node_;
};

       成员函数实现，定义 getter 函数来获取节点基础接口

rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MTCTaskNode::getNodeBaseInterface()
{
  return node_->get_node_base_interface();
}

        使用指定的选项初始化节点

MTCTaskNode::MTCTaskNode(const rclcpp::NodeOptions& options)
  : node_{ std::make_shared<rclcpp::Node>("mtc_node", options) }
{
}

        此类方法用于设置规划场景，创建一个圆柱体，并设置尺寸和位置。如果放置在工作空间外，规划将失败

void MTCTaskNode::setupPlanningScene()
{
  moveit_msgs::msg::CollisionObject object;
  object.id = "object";
  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 };

  geometry_msgs::msg::Pose pose;
  pose.position.x = 0.5;
  pose.position.y = -0.25;
  object.pose = pose;

  moveit::planning_interface::PlanningSceneInterface psi;
  psi.applyCollisionObject(object);
}

         doTask函数与MTC对象交互。首先创建一个任务，包括设置属性和添加阶段。接下来初始化任务并生成规划，找到5个成功的解后规划停止。使用publishSolution发布第1个解使得其可以在rviz中可视化规划方案，执行规划好的方案并检查执行结果

void MTCTaskNode::doTask()
{
  task_ = createTask();

  try
  {
    task_.init();
  }
  catch (mtc::InitStageException& e)
  {
    RCLCPP_ERROR_STREAM(LOGGER, e);
    return;
  }

  if (!task_.plan(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;
}

        createTask方法创建MTC对象并设置任务属性

mtc::Task MTCTaskNode::createTask()
{
  moveit::task_constructor::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);

        5.2 创建阶段

        这里定义了一个当前状态指针，并创建当前状态阶段，随后将stage_state_current实例地址赋值给状态指针，最后添加阶段到任务

mtc::Stage* current_state_ptr = nullptr;  // 将当前状态传递给抓取姿势生成器
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));

        为了规划任何机器人运动，我们需要指定一个求解器。MoveIt Task Constructor 为求解器提供了三个选项：

  • PipelinePlanner使用 MoveIt 的规划管道，该管道通常结合 OMPL。

  • CartesianPath用于在笛卡尔空间中沿直线移动末端执行器。

  • JointInterpolation是一个简单的规划器，用于在开始和目标联合状态之间进行插值。它通常用于简单的运动，因为它计算速度很快，但不支持复杂的运动。

        此外，还为笛卡尔规划器设置属性参数

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);

        添加一个阶段来移动机器人，创建MoveTo传播器阶段并设置阶段的组和目标，将阶段添加到任务中后返回配置好的任务

//使用关节插值规划器 设置hand_group_name机器人关节组到open目标
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;
}

         5.3 编写主程序

        创建MTCTaskNode节点，创建并启动spin线程来设置规划场景和执行任务

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);

  rclcpp::NodeOptions options;
  options.automatically_declare_parameters_from_overrides(true);

  auto mtc_task_node = std::make_shared<MTCTaskNode>(options);
  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();
  return 0;
}

6.添加阶段

        到目前为止，已经演示了创建和执行一个简单的任务，现在我们将拾取和放置阶段添加到任务中，在张开夹爪之后添加拾取阶段，需要将手臂移动到可以捡起物体的位置。包括传入阶段名称、规划组和规划器。之后为阶段设置超时时间、阶段属性，并将其添加到我们的任务中

auto stage_move_to_pick = std::make_unique<mtc::stages::Connect>(
    "move to pick",
    mtc::stages::Connect::GroupPlannerVector{ { arm_group_name, sampling_planner } });
stage_move_to_pick->setTimeout(5.0);
stage_move_to_pick->properties().configureInitFrom(mtc::Stage::PARENT);
task.add(std::move(stage_move_to_pick));

        创建一个指向MTC阶段对象的指针，稍后使用它来保存一个阶段

mtc::Stage* attach_object_stage =
    nullptr;  // 将附加物体阶段传递到生成放置位姿的生成器阶段

        创建pick object容器阶段，用于组织多个子阶段按顺序执行。并完成主任务属性公开和容器配置属性初始化

{
  auto grasp = std::make_unique<mtc::SerialContainer>("pick object");
  task.properties().exposeTo(grasp->properties(), { "eef", "group", "ik_frame" });
  grasp->properties().configureInitFrom(mtc::Stage::PARENT,
                                        { "eef", "group", "ik_frame" });

        然后，创建一个传播器阶段来接近对象，它接收来自相邻阶段的解，并将该阶段解方案传播到下一个或前一个阶段。该阶段的解方案涉及以直线方式移动末端执行器。在这一程序块中，我们设置阶段属性和移动的最小距离和最大距离。此外，创建消息指示我们想要移动的方向，在这个例子中，是从手部坐标系的Z轴方向，最后将这个阶段添加到串行容器中

{
  auto stage =
      std::make_unique<mtc::stages::MoveRelative>("approach object", cartesian_planner);
  stage->properties().set("marker_ns", "approach_object");
  stage->properties().set("link", hand_frame);
  stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });
  stage->setMinMaxDistance(0.1, 0.15);

  // 设置末端执行器移动方向
  geometry_msgs::msg::Vector3Stamped vec;
  vec.header.frame_id = hand_frame;
  vec.vector.z = 1.0;
  stage->setDirection(vec);
  grasp->insert(std::move(stage));
}

        现在，创建一个阶段来生成抓握姿势。这是一个生成器阶段，因此他计算结果时不考虑它之前和它之后的阶段，第一级也是生成器阶段，需要一个连接器阶段连接此阶段与第一个生成器阶段，在前文已经创建了它。此段代码设置了阶段属性，抓取前的姿势、角度增量和监控阶段。角度增量是用于确定生成的姿势数的阶段属性，MoveIt 任务构造器将尝试从许多不同的方向抓取对象，其方向之间的差异由角度增量指定。增量越小，抓取方向越紧密

{
  auto stage = std::make_unique<mtc::stages::GenerateGraspPose>("generate grasp pose");
  stage->properties().configureInitFrom(mtc::Stage::PARENT);
  stage->properties().set("marker_ns", "grasp_pose");
  stage->setPreGraspPose("open");
  stage->setObject("object");
  stage->setAngleDelta(M_PI / 12);
  stage->setMonitoredStage(current_state_ptr);  // 挂入当前状态

        创建一个组合旋转和平移的变换确定机器人末端执行器相对于世界坐标系的位姿

Eigen::Isometry3d grasp_frame_transform;
Eigen::Quaterniond q = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitX()) *
                      Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitY()) *
                      Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ());
grasp_frame_transform.linear() = q.matrix();
grasp_frame_transform.translation().z() = 0.1;

        配置了一个逆运动学求解器，用于计算机器人末端达到抓取物体所需姿势的关节角度。通过设置最大规划数量、最小规划距离和参考坐标系，可以控制求解过程并优化结果。将逆运动学求解器阶段添加到串行容器

  // 解算 IK
  auto wrapper =
      std::make_unique<mtc::stages::ComputeIK>("grasp pose IK", std::move(stage));
  wrapper->setMaxIKSolutions(8);
  wrapper->setMinSolutionDistance(1.0);
  wrapper->setIKFrame(grasp_frame_transform, hand_frame);
  wrapper->properties().configureInitFrom(mtc::Stage::PARENT, { "eef", "group" });
  wrapper->properties().configureInitFrom(mtc::Stage::INTERFACE, { "target_pose" });
  grasp->insert(std::move(wrapper));
}

        为了抓取物体，必须允许手和物体之间发生碰撞。ModifyPlanningScene 用于修改规划场景中的碰撞设置。最后，将该阶段添加到串行容器

{
  auto stage =
      std::make_unique<mtc::stages::ModifyPlanningScene>("allow collision (hand,object)");
  stage->allowCollisions("object",
                        task.getRobotModel()
                            ->getJointModelGroup(hand_group_name)
                            ->getLinkModelNamesWithCollisionGeometry(),
                        true);
  grasp->insert(std::move(stage));
}

        在允许碰撞的情况下，现在可以关闭末端执行器。这是通过一个阶段完成的，类似于从上面开始的阶段，移动到 SRDF 中定义的close位置

{
  auto stage = std::make_unique<mtc::stages::MoveTo>("close hand", interpolation_planner);
  stage->setGroup(hand_group_name);
  stage->setGoal("close");
  grasp->insert(std::move(stage));
}

        再定义一个将对象连接到手上的阶段，并将当前状态保存到attach_object_stage

{
  auto stage = std::make_unique<mtc::stages::ModifyPlanningScene>("attach object");
  stage->attachObject("object", hand_frame);
  attach_object_stage = stage.get();
  grasp->insert(std::move(stage));
}

        接下来，创建阶段举起物体，类似于approach_object阶段

{
  auto stage =
      std::make_unique<mtc::stages::MoveRelative>("lift object", cartesian_planner);
  stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });
  stage->setMinMaxDistance(0.1, 0.3);
  stage->setIKFrame(hand_frame);
  stage->properties().set("marker_ns", "lift_object");

  // Set upward direction
  geometry_msgs::msg::Vector3Stamped vec;
  vec.header.frame_id = "world";
  vec.vector.z = 1.0;
  stage->setDirection(vec);
  grasp->insert(std::move(stage));
}

        随后，将串行容器添加到任务中

  task.add(std::move(grasp));
}

        现在，定义拾取的阶段已经完成，我们继续定义用于放置对象的阶段。我们从连接两者的阶段开始，因为我们很快将使用生成器阶段来生成放置对象的姿势。

{
  auto stage_move_to_place = std::make_unique<mtc::stages::Connect>(
      "move to place",
      mtc::stages::Connect::GroupPlannerVector{ { arm_group_name, sampling_planner },
                                                { hand_group_name, sampling_planner } });
  stage_move_to_place->setTimeout(5.0);
  stage_move_to_place->properties().configureInitFrom(mtc::Stage::PARENT);
  task.add(std::move(stage_move_to_place));
}

        为放置阶段创建一个串行容器。这与抓取串行容器类似。接下来的阶段将添加到串行容器中，而不是添加到任务中

{
  auto place = std::make_unique<mtc::SerialContainer>("place object");
  task.properties().exposeTo(place->properties(), { "eef", "group", "ik_frame" });
  place->properties().configureInitFrom(mtc::Stage::PARENT,
                                        { "eef", "group", "ik_frame" });

        下一阶段生成用于放置对象的姿势，并计算这些姿势的逆运动学，添加到place串行容器中

{
  // 放置位姿
  auto stage = std::make_unique<mtc::stages::GeneratePlacePose>("generate place pose");
  stage->properties().configureInitFrom(mtc::Stage::PARENT);
  stage->properties().set("marker_ns", "place_pose");
  stage->setObject("object");

  geometry_msgs::msg::PoseStamped target_pose_msg;
  target_pose_msg.header.frame_id = "object";
  target_pose_msg.pose.position.y = 0.5;
  target_pose_msg.pose.orientation.w = 1.0;
  stage->setPose(target_pose_msg);
  stage->setMonitoredStage(attach_object_stage);  // Hook into attach_object_stage

  // 逆运动学解算
  auto wrapper =
      std::make_unique<mtc::stages::ComputeIK>("place pose IK", std::move(stage));
  wrapper->setMaxIKSolutions(2);
  wrapper->setMinSolutionDistance(1.0);
  wrapper->setIKFrame("object");
  wrapper->properties().configureInitFrom(mtc::Stage::PARENT, { "eef", "group" });
  wrapper->properties().configureInitFrom(mtc::Stage::INTERFACE, { "target_pose" });
  place->insert(std::move(wrapper));
}

        打开末端执行器，使用插值规划器

{
  auto stage = std::make_unique<mtc::stages::MoveTo>("open hand", interpolation_planner);
  stage->setGroup(hand_group_name);
  stage->setGoal("open");
  place->insert(std::move(stage));
}

        重新启用与对象的碰撞，因为我们不再需要握住它。这与禁用碰撞几乎完全相同，只是将最后一个参数设置为"false"而不是 ''true''

{
  auto stage =
      std::make_unique<mtc::stages::ModifyPlanningScene>("forbid collision (hand,object)");
  stage->allowCollisions("object",
                        task.getRobotModel()
                            ->getJointModelGroup(hand_group_name)
                            ->getLinkModelNamesWithCollisionGeometry(),
                        false);
  place->insert(std::move(stage));
}

        随后，分离object对象

{
  auto stage = std::make_unique<mtc::stages::ModifyPlanningScene>("detach object");
  stage->detachObject("object", hand_frame);
  place->insert(std::move(stage));
}

        创建了后退动作阶段，用于在放置物体后使机器人末端执行器沿 X 轴负方向移动一段距离，以安全地撤退到一个无碰撞区域

{
  auto stage = std::make_unique<mtc::stages::MoveRelative>("retreat", cartesian_planner);
  stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });
  stage->setMinMaxDistance(0.1, 0.3);
  stage->setIKFrame(hand_frame);
  stage->properties().set("marker_ns", "retreat");

  // 设置后退方向
  geometry_msgs::msg::Vector3Stamped vec;
  vec.header.frame_id = "world";
  vec.vector.x = -0.5;
  stage->setDirection(vec);
  place->insert(std::move(stage));
}

        将place串行容器添加到任务中

  task.add(std::move(place));
}

        最后一步是返回主页：创建返回阶段并将目标姿势传递给它，这是panda SRDF 文件中定义的姿势

{
  auto stage = std::make_unique<mtc::stages::MoveTo>("return home", interpolation_planner);
  stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });
  stage->setGoal("ready");
  task.add(std::move(stage));
}

7.编写启动文件

要运行 MoveIt 任务构造函数节点，我们需要第二个启动文件来使用正确的参数启动可执行文件。加载 URDF、SRDF 和 OMPL 参数，或使用 MoveIt Configs Utils 执行此操作

from launch import LaunchDescription
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilder

def generate_launch_description():
    moveit_config = MoveItConfigsBuilder("moveit_resources_panda").to_dict()

    # MTC Demo node
    pick_place_demo = Node(
        package="mtc_tutorial",
        executable="mtc_tutorial",
        output="screen",
        parameters=[
            moveit_config,
        ],
    )

    return LaunchDescription([pick_place_demo])

        在CMakeLists.txt文件中安装启动文件

install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})

8.结果

        编译抓取功能包，分别启动两个启动文件

ros2 launch moveit2_tutorials mtc_demo.launch.py

        执行抓取功能包的启动文件

ros2 launch mtc_tutorial pick_place_demo.launch.py

        

悦读

道可道，非常道；名可名，非常名。 无名，天地之始，有名，万物之母。 故常无欲，以观其妙，常有欲，以观其徼。 此两者，同出而异名，同谓之玄，玄之又玄，众妙之门。

最新收录
MySQL连接查询语句
深入了解 .deb 软件包：Linux 下的包管理基础
C++类和对象（上） 原创
Django启用国际化支持（2）—实现界面内切换语言：activate()
音频demo：使用opencore-amr、vo-amrwbenc将PCM数据与AMR-WB数据进行编解码
事件mousePressEvent、paintEvent、closeEvent、keyPressEvent】
从高考到程序员，做自己就好
ant-design-vue 图片预览组件，支持旋转，放大缩小，下载 （主要是js完成） 原创
推荐一款非常好用的视频编辑软件：Movavi Video Editor Plus
python怎么读取png_如何在python中读取PNG的字节？
;