Not generating a Pose Place IK

[lucasfosco]

I'm trying to do this tutorial: https://moveit.picknik.ai/main/doc/tutorials/pick_and_place_with_moveit_task_constructor/pick_and_place_with_moveit_task_constructor.html

After some tries with the code provided in the tutorial and finding some errors, I've tried to complete the code with this code (which is the one that at the beginning of the tutorial is used as an example):
https://github.com/moveit/moveit_task_constructor/blob/master/demo/src/pick_place_task.cpp

This is the result code mtc_node_panda.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_;
};

MTCTaskNode::MTCTaskNode(const rclcpp::NodeOptions& options)
: node_{ std::make_sharedrclcpp::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.position.z = 0.05;
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

/****************************************************
 *                                                  *
 *               Current State                      *
 *                                                  *
 ***************************************************/

auto stage_state_current = std::make_uniquemtc::stages::CurrentState("current");
current_state_ptr = stage_state_current.get();
task.add(std::move(stage_state_current));

auto sampling_planner = std::make_sharedmtc::solvers::PipelinePlanner(node_);
auto interpolation_planner = std::make_sharedmtc::solvers::JointInterpolationPlanner();
auto cartesian_planner = std::make_sharedmtc::solvers::CartesianPath();
cartesian_planner->setMaxVelocityScalingFactor(1.0);
cartesian_planner->setMaxAccelerationScalingFactor(1.0);
cartesian_planner->setStepSize(.01);

/****************************************************
 *                                                  *
 *               Open Hand                          *
 *                                                  *
 ***************************************************/

mtc::Stage* initial_state_ptr = nullptr;
{
auto stage_open_hand =
std::make_uniquemtc::stages::MoveTo("open hand", interpolation_planner);
stage_open_hand->setGroup(hand_group_name);
stage_open_hand->setGoal("open");
initial_state_ptr = stage_open_hand.get(); // remember start state for monitoring grasp pose generator
task.add(std::move(stage_open_hand));
}

/****************************************************
 *                                                  *
 *               Move to Pick                       *
 *                                                  *
 ***************************************************/

{
auto stage_move_to_pick = std::make_uniquemtc::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));
}

/****************************************************
 *                                                  *
 *               Pick Object                        *
 *                                                  *
 ***************************************************/  

mtc::Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator
{
auto grasp = std::make_uniquemtc::SerialContainer("pick object");
task.properties().exposeTo(grasp->properties(), { "eef", "group", "ik_frame" });
grasp->properties().configureInitFrom(mtc::Stage::PARENT,{ "eef", "group", "ik_frame" });

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

	/****************************************************
 *               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(initial_state_ptr);  // Hook into current state

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

/****************************************************
 *               Allow Collision (hand object)      *
	 ***************************************************/
	{
		auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)");
		stage->allowCollisions("object",
		    task.getRobotModel()
        ->getJointModelGroup(hand_group_name)
        ->getLinkModelNamesWithCollisionGeometry(),
		    true);
		grasp->insert(std::move(stage));
	}

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

	/****************************************************
 *               Attach Object                      *
	 ***************************************************/
	{
		auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("attach object");
		stage->attachObject("object", hand_frame);
		grasp->insert(std::move(stage));
	}

	/****************************************************
 *       Allow collision (object support)           *
	 ***************************************************/
	{
		auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (object,support)");
		stage->allowCollisions({"object"},
                        task.getRobotModel()
                          ->getJointModelGroup(hand_group_name)
                          ->getLinkModelNamesWithCollisionGeometry(),
                          true);
		grasp->insert(std::move(stage));
	}

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

/****************************************************
 *        Forbid collision (object support)         *
	 ***************************************************/
	{
		auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("forbid collision (object,surface)");
		stage->allowCollisions("object",
                    task.getRobotModel()
                        ->getJointModelGroup(hand_group_name)
                        ->getLinkModelNamesWithCollisionGeometry(),
                    false);
  grasp->insert(std::move(stage));
	}

	attach_object_stage = grasp.get();  // remember for monitoring place pose generator

// Add grasp container to task
task.add(std::move(grasp));

}

/******************************************************
 *                                                    *
 *          Move to Place                             *
 *                                                    *
 *****************************************************/

