Pick and Place / minimal.cpp

Create a New Package

ros2 pkg create --build-type ament_cmake --node-name 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()

MoveIt Task Constructor

include

• rclcpp/rclcpp.hpp includes core ROS2 functionality
• moveit/planning_scene/planning_scene.h and moveit/planning_scene_interface/planning_scene_interface.h includes functionality to interface with the robot model and collision objects
• moveit/task_constructor/task.h, moveit/task_constructor/solvers.h, and moveit/task_constructor/stages.h include different components of MoveIt Task Constructor that are used in the example
• tf2_geometry_msgs/tf2_geometry_msgs.hpp and tf2_eigen/tf2_eigen.hpp won’t be used in this initial example, but they will be used for pose generation when we add more stages to the MoveIt Task Constructor task.

    #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

logger

The next line gets a logger for your new node. We also create a namespace alias for moveit::task_constructor for convenience.

    static const rclcpp::Logger LOGGER = rclcpp::get_logger("mtc_tutorial");
    namespace mtc = moveit::task_constructor;

class

We start by defining a class that will contain the main MoveIt Task Constructor functionality. We also declare the MoveIt Task Constructor task object (task_) as a member variable for our class: this isn’t strictly necessary for a given application, but it helps save the task for later visualization purposes. We will explore each function individually below.

    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

These lines define a getter function to get the node base interface, which will be used for the executor later.

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

node

These next lines initialize the node with specified options.

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

PlanningScene

This class method is used to set up the planning scene that is used in the example. It creates a cylinder with dimensions specified by object.primitives[0].dimensions and position specified by pose.position.z and pose.position.x. You can try changing these numbers to resize and move the cylinder around. If you move the cylinder out of the robot’s reach, planning will fail.

    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

This function interfaces with the MoveIt Task Constructor task object. It first creates a task, which includes setting some properties and adding stages. This will be discussed further in the createTask function definition. Next, task.init() initializes the task and task.plan(5) generates a plan, stopping after 5 successful plans are found. The next line publishes the solution to be visualized in RViz - this line can be removed if you don’t care for visualization. Finally, task.execute() executes the plan. Execution occurs via an action server interface with the RViz plugin.

    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

As mentioned above, this function creates a MoveIt Task Constructor object and sets some initial properties. In this case, we set the task name to “demo_task”, load the robot model, define the names of some useful frames, and set those frame names as properties of the task with task.setProperty(property_name, value). The next few code blocks will fill out this function body.

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

AddStage

Now, we add an example stage to the node. The first line sets current_state_ptr to nullptr; this creates a pointer to a stage such that we can re-use stage information in specific scenarios. This line is not used at this moment, but will be used later when more stages are added to the task. Next, we make a current_state stage (a generator stage) and add it to our task - this starts the robot off in its current state. Now that we’ve created the CurrentState stage, we save a pointer to it in the current_state_ptr for later use.

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

solvers

In order to plan any robot motions, we need to specify a solver. MoveIt Task Constructor has three options for solvers:

• PipelinePlanner uses MoveIt’s planning pipeline, which typically defaults to OMPL.

  auto sampling_planner = std::make_shared<mtc::solvers::PipelinePlanner>(node_);
  

• CartesianPath is used to move the end effector in a straight line in Cartesian space.

  auto cartesian_planner = std::make_shared<mtc::solvers::CartesianPath>();
  

• JointInterpolation is a simple planner that interpolates between the start and goal joint states. It is typically used for simple motions as it computes quickly but doesn’t support complex motions.

  auto interpolation_planner = std::make_shared<mtc::solvers::JointInterpolationPlanner>();
  

We also set some properties specific for to the 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->setMaxV