【ROS2】实操Movei2！路径规划

标签：std task pose object 实操 Movei2 mtc ROS2 stage

 

本篇文章需要将前面几个关于moveit的内容手敲以便，熟悉基本流程再来实操！ 

一、规避障碍物

1.1 Add include for Planning Scene Interface 添加引用头文件：

#include <moveit/planning_scene_interface/planning_scene_interface.h>

1.2 Change the Target Pose 重新设定目标位姿：

// Create collision object for the robot to avoid
auto const collision_object = [frame_id =
                                 move_group_interface.getPlanningFrame()] {
  moveit_msgs::msg::CollisionObject collision_object;
  collision_object.header.frame_id = frame_id;
  collision_object.id = "box1";
  shape_msgs::msg::SolidPrimitive primitive;

  // Define the size of the box in meters
  primitive.type = primitive.BOX;
  primitive.dimensions.resize(3);
  primitive.dimensions[primitive.BOX_X] = 0.5;
  primitive.dimensions[primitive.BOX_Y] = 0.1;
  primitive.dimensions[primitive.BOX_Z] = 0.5;

  // Define the pose of the box (relative to the frame_id)
  geometry_msgs::msg::Pose box_pose;
  box_pose.orientation.w = 1.0;
  box_pose.position.x = 0.2;
  box_pose.position.y = 0.2;
  box_pose.position.z = 0.25;

  collision_object.primitives.push_back(primitive);
  collision_object.primitive_poses.push_back(box_pose);
  collision_object.operation = collision_object.ADD;

  return collision_object;
}();

1.3 Create a Collision Object：在场景中建立一个障碍物模型。如果机器人连接有感知设备，则这一信息应当由感知设备提供。这段代码最后的ADD将这一模型加入碰撞检测的场景中。这段代码应该位于设定目标位姿和进行运动规划的代码之间。

  // Create collision object for the robot to avoid
  // 创建一个障碍物：
 auto const collision_object = [frame_id =
 move_group_interface.getPlanningFrame()] {
 moveit_msgs::msg::CollisionObject collision_object;
 collision_object.header.frame_id = frame_id;
 collision_object.id = "box1";
 shape_msgs::msg::SolidPrimitive primitive;

    // Define the size of the box in meters
 primitive.type = primitive.BOX;
 primitive.dimensions.resize(3);
 primitive.dimensions[primitive.BOX_X] = 0.5;
 primitive.dimensions[primitive.BOX_Y] = 0.1;
 primitive.dimensions[primitive.BOX_Z] = 0.5;

    // Define the pose of the box (relative to the frame_id)
 geometry_msgs::msg::Pose box_pose;
 box_pose.orientation.w = 1.0;
 box_pose.position.x = 0.2;
 box_pose.position.y = 0.2;
 box_pose.position.z = 0.25;

 collision_object.primitives.push_back(primitive);
 collision_object.primitive_poses.push_back(box_pose);
 collision_object.operation = collision_object.ADD;  
    // the object getting added to the collision scene

 return collision_object;
  }();

1.4 Add the Object to the Planning Scene 将以上碰撞场景模型加入到运动规划场景中：

// Add the collision object to the scene
  // 添加碰撞障碍物到场景中
 moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
 planning_scene_interface.applyCollisionObject(collision_object);

1.5 Run the Program and Observe the Change 按照之前Visualizing RViz的步骤进行操作：

首先，在 termianl0 中构建程序：

cd ~/ws_moveit2
source /opt/ros/humble/setup.bash
colcon build --mixin debug

然后，在 terminal1 中打开RViz：

cd ~/ws_moveit2
source install/setup.bash
ros2 launch moveit2_tutorials demo.launch.py

取消其中的MotionPlanning；

增加MarkerArray，设置其节点为/rviz_visual_tools；

增加一个RVizVisualToolGUI。

最后，在terminal 2 中运行场景设置和运动规划程序：

cd ~/ws_moveit2
source install/setup.bash
ros2 run hello_moveit hello_moveit

可以看到RViz场景中加入了障碍物，并在这一前提下进行了运动规划并进行执行：

二、抓取和放置

2.1 下载 MoveIt 任务构造函数

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

 创建新包：

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

添加依赖：

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>mtc_tutorial</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="youremail@domain.com">user</maintainer>
<license>TODO: License declaration</license>

