Path constraints

edit

Hello I am trying to keep the end effector of the UR10 arm parallel to the ground using Moveit, which means the pose of end effector's pitch=0, roll=0 and yaw angle can be any value. My code is shown as followed and the motion plan can't be found, Fail: ABORTED: No motion plan found. No execution attempted, . And i am sure the pose 'next_pose" is reachable. Would you mind checking the code and pointing out the mistake?

int main(int argc, char **argv)
{
   ros::init(argc, argv, "move_group_interface_tutorial");
    ros::NodeHandle node_handle;
    ros::AsyncSpinner spinner(1);
    spinner.start();


    /* This sleep is ONLY to allow Rviz to come up */
    sleep(20.0);

    moveit::planning_interface::MoveGroup group_r("arm");
    group_r.setPlannerId("RRTConnectkConfigDefault");
    group_r.setPlanningTime(45);
//ocm
    moveit_msgs::OrientationConstraint ocm;

    ocm.link_name = "tt_base_link";
    ocm.header.frame_id = "base_link";

    ocm.orientation.x = 0.210738;
    ocm.orientation.y = 0.672778;
    ocm.orientation.z = 0.677635;
    ocm.orientation.w = 0.209212;

    ocm.absolute_x_axis_tolerance = 0.01;
    ocm.absolute_y_axis_tolerance = 0.01;
    ocm.absolute_z_axis_tolerance = 3.14;
    ocm.weight = 1.0;
    moveit_msgs::Constraints test_constraints;
    test_constraints.orientation_constraints.push_back(ocm);
    group_r.setPathConstraints(test_constraints);

//start pose


   geometry_msgs::Pose start_pose;
    start_pose.position.x = -0.402141;
    start_pose.position.y = 0.162843;
    start_pose.position.z = 0.384870;
    start_pose.orientation.x = -0.003270;
    start_pose.orientation.y = 0.704558;
    start_pose.orientation.z = 0.709631;
    start_pose.orientation.w = -0.003249;


    const robot_state::JointModelGroup *joint_model_group =
                    //start_state.getJointModelGroup(group_r.getName());
    start_state.setFromIK(joint_model_group, start_pose);
    group_r.setStartState(start_state);

// target  
    geometry_msgs::Pose next_pose;



        next_pose.position.x =-0.402141;
        next_pose.position.y =0.462843;
        next_pose.position.z =0.384870;
        next_pose.orientation.x =0.167744;
        next_pose.orientation.y =0.684582;
        next_pose.orientation.z =0.689516;
        next_pose.orientation.w =0.166666;

        group_r.setPoseTarget(next_pose,"tt_base_link");


    moveit::planning_interface::MoveGroup::Plan my_plan;
    bool success = group_r.plan(my_plan);

    ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");


        sleep(5.0);
    group_r.clearPathConstraints();
}