ROS2 humble 에서 MoveIt2 Pick and Place Example 연습

ROS2 Humble 환경에서 MoveIt2 Pick and Place Example 을 수행하는 방법에 대한 글입니다.

원문은 아래 글이며 좀 더 자세하게 서술하고 디버깅 과정을 거쳤습니다.

Pick and Place with MoveIt Task Constructor — MoveIt Documentation: Humble documentation

 

먼저 새 예제를 위해 환경을 구성합니다.

cd ~/ws_moveit2/src

git clone https://github.com/ros-planning/moveit_task_constructor.git -b ros2

저는 이전 예제를 하면서 이미 git이 존재했습니다.

 

새 패키지를 만듭니다.

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

 

그럼 mtc_tutorial 폴더에 package.xml 파일이 존재하게 되는데 해당 파일의 내용을 아래 내용으로 완전히 수정합니다.

<?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>

 

CMakeLists.txt 도 다음 코드와 동일하게 복사 붙여넣기로 수정해줍니다.

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

 

 

src 폴더로 들어가 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;
}

 

 

mtc_tutorial 에 launch 폴더를 생성하고 들어가줍니다.

(terminal 에서 mkdir, cd)

 

그리고 mtc_demo.launch.py 라는 이름으로 다음 코드를 복사 붙여넣기 해서 생성해줍니다. (vi)

import os
from launch import LaunchDescription
from launch.actions import ExecuteProcess
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from moveit_configs_utils import MoveItConfigsBuilder

def generate_launch_description():
    # planning_context
    moveit_config = (
        MoveItConfigsBuilder("moveit_resources_panda")
        .robot_description(file_path="config/panda.urdf.xacro")
        .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
        .planning_pipelines(
            pipelines=["ompl", "chomp", "pilz_industrial_motion_planner"]
        )
        .to_moveit_configs()
    )

    # Load  ExecuteTaskSolutionCapability so we can execute found solutions in simulation
    move_group_capabilities = {
        "capabilities": "move_group/ExecuteTaskSolutionCapability"
    }

    # Start the actual move_group node/action server
    run_move_group_node = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        output="screen",
        parameters=[
            moveit_config.to_dict(),
            move_group_capabilities,
        ],
    )

    # RViz
    rviz_config_file = (
        get_package_share_directory("moveit2_tutorials") + "/launch/mtc.rviz"
    )
    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="log",
        arguments=["-d", rviz_config_file],
        parameters=[
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.robot_description_kinematics,
        ],
    )

    # Static TF
    static_tf = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        name="static_transform_publisher",
        output="log",
        arguments=["--frame-id", "world", "--child-frame-id", "panda_link0"],
    )

    # Publish TF
    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        output="both",
        parameters=[
            moveit_config.robot_description,
        ],
    )

    # ros2_control using FakeSystem as hardware
    ros2_controllers_path = os.path.join(
        get_package_share_directory("moveit_resources_panda_moveit_config"),
        "config",
        "ros2_controllers.yaml",
    )
    ros2_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[ros2_controllers_path],
        remappings=[
            ("/controller_manager/robot_description", "/robot_description"),
        ],
        output="both",
    )

    # Load controllers
    load_controllers = []
    for controller in [
        "panda_arm_controller",
        "panda_hand_controller",
        "joint_state_broadcaster",
    ]:
        load_controllers += [
            ExecuteProcess(
                cmd=["ros2 run controller_manager spawner {}".format(controller)],
                shell=True,
                output="screen",
            )
        ]

    return LaunchDescription(
        [
            rviz_node,
            static_tf,
            robot_state_publisher,
            run_move_group_node,
            ros2_control_node,
        ]
        + load_controllers
    )

 

 

추가적으로 pick and place 하는 동작의 launch 파일이 필요합니다.

pick_place_demo.launch.py 파일을 만들고 아래 코드를 그대로 붙여 넣습니다.

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

 

 

다음 코드를 하나씩 수행합니다.

cd ~/ws_moveit2

colcon build --mixin release

source ~/ws_moveit2/install/setup.bash

