planning with orientation constraint in moveit

edit

Hi, I set an end-effector orientation constraint for planning using Moveit, but computed motions have some jerky points.

with RRT (OMPL) : http://youtu.be/waVhHUCFRvo with RRT* (OMPL) : http://youtu.be/5vDLr5XhvJQ

It looks the configuration suddenly changes to another IK solution in such points. What should I do to get smooth trajectories?

I am using ROS Hydro and Moveit. I used below code for setting the orientation constraint.

moveit_msgs::OrientationConstraint ocm;
ocm.link_name = "end_effector_link";
ocm.header.frame_id = "segment_0";
ocm.orientation.x = 0;
ocm.orientation.y = 0;
ocm.orientation.z = -INV_SQRT_2;
ocm.orientation.w = INV_SQRT_2;
ocm.absolute_x_axis_tolerance = 0.5;
ocm.absolute_y_axis_tolerance = 0.5;
ocm.absolute_z_axis_tolerance = M_PI;
ocm.weight = 1.0;
req.path_constraints.orientation_constraints.push_back(ocm);