<buildtool_depend>ament_cmake</buildtool_depend>

<depend>moveit_task_constructor_core</depend>
<depend>rclcpp</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

<export>
    <build_type>ament_cmake</build_type>
</export>
</package>

cmake_minimum_required(VERSION 3.8)
project(mtc_tutorial)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(moveit_task_constructor_core REQUIRED)
find_package(rclcpp REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)

add_executable(mtc_tutorial src/mtc_tutorial.cpp)
ament_target_dependencies(mtc_tutorial moveit_task_constructor_core rclcpp)
target_include_directories(mtc_tutorial PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_compile_features(mtc_tutorial PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17

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

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# uncomment the line when a copyright and license is not present in all source files
#set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()

ament_package()

2.2 使用 MoveIt 任务构造函数设置项目

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

#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:
  // Compose an MTC task from a series of stages.
  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";

  // Set task properties
  task.setProperty("group", arm_group_name);
  task.setProperty("eef", hand_group_name);
  task.setProperty("ik_frame", hand_frame);

// Disable warnings for this line, as it's a variable that's set but not used in this example
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-but-set-variable"
  mtc::Stage* current_state_ptr = nullptr;  // Forward current_state on to grasp pose generator
#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);

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

代码解释：

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

rclcpp/rclcpp.hpp包括核心 ROS2 功能
moveit/planning_scene/planning_scene.h并包括与 Robot 模型和碰撞对象交互的功能moveit/planning_scene_interface/planning_scene_interface.h
moveit/task_constructor/task.h、 和 ，并包括示例中使用的 MoveIt 任务构造函数的不同组件moveit/task_constructor/solvers.hmoveit/task_constructor/stages.h
tf2_geometry_msgs/tf2_geometry_msgs.hpp，并且不会在此初始示例中使用，但当我们向 MoveIt Task Constructor 任务添加更多阶段时，它们将用于姿势生成。tf2_eigen/tf2_eigen.hpp

        下一行获取新节点的 Logger。为了方便起见，我们还创建了一个命名空间别名。moveit::task_constructor

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

        首先定义一个类，该类将包含主要的 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:
  // Compose an MTC task from a series of stages.
  mtc::Task createTask();
  mtc::Task task_;
  rclcpp::Node::SharedPtr node_;
};

        这几行定义了一个 getter 函数来获取 node 基接口，该接口稍后将用于 executor。

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) }
{
}

        此类方法用于设置示例中使用的规划场景。它将创建一个尺寸由 和 和 指定位置的圆柱体。您可以尝试更改这些数字以调整圆柱体的大小并四处移动。如果将圆柱体移到机器人够不到的地方，规划就会失败。object.primitives[0].dimensionspose.position.zpose.position.x

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

        此函数与 MoveIt Task Constructor 任务对象交互。它首先创建一个任务，其中包括设置一些属性和添加阶段。这将在函数定义中进一步讨论。接下来，初始化任务并生成计划，在找到 5 个成功的计划后停止。下一行发布要在 RViz 中可视化的解决方案 - 如果您不关心可视化，可以删除此行。最后，执行计划。执行通过与 RViz 插件的操作服务器接口进行。createTasktask.init()task.plan(5)task.execute()

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

        如上所述，此函数创建一个 MoveIt Task Constructor 对象并设置一些初始属性。在本例中，我们将任务名称设置为 “demo_task”，加载机器人模型，定义一些有用的帧的名称，并使用 将这些帧名称设置为任务的属性。接下来的几个代码块将填充此函数体。task.setProperty(property_name, value)

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

  // Set task properties
  task.setProperty("group", arm_group_name);
  task.setProperty("eef", hand_group_name);
  task.setProperty("ik_frame", hand_frame);

        现在，我们将示例阶段添加到节点。第一行设置为 ;这将创建一个指向 stage 的指针，以便我们可以在特定场景中重用 stage 信息。此行目前未使用，但稍后在向任务添加更多阶段时将使用。接下来，我们创建一个阶段（生成器阶段）并将其添加到我们的任务中 - 这将以当前状态启动机器人。现在我们已经创建了阶段，我们在 中保存指向它的指针以备后用。current_state_ptrnullptrcurrent_stateCurrentStatecurrent_state_ptr

