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-with-CM-planning

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

#include 

#include 
#include 
#include 
#include 
#include 
#include 

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 first planning problem");
  // Create the first planning problem
  auto target_pose = get_relative_pose(0.0, -0.01, -0.01);

  //for debug
  RCLCPP_INFO_STREAM(LOGGER, "here to visualize the start and the end position");


  // Let's try the simple box constraints first!
  moveit_msgs::msg::PositionConstraint box_constraint;
  box_constraint.header.frame_id = constrain_interface.getPoseReferenceFrame();
  // box_constraint.link_name = move_group_interface.getEndEffectorLink();
  box_constraint.link_name = constrain_interface.getEndEffectorLink();
  RCLCPP_INFO_STREAM(LOGGER, "============================= the constraint link is " << box_constraint.link_name);

  // the output is "panda_link8"
  shape_msgs::msg::SolidPrimitive box;
  box.type = shape_msgs::msg::SolidPrimitive::BOX;
  box.dimensions = { 0.05, 0.3, 0.05 };
  box_constraint.constraint_region.primitives.emplace_back(box);

  geometry_msgs::msg::Pose box_pose;
  box_pose.position.x = current_pose.pose.position.x;
  box_pose.position.y = current_pose.pose.position.y;
  box_pose.position.z = current_pose.pose.position.z;
  box_pose.orientation.w = 1.0;
  box_constraint.constraint_region.primitive_poses.emplace_back(box_pose);
  box_constraint.weight = 1.0;

  // Visualize the box constraint
  Eigen::Vector3d box_point_1(box_pose.position.x - box.dimensions[0] / 2, box_pose.position.y - box.dimensions[1] / 2,
                              box_pose.position.z - box.dimensions[2] / 2);
  Eigen::Vector3d box_point_2(box_pose.position.x + box.dimensions[0] / 2, box_pose.position.y + box.dimensions[1] / 2,
                              box_pose.position.z + box.dimensions[2] / 2);
  moveit_visual_tools.publishCuboid(box_point_1, box_point_2, rviz_visual_tools::TRANSLUCENT_DARK);
  moveit_visual_tools.trigger();

  // We need to wrap the constraints in a generic `Constraints` message.
  moveit_msgs::msg::Constraints box_constraints;
  box_constraints.position_constraints.emplace_back(box_constraint);

  // Don't forget the path constraints! That's the whole point of this tutorial.
  move_group_interface.setPathConstraints(box_constraints);

  // Now we have everything we need to configure and solve a planning problem - plan to the target pose
  move_group_interface.setPoseTarget(target_pose);
  move_group_interface.setPlanningTime(10.0);

  // And let the planner find a solution.
  // The move_group node should automatically visualize the solution in Rviz if a path is found.
  moveit::planning_interface::MoveGroupInterface::Plan plan;
  auto success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
  RCLCPP_INFO(LOGGER, "Plan with box constraint %s", success ? "" : "FAILED");

  // Now wait for the user to press Next before trying the planar constraints.
  moveit_visual_tools.prompt(
      "Press 'Next' in the RvizVisualToolsGui window to continue to the planar constraint example");

  // Clear the path constraints and markers for the next example
  reset_demo();

  // Done!
  moveit_visual_tools.prompt("Press 'Next' in the RvizVisualToolsGui window to clear the markers");
  moveit_visual_tools.deleteAllMarkers();
  moveit_visual_tools.trigger();
  move_group_interface.clearPathConstraints();

  rclcpp::shutdown();
  spinner.join();
  return 0;
}