ros2 launch moveit2_tutorials mtc_demo.launch.py

 

 

다음과 같은 창이 열립니다.

1. MotionPlanning 의 체크 박스 해제
2. Global Options 아래에서 Fixed Frame을 panda_link0 로 설정
3. 창 왼쪽 하단에서 Add 버튼을 클릭합니다.
4. moveit_task_constructor_visualization 항목에서 Motion Planning Tasks 를 선택 하고 확인을 클릭합니다. Motion Planning Tasks디스플레이가 왼쪽 하단에 나타나야 합니다.저는 이미 이렇게 Displays에 이미 존재해서 별다른 작업을 하지 않았습니다.만약 아래쪽에 Motion Planning Tasks 패널이 나타나지 않으면 Panels 메뉴에서 Motion Planning Tasks를 선택합니다.

   5. Displays의 Motion Planning Tasks 에서 Task Solution Topic 을 /solution 으로 설정

 

이제 새 터미널을 열어서 다음 명령을 입력합니다.

cd ~/ws_moveit2

ros2 launch mtc_tutorial pick_place_demo.launch.py

 

위와 같이 원통이 하나 추가 되고

Motion Planning Tasks에 동작이 추가된 것이 확인 가능합니다.

Display 의 Motion Planning Tasks에서 Loop Animation을 선택하면 아래와 같이 어떤 동작인지 확인하실 수 있습니다.

 

 

그리퍼를 작동시키는 동작입니다.

그럼 이제 원통을 집을 수 있게 동작을 추가해보겠습니다.

 

mtc_tutorial.cpp 를 열고

136번째 line (task.add(std::move(stage_open_hand));) 이후로 동작을 추가하면 됩니다.

 

아래 내용을 추가합니다.

설명은 아래에서 전체 코드 설명을 하겠습니다.

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

  // clang-format off
  mtc::Stage* attach_object_stage =
      nullptr;  // Forward attach_object_stage to place pose generator
  // clang-format on

  // This is an example of SerialContainer usage. It's not strictly needed here.
  // In fact, `task` itself is a SerialContainer by default.
  {
    auto grasp = std::make_unique<mtc::SerialContainer>("pick object");
    task.properties().exposeTo(grasp->properties(), { "eef", "group", "ik_frame" });
    // clang-format off
    grasp->properties().configureInitFrom(mtc::Stage::PARENT,
                                          { "eef", "group", "ik_frame" });
    // clang-format on

    {
      // clang-format off
      auto stage =
          std::make_unique<mtc::stages::MoveRelative>("approach object", cartesian_planner);
      // clang-format on
      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));
    }

    /****************************************************
  ---- *               Generate Grasp Pose                *
     ***************************************************/
    {
      // 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

      // This is the transform from the object frame to the end-effector frame
      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;

      // Compute IK
      // clang-format off
      auto wrapper =
          std::make_unique<mtc::stages::ComputeIK>("grasp pose IK", std::move(stage));
      // clang-format on
      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));
    }

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

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

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

    {
      // clang-format off
      auto stage =
          std::make_unique<mtc::stages::MoveRelative>("lift object", cartesian_planner);
      // clang-format on
      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));
  }

  {
    // clang-format off
    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 } });
    // clang-format on
    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" });
    // clang-format off
    place->properties().configureInitFrom(mtc::Stage::PARENT,
                                          { "eef", "group", "ik_frame" });
    // clang-format on

    /****************************************************
  ---- *               Generate Place Pose                *
     ***************************************************/
    {
      // 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
      // clang-format off
      auto wrapper =
          std::make_unique<mtc::stages::ComputeIK>("place pose IK", std::move(stage));
      // clang-format on
      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));
    }

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

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

    {
      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));
    }
    task.add(std::move(place));
  }

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

 

그리고 터미널을 새로 열어 하나에는 다음 명령어를 입력합니다.

cd ~/ws_moveit2

colcon build --mixin release

source ~/ws_moveit2/install/setup.bash

ros2 launch moveit2_tutorials mtc_demo.launch.py

 

그리고 터미널을 하나 더 열어 다음 명령어를 입력합니다.

