Moveit path planning with constraints fails
Hey,
I'm trying to move an object with the kinova mico arm, using ROS and moveit, while keeping the end effector horizontal. Relocating an object proved successful, however when I add some path constraints the planning time shoots up to over 120 seconds, where 10 seconds sufficed before, and the success ratio has dropped to 1 out of 10. I tried to fine-tune the range variable between 0.05 and 3.0 at both the RRT and the RRTConnect libraries which didn't change anything in the results. The constraints are defined like shown below.
moveit_msgs::Constraints PC;
moveit_msgs::OrientationConstraint OC;
OC.header.frame_id = "mico_link_base";
OC.link_name = "mico_link_7";
OC.orientation = target_pose1.orientation;
OC.absolute_x_axis_tolerance = 0.2;
OC.absolute_y_axis_tolerance = 2*PI;
OC.absolute_z_axis_tolerance = 0.2;
OC.weight = 1.0;
PC.orientation_constraints.push_back(OC);
group.setPathConstraints(PC);
The most common error from moveit is that the orientation constraints are being violated with some steps.
[ERROR] [1465478261.387712988]: Computed path is not valid. Invalid states at index locations: [ 76 78 80 81 82 83 84 85 92 93 94 ] out of 128. Explanations follow in command line. Contacts are published on /move_group/display_contacts
[ INFO] [1465478261.390961441]: Orientation constraint violated for link 'mico_link_7'. Quaternion desired: -0.653281 0.270598 -0.270598 0.653281, quaternion actual: 0.335662 0.155673 0.900074 0.230140, error: x=0.908021, y=0.399568, z=0.423861, tolerance: x=0.200000, y=6.283185, z=0.200000
....
[ INFO] [1465478261.434939990]: Orientation constraint violated for link 'mico_link_7'. Quaternion desired: -0.653281 0.270598 -0.270598 0.653281, quaternion actual: -0.573032 0.355789 -0.231741 0.700960, error: x=0.090580, y=0.089167, z=0.228256, tolerance: x=0.200000, y=6.283185, z=0.200000
[ERROR] [1465478261.437059768]: Completed listing of explanations for invalid states.
I really want to bring down its planning time and bring its success ratio to at least 50%.
Are there some variables I can add in the ompl_planning.yaml file?
Or is there a better way to set its path constraints?
