How to keep the robot level in motion

edit

Hello guys, I want to get my Universal Robot model keep level in the simulation in Moveit which means the rotation of X axis and Y axis of the robot's end-effector will not be changed and the the rotation of Z axis can be controlled by the planner. My code is listed as followed

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);

  // BEGIN_TUTORIAL
  // 
  // Setup
  // ^^^^^
  // 
  // The :move_group_interface:`MoveGroup` class can be easily 
  // setup using just the name
  // of the group you would like to control and plan for.
  moveit::planning_interface::MoveGroup group("arm");

  // We will use the :planning_scene_interface:`PlanningSceneInterface`
  // class to deal directly with the world.
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;  

  // (Optional) Create a publisher for visualizing plans in Rviz.


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

  // Getting Basic Information
  // ^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // We can print the name of the reference frame for this robot.
  ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());

  // We can also print the name of the end-effector link for this group.
  ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str());

  // Planning to a Pose goal
  // ^^^^^^^^^^^^^^^^^^^^^^^
  // We can plan a motion for this group to a desired pose for the 
  // end-effector.
  robot_state::RobotState start_state(*group.getCurrentState());
  geometry_msgs::Pose start_pose = group.getCurrentPose().pose;
  //geometry_msgs::Pose start_pose2;



  group.setStartState(start_state);


  moveit_msgs::OrientationConstraint ocm;  
  ocm.link_name = "tt_base_link";  
  ocm.header.frame_id = "base_link";
   ocm.orientation.x = 0.003;
  ocm.orientation.y = 0.002;
  //ocm.orientation.z = 0.101;
  //ocm.orientation.w = 0.949;
  //ocm.orientation.z = 0;
    ocm.absolute_x_axis_tolerance = 0.1;
    ocm.absolute_y_axis_tolerance = 0.1;
    ocm.absolute_z_axis_tolerance = 0.1;
  ocm.weight = 1.0;




// Now, set it as the path constraint for the group.
  moveit_msgs::Constraints test_constraints;
  test_constraints.orientation_constraints.push_back(ocm);  
  group.setPathConstraints(test_constraints);

  std::vector<geometry_msgs::Pose> waypoints;
  geometry_msgs::Pose target_pose1;
   //ocm.orientation.x = 0;
  //ocm.orientation.y = 0;

 // ocm.orientation.x = 0.003;
  //ocm.orientation.y = 0.002;
  //ocm.orientation.z = 0.101;
 // ocm.orientation.w = 0.949;
target_pose1.position.x = 0.53;
target_pose1.position.y = 0.24;
  target_pose1.position.z = 0.81;
  waypoints.push_back(target_pose1);


   moveit_msgs::RobotTrajectory trajectory;
  double fraction = group.computeCartesianPath(waypoints, 0.005, 0.0, trajectory);
  moveit::planning_interface::MoveGroup::Plan my_plan;
  bool success = group.plan(my_plan);
  // Now we will plan to the earlier pose target from the new 
  // start state that we have just created.
  group.setPoseTarget(target_pose1);
  success = group.plan(my_plan);

  ROS_INFO("Visualizing plan 3 (constraints) %s",success?"":"FAILED");
  /* Sleep to give Rviz time to visualize the plan. */
  sleep(10.0);

  // When done with the path constraint be sure to clear it.
  group.clearPathConstraints();



  ros::shutdown();  
  return 0;
}

So could you check the code and tell me about the mistakes?

Edit: @v4hn Thanks for your answer. I have used the computeCartesianPath and setPathConstraintsto constraint the orientation and let the robot move to a point next_pose. And the robot's orientation x,y,z,w did not changed in the process ... (more)