cd ~/ws_moveit2

ros2 launch mtc_tutorial pick_place_demo.launch.py

 

그러면 다음과 같은 동작 애니메이션을 확인하실 수 있습니다.

Loop 옵션을 걸면 계속 확인할 수 있습니다.

 

 

그럼 어떻게 코드로 이것이 가능한지 확인해보겠습니다.

기본 MoveIt 작업 생성자 기능을 포함할 클래스를 정의하는 것부터 시작합니다.

또한, 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 함수를 정의합니다.

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

 

이 클래스 메소드는 예제에서 사용되는 계획 장면을 설정하는 데 사용됩니다.

지정된 치수와 지정된 위치를 갖는 원통을 생성합니다 .

여기에 있는 숫자를 변경하여 원통의 크기를 조정하고 이동할 수 있습니다.

실린더를 로봇이 닿지 않는 곳으로 옮기면 Planning에 실패합니다.

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 작업 개체의 인터페이스가 되어줍니다.

먼저 일부 속성 설정 및 단계 추가를 포함하는 작업을 생성합니다.

이에 대해서는 createTask 함수 정의에서 더 자세히 논의하겠습니다.

다음으로 task.init() 작업을 초기화하고 task.plan(5) 계획을 생성하며,

5개의 성공적인 계획이 발견되면 중지됩니다.

그 다음 줄은 RViz에서 시각화할 solution을 publish 합니다.

마지막으로 task.execute()계획을 실행합니다.

실행은 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**;
}

 

위에서 언급했듯이 이 함수는 MoveIt Task Constructor 객체를 생성하고,

일부 초기 속성을 설정합니다.

이 경우 작업 이름을 "demo_task"로 설정하고,

로봇 모델을 로드하고, 프레임의 이름을 정의하고,

해당 프레임의 속성을 설정합니다.

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

 

이제 노드에 Example 을 위한 단계를 추가합니다.

current_state_ptr 은 nullptr로 설정됩니다.

이는 특정 시나리오에서 단계 정보를 재사용할 수 있도록 단계에 대한 포인터를 생성합니다.

이 줄은 현재 사용되지 않지만 나중에 작업에 더 많은 단계가 추가되면 사용됩니다.

다음으로, current_state Stage를 만들고 이를 작업에 추가합니다.

이렇게 하면 로봇이 현재 상태에서 시작됩니다.

이제 스테이지를 만들었으므로 나중에 사용할 수 있도록

Current State Stage에 대한 포인터 current_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));

 

로봇 동작을 계획하려면 solver를 지정해야 합니다.

MoveIt Task Constructor에는 solver에 대한 세 가지 옵션이 있습니다.

1. PipelinePlanner 일반적으로 기본값이 OMPL인 MoveIt의 planning pipeline을 사용합니다.
2. CartesianPath Cartesian space에서 end effector를 직선으로 이동하는 데 사용됩니다.
3. JointInterpolation빠르게 계산되지만 복잡한 동작을 지원하지 않으므로 일반적으로 간단한 동작에 사용됩니다. 시작 관절 상태와 목표 관절 상태 사이를 interpolate하는 간단한 플래너입니다.

 

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

 

이제 플래너를 추가했으므로 로봇을 움직일 Stage를 추가할 수 있습니다.

다음 줄에서는 MoveTo Stage(a propagator stage)를 사용합니다.

손을 여는 것은 비교적 간단한 동작이므로 joint interpolation planner를 사용할 수 있습니다.

이 단계에서는 Panda 로봇에 대해 SRDF 에 정의된 포즈인 "open hand" 포즈로의 전환을 planning 합니다.

작업을 반환하고 createTask() 함수로 마무리합니다.

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

 

물체를 집을 수 있는 위치로 팔을 움직여야 합니다.

이 작업은 이름에서 알 수 있듯이 Connect Stage를 통해 수행됩니다 .

이는 Stage 전후의 결과를 연결하려고 시도한다는 의미입니다.

이 단계는 planning group과 planner를 지정하는 이름 move_to_pick,

및 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 Stage 객체에 대한 포인터를 생성하고