{
auto stage_move_to_place = std::make_uniquemtc::stages::Connect(
"move to place",
mtc::stages::Connect::GroupPlannerVector{ { arm_group_name, sampling_planner },
{ hand_group_name, interpolation_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 Object                              *
 *                                                    *
 *****************************************************/

{
auto place = std::make_uniquemtc::SerialContainer("place object");
task.properties().exposeTo(place->properties(), { "eef", "group", "ik_frame" });
place->properties().configureInitFrom(mtc::Stage::PARENT, { "eef", "group", "ik_frame" });

	/******************************************************
 *          Lower Object                              *
	 *****************************************************/
	{
		auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lower object", cartesian_planner);
		stage->properties().set("marker_ns", "lower_object");
		stage->properties().set("link", hand_frame);
		stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });
		stage->setMinMaxDistance(.03, .13);

		// Set downward direction
		geometry_msgs::msg::Vector3Stamped vec;
		vec.header.frame_id = "world";
		vec.vector.z = -1.0;
		stage->setDirection(vec);
		place->insert(std::move(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.position.z = 0.05;
  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(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" });
  place->insert(std::move(wrapper));
}

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

	/******************************************************
 *          Forbid collision (hand, object)        *
	 *****************************************************/
{
  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));
}

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

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

// Add place container to task
task.add(std::move(place));

}

/******************************************************
 *                                                    *
 *          Move to Home                              *
 *                                                    *
 *****************************************************/

{
auto stage = std::make_uniquemtc::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(options);
rclcpp::executors::MultiThreadedExecutor executor;

auto spin_thread = std::make_uniquestd::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;
}

And the logs I receive (I've already tried with lower values of cartesian Step Size and same result):

luki@luki-Lenovo-B50-80:~/ws_moveit2$ ros2 launch mtc_tutorial_panda pick_place_demo.launch.py
[INFO] [launch]: All log files can be found below /home/luki/.ros/log/2024-05-29-21-14-02-699214-luki-Lenovo-B50-80-15255
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [mtc_node_panda-1]: process started with pid [15258]
[mtc_node_panda-1] [INFO] [1717010043.873541429] [planning_scene_interface_108803162633264.moveit.RDFLoader]: Loaded robot model in 0.00762448 seconds
[mtc_node_panda-1] [INFO] [1717010043.873783262] [planning_scene_interface_108803162633264.moveit.robot_model]: Loading robot model 'panda'...
[mtc_node_panda-1] [INFO] [1717010043.943075838] [planning_scene_interface_108803162633264.moveit.kdl_kinematics_plugin]: Joint weights for group 'panda_arm': 1 1 1 1 1 1 1
[mtc_node_panda-1] [INFO] [1717010044.129018848] [planning_scene_interface_108803162633264.moveit.planning_pipeline]: Successfully loaded planner 'OMPL'
[mtc_node_panda-1] [INFO] [1717010044.147246651] [mtc_node]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames'
[mtc_node_panda-1] [INFO] [1717010044.150986068] [mtc_node]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames'
[mtc_node_panda-1] [INFO] [1717010044.151051695] [mtc_node]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds'
[mtc_node_panda-1] [INFO] [1717010044.152187358] [mtc_node]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds'
[mtc_node_panda-1] [INFO] [1717010044.152263498] [mtc_node]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds'
[mtc_node_panda-1] [INFO] [1717010044.152413553] [mtc_node]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds'
[mtc_node_panda-1] [INFO] [1717010044.152442319] [mtc_node]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision'
[mtc_node_panda-1] [INFO] [1717010044.152485459] [mtc_node]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision'
[mtc_node_panda-1] [INFO] [1717010044.169735655] [mtc_node]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization'
[mtc_node_panda-1] [INFO] [1717010044.180261330] [mtc_node]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization'
[mtc_node_panda-1] [INFO] [1717010044.180531143] [mtc_node]: Try loading adapter 'default_planning_response_adapters/ValidateSolution'
[mtc_node_panda-1] [INFO] [1717010044.183789536] [mtc_node]: Loaded adapter 'default_planning_response_adapters/ValidateSolution'
[mtc_node_panda-1] [INFO] [1717010044.183900349] [mtc_node]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath'
[mtc_node_panda-1] [INFO] [1717010044.186282417] [mtc_node]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath'
[mtc_node_panda-1] [WARN] [1717010060.369908182] [planning_scene_interface_108803162633264.moveit.cartesian_interpolator]: The computed path is too short to detect jumps in joint-space. Need at least 10 steps, only got 6. Try a lower max_step.
[mtc_node_panda-1] 0 - ← 0 → - 0 / demo task
[mtc_node_panda-1] 1 - ← 1 → - 0 / current
[mtc_node_panda-1] - 0 → 1 → - 1 / open hand
[mtc_node_panda-1] - 1 → 0 ← 39 - / move to pick
[mtc_node_panda-1] 39 - ← 39 → - 39 / pick object
[mtc_node_panda-1] 40 - ← 40 ← 4 - / approach object
[mtc_node_panda-1] 4 - ← 51 → - 4 / grasp pose IK
[mtc_node_panda-1] 25 - ← 25 → - 25 / generate grasp pose
[mtc_node_panda-1] - 4 → 47 → - 0 / allow collision (hand,object)
[mtc_node_panda-1] - 0 → 47 → - 0 / close hand
[mtc_node_panda-1] - 0 → 47 → - 0 / attach object
[mtc_node_panda-1] - 0 → 47 → - 0 / allow collision (object,support)
[mtc_node_panda-1] - 0 → 42 → - 0 / lift object
[mtc_node_panda-1] - 0 → 42 → - 42 / forbid collision (object,surface)
[mtc_node_panda-1] - 39 → 0 ← 0 - / move to place
[mtc_node_panda-1] 0 - ← 0 → - 0 / place object
[mtc_node_panda-1] 0 - ← 0 ← 0 - / lower object
[mtc_node_panda-1] 0 - ← 0 → - 0 / place pose IK
[mtc_node_panda-1] 390 - ← 390 → -390 / generate place pose
[mtc_node_panda-1] - 0 → 0 → - 0 / open hand
[mtc_node_panda-1] - 0 → 0 → - 0 / forbid collision (hand,object)
[mtc_node_panda-1] - 0 → 0 → - 0 / detach object
[mtc_node_panda-1] - 0 → 0 → - 0 / retreat
[mtc_node_panda-1] - 0 → 0 → - 0 / return home
[mtc_node_panda-1] Failing stage(s):
[mtc_node_panda-1] place pose IK (0/390)
[mtc_node_panda-1] [ERROR] [1717010060.919964371] [mtc_tutorial]: Task planning failed