search

moveit / moveit2

:robot: MoveIt for ROS 2
https://moveit.ai/
BSD 3-Clause "New" or "Revised" License
1.08k stars 525 forks source link

move_group_interface.setPathConstraints failed #2918

Closed Helloxuxuxu closed 2 weeks ago

Helloxuxuxu commented 3 months ago

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;
}
rhaschke commented 3 months ago

Probably the planner is not able to find a path satisfying your constraint. Are you sure, the constraint is feasible?

Helloxuxuxu commented 3 months ago

Probably the planner is not able to find a path satisfying your constraint. Are you sure, the constraint is feasible?

Thank you for your reply! 1.First, I have adjusted the initial orientation to the target orientation, and this step has been successful.

/*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;
    }

2.Then, I set the constraint to 3.1515926, which is almost equal to no constraint.

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

3.In the end, the plan failed, and the planned time far exceeded the time I had set before (more than 2min). move_group_interface_.setPlanningTime(10);

I really don't understand how to use this constraint. Finally, thank you again for your prompt reply

github-actions[bot] commented 2 months ago

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

github-actions[bot] commented 2 weeks ago

This issue was closed because it has been stalled for 45 days with no activity.