Path Constraints not keeping end effector in fixed configuration move

edit

Hi, I am trying to keep the end effector of the UR arm parallel to the ground using Moveit. I give the Poses to the arm via group.setPoseTarget(tgt_pose). I am testing yet simple poses. For instance the arm should move from location 1 to location 2, while keeping the gripper horizontal levelled. I am applying Path constraints to limit the motion of the gripper from the wrist, so that in any moveplan the gripper always remains horizontal.

For example consider a heavy box attached to the box and the gripper has to keep it horizontal while the gripper moves to the other point. or may be I am doing some thing wrong or everything wrong. I am following this and trying to apply the constraints. The result is that no motion plan is computed and the node ends. I am attaching snipet of code and the Moveit planer results.

CODE

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

    moveit::planning_interface::MoveGroupInterface group("arm");

    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;  
     group.setPlannerId("RRTConnectkConfigDefault");
     //group.setPlanningTime(30.0);
    const robot_state::JointModelGroup *joint_model_group =
          group.getCurrentState()->getJointModelGroup("arm");

    ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>
            ("/move_group/display_planned_path", 1, true);

    rviz_updater = node_handle.advertise<std_msgs::Empty>("/rviz/moveit/update_goal_state", 1, true);
           timer = node_handle.createTimer(ros::Duration(0.1), timerCallback);

       moveit_msgs::DisplayTrajectory display_trajectory;

       ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());

       ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str());

       moveit::planning_interface::MoveItErrorCode error_code;

       srand(time(NULL));

    // Planning to a Pose goal

    geometry_msgs::Pose target_pose0,target_pose1,target_pose2,target_pose3,target_pose4,target_pose5;
    geometry_msgs::PoseStamped target_pose;
    bool success;
            moveit_msgs::OrientationConstraint ocm;
    ocm.link_name = "arm_wrist_3_link";
        ocm.header.frame_id = "base_link";
    ocm.orientation.x = -0.0000;
    ocm.orientation.y = -0.0000;
    ocm.orientation.z = 0.718;
    ocm.orientation.w = 0.696;
       //ocm.orientation.w = 1.0;
    ocm.absolute_x_axis_tolerance = 0.1;
    ocm.absolute_y_axis_tolerance = 0.1;
    ocm.absolute_z_axis_tolerance = 3.14;
    ocm.weight = 1.0;

    moveit_msgs::Constraints test_constraints;
    test_constraints.orientation_constraints.push_back(ocm);
    group.setPathConstraints(test_constraints);

    moveit::planning_interface::MoveGroupInterface::Plan move_plan;     //Now, we call the planner to compute the plan and visualize it.
    //----------------------------------------------------------------
    std::cout << "current pose = " << curentPose()<<endl;

    target_pose0=Pose_UP();                                                                                     
           //calling the target pose
    group.setPoseTarget(target_pose0);
    success = group.plan(move_plan);
    cout<<"\tsuccess\t"<<success<<endl;
    ROS_INFO("----------EXECUTING THE MOVE to POSE UP----------");
    error_code = group.move();//group.move();      //Execute Move (blocking)
    cout<<"\tError Code is: \t"<<error_code<<endl;
    sleep (2);


    //ros::shutdown();  
    timer.stop();
    return 0;

}

Node Ouput

[ INFO] [1506372798.012616238]: Loading robot model 'summit_xl_wa'...
[ INFO] [1506372798.231563642, 75.864000000]: Loading robot model 'summit_xl_wa'...
[ INFO] [1506372799.524416658, 77.145000000]: Ready to take commands for planning group arm.
[ INFO] [1506372799.813725057, 77.433000000]: Reference frame: /odom
[ INFO] [1506372799.813781414, 77.434000000]: Reference frame: arm_wrist_3_link
[ INFO] [1506372800.140514836, 77.759000000]: Ready to take commands for planning group arm.
current pose = position: 
  x: -0.710742
  y: -0.639824
  z: 0.763397
orientation: 
  x: -0.92388
  y: -0.382679
  z: -0.00150841
  w: 0.000624808

[ WARN] [1506372830.612528839, 108.120000000]: Fail: ABORTED: No motion plan found. No execution attempted.
    success 0 ...