MTC tutorial fails to execute on franka panda real hardware

[PietroValerio]

I'm trying execute the mtc pick and place example that is copied as-is from the tutorial for ros2 humble and I get this error:

[move_group-2] [WARN] [1732697278.834689542] [moveit_task_constructor_visualization.execute_task_solution]: The trajectory of stage '3' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.
[move_group-2] [WARN] [1732697278.834703942] [moveit_robot_state.conversions]: The transform for multi-dof joints was specified in frame 'world' but it was not possible to transform that to frame 'panda_link0'
[move_group-2] [WARN] [1732697278.834922998] [moveit_task_constructor_visualization.execute_task_solution]: The trajectory of stage '4' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.
[move_group-2] [WARN] [1732697278.834933785] [moveit_robot_state.conversions]: The transform for multi-dof joints was specified in frame 'world' but it was not possible to transform that to frame 'panda_link0'
[move_group-2] [WARN] [1732697278.835019571] [moveit_task_constructor_visualization.execute_task_solution]: The trajectory of stage '6' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.
[move_group-2] [WARN] [1732697278.835027951] [moveit_robot_state.conversions]: The transform for multi-dof joints was specified in frame 'world' but it was not possible to transform that to frame 'panda_link0'
[move_group-2] [WARN] [1732697278.835036766] [moveit_robot_state.conversions]: The transform for multi-dof joints was specified in frame 'world' but it was not possible to transform that to frame 'panda_link0'
[move_group-2] [WARN] [1732697278.835140434] [moveit_task_constructor_visualization.execute_task_solution]: The trajectory of stage '10' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.
[move_group-2] [WARN] [1732697278.835149997] [moveit_robot_state.conversions]: The transform for multi-dof joints was specified in frame 'world' but it was not possible to transform that to frame 'panda_link0'
[move_group-2] [WARN] [1732697278.835163008] [moveit_robot_state.conversions]: The transform for multi-dof joints was specified in frame 'world' but it was not possible to transform that to frame 'panda_link0'
[move_group-2] [WARN] [1732697278.835373624] [moveit_task_constructor_visualization.execute_task_solution]: The trajectory of stage '12' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.
[move_group-2] [WARN] [1732697278.835383271] [moveit_robot_state.conversions]: The transform for multi-dof joints was specified in frame 'world' but it was not possible to transform that to frame 'panda_link0'
[move_group-2] [ERROR] [1732697278.835434773] [moveit_task_constructor_visualization.execute_task_solution]: Could not find JointModelGroup that actuates {panda_joint1, panda_joint2, panda_joint3, panda_joint4, panda_joint5, panda_joint6, panda_joint7, panda_finger_joint1} 

On rviz I can see the planning that is looped, so the code fails on execution. As reference i put also the code for the task

#include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/task_constructor/solvers.h>
#include <moveit/task_constructor/stages.h>
#include <moveit/task_constructor/task.h>

#include <rclcpp/rclcpp.hpp>
#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;
  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);

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

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

  // planner vector specifies movegroup and the planner
  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::Stage *attach_object_stage = nullptr;
  // Forward attach_object_stage to place pose generator
  //  In this case, we create a serial container that will contain the stages
  //  relevant to the picking action.
  // Instead of adding the stages to the task, we will add the relevant stages
  // to the serial container. We use exposeTo to declare the task properties
  // from the parent task in the new serial container, and use
  // configureInitFrom() to initialize them. This allows the contained stages to
  // access these properties.
  {
    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"});
    {
      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 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));
    }
    // this is a generator stage, so it computes its results without regard to
    // the stages before and after it. The first stage, CurrentState is a
    // generator stage as well - to connect the first stage and this stage, a
    // connecting stage must be used, which we already created above. This code
    // sets the stage properties, sets the pose before grasping, the angle
    // delta, and the monitored stage. Angle delta is a property of the
    // GenerateGraspPose stage that is used to determine the number of poses to
    // generate; when generating solutions, MoveIt Task Constructor will try to
    // grasp the object from many different orientations, with the difference
    // between the orientations specified by the angle delta. The smaller the
    // delta, the closer together the grasp orientations will be. When defining
    // the current stage, we set current_state_ptr, which is now used to forward
    // information about the object pose and shape to the inverse kinematic
    // solver. This stage won’t be directly added to the serial container like
    // previously, as we still need to do inverse kinematics on the poses it
    // generates.
    {
      // 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

      // Before we compute inverse kinematics for the poses generated above, we
      // first need to define the frame. This can be done with a PoseStamped
      // message from geometry_msgs or in this case, we define the transform
      // using Eigen transformation matrix and the name of the relevant link
      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
      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));
    }

    // In order to pick up the object, we must allow collision between the hand
    // and the object. This can be done with a ModifyPlanningScene stage. The
    // allowCollisions function lets us specify which collisions to disable.
    // allowCollisions can be used with a container of names, so we can use
    // getLinkModelNamesWithCollisionGeometry to get all the names of links with
    // collision geometry in the hand group.
    {
      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));
    }

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

    {
      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"});
    {
      // generate the place pose and compute ik
      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);

      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));
    }
    // re-enable collision checking between hand and object after placing
    {
      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));
    }

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

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

As note an example should work both in simulation and on real hardware without any effort required. If you need more info I am available.

[rhaschke]

There are two issues, both unrelated to MTC:

1. The transform for multi-dof joints was specified in frame 'world' but it was not possible to transform that to frame 'panda_link0'
   This means that your Panda launch files don't declare a static TF2 transform from world to panda_link0.
2. Could not find JointModelGroup that actuates {panda_joint1, panda_joint2, panda_joint3, panda_joint4, panda_joint5, panda_joint6, panda_joint7, panda_finger_joint1}
   This means that you don't have any controllers defined or your joints are named differently.