nullptr로 설정합니다.

나중에 이를 사용하여 Stage를 저장하겠습니다.

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

 

다음 코드 블록은 SerialContainer 입니다.

이는 작업에 추가할 수 있고 여러 하위 단계를 보유할 수 있는 컨테이너입니다.

이 경우 picking action과 관련된 단계를 포함하는 serial container를 만듭니다.

작업에 단계를 추가하는 대신 serial container에 관련 단계를 추가하였습니다.

새 serial container의 상위 작업에서 작업 속성 exposeTo을 선언하고

이를 초기화하기 위해 configureInitFrom()을 사용합니다 .

이를 통해 contained stage가 이러한 속성에 액세스할 수 있습니다.

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

 

그런 다음 물체에 접근하기 위한 Stage를 만듭니다.

이 단계는 현재 위치에서 상대적인 이동을 지정할 수 있는 MoveRelative 단계입니다.

MoveRelative propagator stage: 인접한 Stage로부터 솔루션을 수신하여 다음 또는 이전 Stage로 전파합니다.

cartesian_planner를 사용하면 end effector를 직선으로 이동하는 것과 관련된 솔루션을 찾습니다.

속성을 설정하고 이동할 최소 및 최대 거리를 설정합니다.

이제 이동하려는 방향(이 경우 손 프레임의 Z 방향)을 나타내는 Vector3Stamped 메시지를 만듭니다.

마지막으로 이 단계를 Serial Container에 추가합니다.

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

 

이제 물건을 집는 자세를 생성하기 위한 Stage를 만듭니다.

이는 generator stage이므로 이전 stage와 이후 stage에 관계없이 결과를 계산합니다.

첫 번째 stage CurrentState는 generator stage이기도 합니다.

첫 번째 stage와 이 stage를 연결하려면 위에서 이미 생성한 연결 stage를 사용해야 합니다.

이 코드는 stage속성을 설정하고, 잡기 전 포즈, Angle delta 및 모니터링되는 stage를 설정합니다.

Angle delta는 GenerateGraspPose stage의 속성으로, 생성할 포즈 수를 결정하는 데 사용됩니다.

Solution을 생성할 때 MoveIt 작업 생성자는 Angle delta로 지정된 방향 간의 차이를 사용하여 다양한 방향에서 개체를 파악하려고 시도합니다.

Delta가 작을수록 파지 방향이 더 가까워집니다.

현재 Stage를 정의할 때 객체 포즈 및 모양에 대한 정보를 inverse kinematic solver에 전달하는 데 사용되는 current_state_ptr를 설정합니다.

이 Stage는 이전처럼 Serial Container에 직접 추가되지 않습니다.

생성된 포즈에 대해 inverse kinematic을 계속 수행해야 하기 때문입니다.

{
  *// 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*

 

위에서 생성된 포즈에 대한 inverse kinematic을 계산하기 전에 먼저 프레임을 정의해야 합니다.

이는 geometry_msgs 으로 부터의 PoseStamped 메시지를 통해 수행할 수 있으며

이 경우 Eigen transformation matrix과 관련 링크 이름을 사용하여 변환을 정의합니다.

여기서는 transformation matrix을 정의합니다.

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 Stage를 생성 하고 위에서 정의한 generate grasp pose Stage 와 함께 이름을 generate pose IK 으로 지정합니다.

일부 로봇에는 주어진 포즈에 대해 여러 inverse kinematic solution이 있습니다.

일단 최대 8개까지로 해결해야 하는 솔루션의 양에 대한 제한을 설정했습니다.

또한, 각 solution이 얼마나 달라야 하는지에 대한 임계값인 minimum solution distance도 설정합니다.

Solution의 위치가 이전 Solution과 너무 유사하면 유효하지 않은 것으로 표시됩니다.

다음으로 몇 가지 추가 속성을 구성하고 Serial Container에 ComputeIK단계를 추가합니다.

  *// 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 Stage를 통해 이루어질 수 있습니다 .

이 allowCollisions 기능을 사용하면 비활성화할 충돌을 지정할 수 있습니다.

allowCollisions 는 container의 이름으로 함께 사용할 수 있으므로

hand 그룹에서 collision geometry가 있는 링크의 모든 이름을 얻을 때 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));
}

 