mtc::Stage* current_state_ptr = nullptr;  // Forward current_state on to grasp pose generator
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是一个简单的规划器，它在 Start 和 Goal Joint 状态之间进行插值。它通常用于简单运动，因为它计算速度快，但不支持复杂运动。

        我们还为 Cartesian planner 设置了一些特定的属性。

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

        现在我们已经添加了 planners，我们可以添加一个将移动机器人的舞台。以下行使用阶段 （propagator stage）。由于开牌是一个相对简单的动作，我们可以使用 joint interpolation planner。此阶段计划移动到“张开手”姿势，这是在 SRDF 中为 panda 机器人定义的命名姿势。我们返回任务并使用 createTask（） 函数完成。MoveTo

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

        最后，我们有：以下几行使用上面定义的类创建一个节点，并调用类方法来设置和执行基本的 MTC 任务。在此示例中，任务执行完毕后，我们不会取消执行程序，以保持节点处于活动状态以检查 RViz 中的解决方案。main

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

2.3 运行 Demo

        需要创建一个启动文件来启动move_group、ros2_control、static_tf、robot_state_publisher和rviz等节点。同时，还需要一个启动文件来启动mtc_tutorial可执行文件，并加载URDF、SRDF和OMPL参数。

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

将这个文件保存为pick_place_demo.launch.py，并确保在CMakeLists.txt中添加以下行以正确安装启动文件：

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

构建工作空间：

cd ~/ws_moveit2
colcon build --mixin release
source ~/ws_moveit2/install/setup.bash

启动第一个启动文件：

ros2 launch moveit2_tutorials mtc_demo.launch.py

启动mtc_tutorial节点：

ros2 launch mtc_tutorial pick_place_demo.launch.py

您应该看到机械臂执行任务，使用单个阶段打开手部，其前面是绿色的圆柱体。它应如下所示：

如果您还没有创建自己的包，但仍想查看它是什么样子，您可以从教程中启动此文件：

ros2 launch moveit2_tutorials mtc_demo_minimal.launch.py

2.4 添加 pick-and-place 任务

        我们需要移动机械臂到可以抓取物体的位置。这通过Connect阶段实现，它尝试连接前一个阶段和后一个阶段的结果。这个阶段初始化时需要一个名称和一个GroupPlannerVector，用于指定规划组和规划器。然后，设置阶段的超时时间、属性，并将其添加到任务中。

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

接下来，创建一个指向MoveIt Task Constructor阶段对象的指针，并将其设置为nullptr，稍后将用于保存阶段。

mtc::Stage* attach_object_stage = nullptr;  // Forward attach_object_stage to place pose generator

创建一个SerialContainer，它可以添加到任务中，并可以包含多个子阶段。在这个例子中，创建一个包含抓取动作相关阶段的SerialContainer。使用exposeTo声明父任务的属性，并使用configureInitFrom初始化它们，以便子阶段可以访问这些属性。

{
  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" });

创建一个MoveRelative阶段，用于接近物体。这个阶段是一个传播阶段，它接收相邻阶段的解决方案并传播到下一个或上一个阶段。使用cartesian_planner找到一个涉及末端执行器直线运动的解决方案。设置属性，并设置最小和最大移动距离。然后，创建一个Vector3Stamped消息，指示我们想要移动的方向——在这个例子中，是从手部框架的Z方向。最后，将这个阶段添加到SerialContainer中。

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

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

创建一个阶段来生成抓取姿态。这是一个生成阶段，因此它在不考虑前一个和后一个阶段的情况下计算结果。第一个阶段CurrentState也是一个生成阶段——为了连接第一个阶段和这个阶段，必须使用连接阶段，我们已经在上面创建了。这段代码设置阶段属性，设置抓取前的姿态、角度增量和监控阶段。角度增量是GenerateGraspPose阶段的一个属性，用于确定要生成的姿态数量；在生成解决方案时，MoveIt Task Constructor将尝试从许多不同方向抓取物体，方向之间的差异由角度增量指定。角度增量越小，抓取方向就越接近。在定义当前阶段时，我们设置了current_state_ptr，现在用于将物体姿态和形状的信息传递给逆运动学求解器。这个阶段不会像以前那样直接添加到SerialContainer中，因为我们还需要对它生成的姿态进行逆运动学计算。

{
  // Sample grasp pose
  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);  // Hook into current state

在计算上面生成的姿态的逆运动学之前，我们首先需要定义框架。这可以通过geometry_msgs中的PoseStamped消息完成，或者在这个例子中，我们使用Eigen变换矩阵和相关链接的名称来定义变换矩阵。

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;

现在，创建ComputeIK阶段，并给它命名为grasp pose IK，以及上面定义的generate grasp pose阶段。有些机器人对于给定姿态有多个逆运动学解——我们设置最多解算8个解。我们还设置最小解距离，这是对解必须不同的一个阈值：如果解的关节位置与前一个解太相似，它将被标记为无效。接下来，配置一些额外的属性，并将ComputeIK阶段添加到SerialContainer中。

  // Compute 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阶段完成。allowCollisions函数让我们指定要禁用的碰撞。allowCollisions可以与容器名称一起使用，因此我们可以使用getLinkModelNamesWithCollisionGeometry获取手部组中所有具有碰撞几何形状的链接名称。

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

允许碰撞后，我们现在可以关闭手部。这通过MoveTo阶段完成，类似于上面的open hand阶段，只是移动到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));
}

