MoveIt 2에서 MoveIt Task Constructor를 활용한 객체 습득 및 배치 작업 수행

이 튜토리얼에서는 MoveIt Task Constructor를 사용하여 물체 습득 및 배치 동작을 계획하는 패키지를 생성하는 방법을 안내합니다. MoveIt Task Constructor(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의 핵심 아이디어는 복잡한 로봇 운동 계획 문제는 더 간단한 하위 문제들로 분해될 수 있음입니다. 최상위 계획 문제는 작업(Task)로 정의되고, 하위 문제들은 단계(Stage)로 표현됩니다. 단계들은 임의의 순서와 계층 구조로 배열될 수 있으며, 단계 유형에 따라 제한이 있습니다. 단계들의 연결은 결과 전달 방향에 의해 제약됩니다. 결과 흐름과 관련된 단계에는 세 가지 유형이 있습니다:

**생성기(Generator)**는 인접한 단계들과 독립적으로 결과를 계산하며 양방향으로 전달합니다. 예를 들어, 위치 역학(IK) 샘플러는 접근 및 이탈 동작이 해결책에 따라 달라지는 경우입니다.

**전파기(Propagator)**는 인접한 단계로부터 결과를 받아 하나의 하위 문제를 해결하고 그 결과를 반대쪽 인접 단계로 전달합니다. 구현에 따라 단계는 앞뒤 또는 양방향으로 전달이 가능합니다. 예를 들면, 시작 상태나 목표 상태에서 카르테시안 경로를 계산하는 단계가 있습니다.

**연결기(Connector)**는 결과를 전달하지 않고 인접한 두 상태 사이의 차이를 해결하려고 시도합니다. 예를 들면, 자유 공간 내에서 상태 간의 움직임을 계획하는 것입니다.

단계의 순서 유형 외에도 다양한 계층 유형이 하위 단계들을 캡슐화할 수 있도록 합니다. 하위 단계가 없는 단계는 원시 단계(Primitive Stage)라고 하고, 하위 단계를 포함하는 단계는 컨테이너 단계(Container Stage)라고 합니다. 컨테이너의 종류에는 세 가지가 있습니다:

**래퍼(Wrapper)**는 단일 하위 단계를 감싸고 결과를 수정하거나 필터링합니다. 예를 들어, 특정 제약 조건을 만족하는 하위 단계의 해결책만 허용하는 필터링 래퍼가 있습니다. 또 다른 일반적인 용법은 자세 목표 속성을 포함한 계획 환경에서 역운동학 해답을 생성하는 IK 래퍼 단계입니다.

시퀀스 컨테이너(Sequence Container)는 연속적인 하위 단계들의 집합을 포함하며, 전체적인 엔드투엔드 해결책만 결과로 간주됩니다. 습득 동작은 여러 단계로 구성되는 예시입니다.

병렬 컨테이너(Parallel Container)는 여러 하위 단계를 결합하여 최적의 대안 결과를 전달하거나 백업 해결기를 실행하거나 여러 독립적인 해결책을 통합하는 데 사용됩니다. 예시로는 자유 공간 계획을 위한 대안 계획기 실행, 오른손 또는 왼손으로 물체를 습득하는 백업, 또는 팔과 클램프를 동시에 움직이는 작업 등이 있습니다.

단계는 단순히 운동 계획 문제 해결뿐만 아니라 다양한 상태 변환에도 사용됩니다. 예를 들어 계획 환경 수정 등. 클래스 상속 기능과 결합하면 구조화된 원시 단계 집합만으로 매우 복잡한 행동을 구축할 수 있습니다.

MTC에 대한 자세한 내용은 MoveIt Task Constructor 개념 페이지(https://moveit.picknik.ai/main/doc/concepts/moveit_task_constructor/moveit_task_constructor.html)를 참고하세요.

2. 시작하기

이미 완료하지 않았다면, 시작 가이드 단계를 먼저 진행하세요.

2.1 MoveIt Task Constructor 다운로드

colcon 작업 공간으로 이동하여 MoveIt Task Constructor 소스 코드를 가져옵니다: 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 Task Constructor 패키지는 몇 가지 기본 예제와 습득 및 배치 데모를 포함합니다. 모든 데모를 실행하려면 기본 환경을 시작해야 합니다:

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 Task Constructor로 프로젝트 설정하기

이 섹션에서는 MoveIt Task Constructor를 사용하여 간단한 작업을 구축하는 데 필요한 단계들을 설명합니다.

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>
#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::Task createTask();
  mtc::Task task_;
  rclcpp::Node::SharedPtr node_;
};

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

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-but-set-variable"
  mtc::Stage* current_state_ptr = nullptr;
#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::CartesianPathPlanner>();
  
  ...
}

태그: MoveIt2 MoveIt Task Constructor ros2 Motion Planning robotics

9월 29일 15:40에 게시됨

인기 태그

©2026 괴물 클럽