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.
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:
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
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.