现在再次使用ModifyPlanningScene阶段，这次是将物体附加到手部，使用attachObject。与current_state_ptr类似，我们获取这个阶段的指针，稍后在生成物体放置姿态时使用。

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

接下来，使用MoveRelative阶段提起物体，类似于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));
}

至此，我们已经完成了抓取物体所需的所有阶段。现在，将包含所有子阶段的SerialContainer添加到任务中。如果按照当前包构建，可以看到机器人计划抓取物体。

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

2.5 放置阶段

现在抓取阶段已经完成，我们继续定义放置物体的阶段。我们首先使用Connect阶段连接两者，因为我们很快将使用生成阶段来生成放置物体的姿态。

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

我们还为放置阶段创建了一个SerialContainer。这与抓取SerialContainer的创建方式类似。接下来的阶段将添加到SerialContainer而不是任务中。

{
  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" });

接下来的阶段生成用于放置物体的姿态，并为这些姿态计算逆运动学——它与抓取SerialContainer中的generate grasp pose阶段有些相似。我们首先创建一个阶段来生成姿态并继承任务属性。我们使用geometry_msgs中的PoseStamped消息指定我们想要放置物体的位置——在这个例子中，我们在"object"框架中选择y = 0.5。然后，我们使用setPose将目标姿态传递给阶段。接下来，我们使用setMonitoredStage并传递attach_object阶段的指针。这允许阶段知道物体是如何附加的。然后，我们创建一个ComputeIK阶段并将我们的GeneratePlacePose阶段传递给它——其余的逻辑与抓取阶段相同。

{
  // Sample place pose
  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

  // Compute IK
  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));
}

现在我们准备放置物体，我们使用MoveTo阶段和关节插值规划器打开手部。

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

现在我们可以重新启用与物体的碰撞，因为我们不再需要持有它。这通过allowCollisions几乎完全相同的方式完成，只是将最后一个参数设置为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));
}

现在，我们可以使用detachObject分离物体。

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

我们使用MoveRelative阶段从物体后退，这与approach object和lift object阶段类似。

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

  // Set retreat direction
  geometry_msgs::msg::Vector3Stamped vec;
  vec.header.frame_id = "world";
  vec.vector.x = -0.5;
  stage->setDirection(vec);
  place->insert(std::move(stage));
}

我们完成放置SerialContainer并将其添加到任务中。

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

最后一步是返回初始位置：我们使用MoveTo阶段并传递目标姿态ready，这是在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));
}

所有这些阶段都应该添加到这些行的上方。

  // Stages all added to the task above this line

  return task;
}

现在已经使用MoveIt Task Constructor定义了一个抓取和放置任务！

2.6 运行

在第一个终端中：

ros2 launch moveit2_tutorials mtc_demo.launch.py

在第二个终端中：

ros2 launch moveit2_tutorials pick_place_demo.launch.py

 再具体问题具体分析！

 

标签：std,task,pose,object,实操,Movei2,mtc,ROS2,stage
From： https://blog.csdn.net/qq_68192341/article/details/145113687