Moveit path planning with constraints fails

Question

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?

---

Originally posted by Lex van Dommelen on ROS Answers with karma: 13 on 2016-06-09

Post score: 1

Answer 1

Although this is not directly an answer to your question, you might want to look into using the trac_ik package.

---

Originally posted by kramer with karma: 1470 on 2016-06-10

This answer was ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by Lex van Dommelen on 2016-06-13:
Thanks for your answer! it works with a 4 out of 6 success rate with 60 seconds planning time. The ompl_planning.yaml file is restored to its default and maybe even a better performance can be realized after fine-tuning the variables. Thanks a lot!

Comment by pbeeson on 2016-06-15:
Yes, for future reference for anyone reading this, when MoveIt! fails often for your particular manipulator, it usually the fault of the default (KDL) IK solver, not necessarily the fault of the MoveIt! framework or OMPL.

Comment by pbeeson on 2016-06-15:
Additionally, for Indigo, and Jade, TRAC-IK (including the moveit plugin) has recently been released into the ROS Debian repo, so any day now, apt-get install ros-indigo-trac-ik-kinematics-plugin will get you the MoveIt! plugin quickly.