충돌이 허용되면 이제 Hand를 닫을 수 있습니다.

이 작업은 SRDF에 정의된 close 위치로 이동한다는 점을 제외하면

위의 open hand Stage 유사하게 MoveTo Stage를 통해 수행됩니다.

{
  **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 Stage를 사용합니다 .

이번에는 attachObject를 사용하여 개체를 손에 연결합니다

current_state_ptr 에서 수행한 것과 유사하게

나중에 개체에 대한 place pose를 생성할 때 사용할 수 있도록 이 단계에 대한 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));
}

 

다음으로, approach_object Stage와 유사하게 MoveRelative Stage로 물체를 들어올립니다

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

 

이로써 object를 Picking 하는데 필요한 모든 Stage가 완성됩니다.

이제 모든 하위 Stage가 포함된 Serial Container를 작업에 추가합니다.

패키지를 있는 그대로 빌드하면 로봇이 물체를 집어 올리려는 Plan을 볼 수 있습니다.

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

 

이제 picking을 정의하는 Stage가 완료되었으므로 Placing Stage를 정의하는 Stage로 넘어갑니다.

곧 Connect Stage를 사용하여 Placing을 위한 포즈를 생성하게 되므로 두 가지를 연결하는 Stage 부터 시작합니다 .

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

 

또한, place stage를 위한 serial container도 만듭니다.

이는 Picking stage의 serial container 와 유사하게 수행됩니다.

다음 stage는 작업이 아닌 serial container에 추가됩니다.

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

 

다음 단계에서는 object 를 placing 하기 위한 포즈를 생성하고,

이러한 포즈에 대한 inverse kinematic을 계산합니다.

이는 pick serial container에서 그립 포즈를 생성하는 Stage와 유사합니다.

geometry_msgs의 PoseStamped 메시지를 사용하여 Object를 placing 하려는 포즈를 지정합니다.

이 경우, y = 0.5를 선택합니다.

그런 다음 setPose를 사용하여 target pose를 Stage에 전달합니다.

다음으로 setMonitoredStage를 사용하여 이전에 만든 attach_object stage에 대한 ptr를 전달합니다.

이를 통해 Stage는 객체가 어떻게 attach 되어 있는지 알 수 있습니다.

그런 다음 ComputeIK Stage를 생성하고 GeneratePlacePose Stage를 전달합니다.

나머지는 위의 Pick up Stage와 동일한 로직을 따라갑니다.

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

 

이제 object를 배치할 준비가 되었으므로 MoveTo Stage와 joint interpolation planner를 사용하여 hand를 open 합니다.

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

 

또한 더 이상 Object를 붙잡을 필요가 없으므로 Object와의 충돌을 다시 활성화할 수 있습니다.

이는 마지막 인수를 true가 아닌 false로 설정하는 것을 제외하면 충돌을 비활성화하는 것과 거의 똑같은 방식으로 수행됩니다.

{
  **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 Stage를 사용하여 물체로부터 떨어지는데 ,

이는 approach object Stage와 lift object Stage와 유사하게 수행됩니다 .

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

 

place serial container를 완성하고 이를 작업에 추가합니다.

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

 

마지막 Stage는 원래 위치로 돌아가는 것입니다.

MoveTo Stage를 사용하여 Panda SRDF에 정의된 포즈인 ready 포즈를 전달합니다 .

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

 

task를 반환합니다.

  *// Stages all added to the task above this line*  
  **return** task;
}

 

마지막으로 main 함수입니다.

다음 줄은 위에 정의된 클래스를 사용하여 노드를 생성하고,

클래스 메서드를 호출하여 basic MTC 작업을 설정하고 실행합니다.

이 예에서는 RViz에서 Solution을 검사하기 위해,

작업 실행이 완료된 후에 executor를 취소하지 않습니다.

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

 

도움이 되셨길 바라면서 글 마치겠습니다.