IK cannot be performed for link 'tool0'. The solver can report IK solutions for link '_link_ee'.

edit

Hi! I'm now working on ros2 motion planning with moveit2. my robot is basically ur5 with a continuum manipulator. I want my continuum manipulator tip to move from one position to another and I also want to add position constraint to the ur5 tool frame. (imagine this as a human arm that I want my finger to move from one to another position but my wrist stays in certain box). I tried OMPL-constrained planning. However, it seems that I can only add position constraints to the group end effector (I am not quite sure if I understand this correctly).

Look at github for code: https://github.com/yaqianchen/UR5-wit...

Below is my ompl_constrained _planning.cpp. _link_ee is the tip of my continuum manipulator and tool0 is the tip of ur5. Thanks in advance for any suggestion.

#include <chrono>

#include <moveit move_group_interface="" move_group_interface.h="">
#include <moveit planning_scene_interface="" planning_scene_interface.h="">
#include <moveit_msgs msg="" collision_object.hpp="">
#include <moveit_visual_tools moveit_visual_tools.h="">
#include <std_msgs msg="" color_rgba.hpp="">
#include <iostream>

static const auto LOGGER = rclcpp::get_logger("ompl_constrained_planning_demo");
int main(int argc, char** argv)
{
  using namespace std::chrono_literals;
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions node_options;
  node_options.automatically_declare_parameters_from_overrides(true);
  auto node = rclcpp::Node::make_shared("ompl_constrained_planning_demo_node", node_options);

  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node);
  auto spinner = std::thread([&executor]() { executor.spin(); });

  moveit::planning_interface::MoveGroupInterface move_group_interface(node, "whole");
  moveit::planning_interface::MoveGroupInterface constrain_interface(node, "ur_arm");
  auto moveit_visual_tools =
      moveit_visual_tools::MoveItVisualTools{ node, "world", rviz_visual_tools::RVIZ_MARKER_TOPIC,
                                              move_group_interface.getRobotModel() };

  rclcpp::sleep_for(1s);

  // Create some helpful lambdas
  auto current_pose = constrain_interface.getCurrentPose("tool0");
  auto tip_pose = move_group_interface.getCurrentPose();
  // current_pose.pose.position.x = 0.72795;
  // current_pose.pose.position.y = 0.26645;
  // current_pose.pose.position.z = 0.4725;

  //for debug
  RCLCPP_INFO_STREAM(LOGGER, "Value of current pose = " << current_pose.pose.position.x << " " << current_pose.pose.position.y << " " << current_pose.pose.position.z);
  auto ee_pose = move_group_interface.getCurrentPose("_link_ee");
  auto world_pose = move_group_interface.getCurrentPose("world");
  auto test_pose = move_group_interface.getCurrentPose("tool0");
  RCLCPP_INFO_STREAM(LOGGER, "Value of ee pose = " << ee_pose.pose.position.x << " " << ee_pose.pose.position.y << " " << ee_pose.pose.position.z);
  RCLCPP_INFO_STREAM(LOGGER, "Value of world pose = " << world_pose.pose.position.x << " " << world_pose.pose.position.y << " " << world_pose.pose.position.z);
  RCLCPP_INFO_STREAM(LOGGER, "Value of tool0 pose = " << test_pose.pose.position.x << " " << test_pose.pose.position.y << " " << test_pose.pose.position.z);
  RCLCPP_INFO_STREAM(LOGGER, "Value of end effector = " << move_group_interface.getEndEffectorLink());

  // Creates a pose at a given positional offset from the current pose
  auto get_relative_pose = [tip_pose, &moveit_visual_tools](double x, double y, double z) {
    auto target_pose = tip_pose;
    target_pose.pose.position.x += x;
    target_pose.pose.position.y += y;
    target_pose.pose.position.z += z;
    moveit_visual_tools.publishSphere(tip_pose.pose, rviz_visual_tools::RED, 0.05);
    moveit_visual_tools.publishSphere(target_pose.pose, rviz_visual_tools::GREEN, 0.05);
    moveit_visual_tools.trigger();
    return target_pose;
  };

  // Resets the demo by cleaning up any constraints and markers
  auto reset_demo = [&move_group_interface, &moveit_visual_tools]() {
    move_group_interface.clearPathConstraints();
    moveit_visual_tools.deleteAllMarkers();
    moveit_visual_tools.trigger();
  };

  reset_demo();
  moveit_visual_tools.loadRemoteControl();
  moveit_visual_tools.prompt("Press 'Next' in the RvizVisualToolsGui window to start with the box constraint example");

  //====================================================================================================================================================================
  // moveit_visual_tools.prompt("finished the initialization, and ready to get the ...