move_group_interface.setPathConstraints failed #2918
Description
Description
I am encountering an issue with MoveIt! where the motion planning fails to generate a valid trajectory when path constraints are applied. Specifically, the code hangs after setting path constraints and attempting to plan a motion, ultimately timing out and returning a failure message.
Your environment
ROS 2: Humble
OS Version: Ubuntu 22.04
source: moveit2 release 2.5.5
Steps to reproduce
Launch the MoveIt! node with the specified robot model.
Set the path constraints using the setPathConstraints method.
Attempt to plan a motion using the plan method.
Observe the code hanging at the indicated location and eventually timing out.
move_group_interface_.setPathConstraints(test_constraints);
std::cout << "here2" << std::endl;
bool success_1 = (move_group_interface_.plan(my_plan_1)
== moveit::core::MoveItErrorCode::SUCCESS);
std::cout << "here3" << std::endl;
Expected behaviour
The motion planning should successfully generate a valid trajectory that adheres to the specified path constraints.
Actual behaviour
The code hangs after setting the path constraints and does not proceed to execute the motion planning. It exceeds the predefined time limit and returns a failure message.
Even I set:
ocm.orientation = target_pose.orientation;
ocm.absolute_x_axis_tolerance = 3.1415926; // Set tolerance
ocm.absolute_y_axis_tolerance = 3.1415926;
ocm.absolute_z_axis_tolerance = 3.1415926;
when I cancel the
move_group_interface_.setPathConstraints(test_constraints);
every thing will be ok.
Backtrace or Console output
moveit console
[move_group-1] [INFO] [1721298140.183175101] [moveit.ompl_planning.model_based_planning_context]: ur_manipulator: Allocating specialized state sampler for state space
[move_group-1] [ERROR] [1721298140.345854076] [moveit.ros_planning.planning_pipeline]: Computed path is not valid. Invalid states at index locations: [ 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 ] out of 183. Explanations follow in command line. Contacts are published on /display_contacts
[move_group-1] [INFO] [1721298140.346235379] [moveit_collision_detection_fcl.collision_common]: Found a contact between 'forearm_link' (type 'Robot link') and 'robotiq_85_right_finger_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[move_group-1] [INFO] [1721298140.346243725] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[move_group-1] [INFO] [1721298140.346505154] [moveit_collision_detection_fcl.collision_common]: Found a contact between 'forearm_link' (type 'Robot link') and 'robotiq_85_right_knuckle_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[move_group-1] [INFO] [1721298140.346516042] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[move_group-1] [INFO] [1721298140.346733690] [moveit_collision_detection_fcl.collision_common]: Found a contact between 'forearm_link' (type 'Robot link') and 'robotiq_85_right_knuckle_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[move_group-1] [INFO] [1721298140.346740466] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[move_group-1] [INFO] [1721298140.346991818] [moveit_collision_detection_fcl.collision_common]: Found a contact between 'forearm_link' (type 'Robot link') and 'robotiq_85_right_knuckle_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[move_group-1] [INFO] [1721298140.346996970] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[move_group-1] [INFO] [1721298140.347283209] [moveit_collision_detection_fcl.collision_common]: Found a contact between 'forearm_link' (type 'Robot link') and 'robotiq_85_right_knuckle_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[move_group-1] [INFO] [1721298140.347292113] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[move_group-1] [INFO] [1721298140.347575001] [moveit_collision_detection_fcl.collision_common]: Found a contact between 'forearm_link' (type 'Robot link') and 'robotiq_85_right_knuckle_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
code console
[ur_gripper_motion_plan_sub-1] [INFO] [1721297945.823883195] [ur3_motion_plan_TEST]: Move to target_pose_temp okokokokokokokokokokok
[ur_gripper_motion_plan_sub-1] [INFO] [1721297945.824574392] [move_group_interface]: Execute request accepted
[ur_gripper_motion_plan_sub-1] [INFO] [1721297975.704443964] [move_group_interface]: Execute request success!
[ur_gripper_motion_plan_sub-1] here0
[ur_gripper_motion_plan_sub-1] here1
[ur_gripper_motion_plan_sub-1] here2
[ur_gripper_motion_plan_sub-1] [INFO] [1721297975.705509483] [move_group_interface]: MoveGroup action client/server ready
[ur_gripper_motion_plan_sub-1] [INFO] [1721297975.706400007] [move_group_interface]: Planning request accepted
[ur_gripper_motion_plan_sub-1] [INFO] [1721298140.383795449] [move_group_interface]: Planning request aborted
[ur_gripper_motion_plan_sub-1] [ERROR] [1721298140.384834143] [move_group_interface]: MoveGroupInterface::plan() failed or timeout reached
[ur_gripper_motion_plan_sub-1] [ERROR] [1721298140.384884101] [ur3_motion_plan_TEST]: Planning failed for goal_pos [0.150000, 0.300000, 0.400000, -0.002000, -0.000000, 0.700000, 0.714000]
[ur_gripper_motion_plan_sub-1] here3
code:
void MoveIt_Control::Move_pose_constrains(const std::vector<double> &goal_pos){
if (goal_pos.size() != 7){
RCLCPP_INFO(logger_,"The input 'goal_pos' number is incorrect. Expected 7 values [x,y,z,qx,qy,qz,qw].");
return;
} else {
geometry_msgs::msg::Pose target_pose;
target_pose.position.x = goal_pos[0];
target_pose.position.y = goal_pos[1];
target_pose.position.z = goal_pos[2];
target_pose.orientation.x = goal_pos[3];
target_pose.orientation.y = goal_pos[4];
target_pose.orientation.z = goal_pos[5];
target_pose.orientation.w = goal_pos[6];
/*First, align the end-effector pose with the target pose*/
geometry_msgs::msg::PoseStamped current_pose;
current_pose = move_group_interface_.getCurrentPose("gripper_fake_center_link");
geometry_msgs::msg::Pose target_pose_temp; //Intermediate pose
target_pose_temp.position = current_pose.pose.position;
target_pose_temp.orientation = target_pose.orientation;
move_group_interface_.setPoseTarget(target_pose_temp);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group_interface_.plan(my_plan)
== moveit::core::MoveItErrorCode::SUCCESS);
if (success){
RCLCPP_INFO(logger_,"Move to target_pose_temp successful.");
move_group_interface_.execute(my_plan);
} else {
RCLCPP_ERROR(logger_,"Planning failed to target_pose_temp.");
return;
}
/*Set the end-effector rotation angle constraints*/
std::cout<<"here0"<<std::endl;
moveit_msgs::msg::OrientationConstraint ocm;
ocm.link_name = "gripper_fake_center_link";
ocm.header.frame_id = "base_link";
ocm.orientation = target_pose.orientation;
ocm.absolute_x_axis_tolerance = 3.1415926; // Set tolerance
ocm.absolute_y_axis_tolerance = 3.1415926;
ocm.absolute_z_axis_tolerance = 3.1415926;
ocm.weight = 1.0; // Weight can be set to 1.0, to make the planner prioritize this constraint
moveit_msgs::msg::Constraints test_constraints;
test_constraints.orientation_constraints.push_back(ocm);
move_group_interface_.setPathConstraints(test_constraints);
std::cout<<"here1"<<std::endl;
/*Begin planning*/
move_group_interface_.setPlanningTime(10);
move_group_interface_.setStartStateToCurrentState();
move_group_interface_.setPoseTarget(target_pose);
moveit::planning_interface::MoveGroupInterface::Plan my_plan_1;
std::cout<<"here2"<<std::endl;
bool success_1 = (move_group_interface_.plan(my_plan_1)
== moveit::core::MoveItErrorCode::SUCCESS);
std::cout<<"here3"<<std::endl;
if (success_1){
RCLCPP_INFO(logger_,"Move_pose_constrains successful.");
move_group_interface_.execute(my_plan_1);
} else {
RCLCPP_ERROR(logger_,"Planning failed for goal_pos [%f, %f, %f, %f, %f, %f, %f]",
goal_pos[0],goal_pos[1],goal_pos[2],goal_pos[3],goal_pos[4],goal_pos[5],goal_pos[6]);
}
/*Remove constraints*/
move_group_interface_.clearPathConstraints();
move_group_interface_.setPlanningTime(5);
std::this_thread::sleep_for(std::chrono::milliseconds(500));
}
return;
}