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](http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/pr2_tutorials/planning/src/doc/move_group_interface_tutorial.html#planning-with-path-constraints) 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
("/move_group/display_planned_path", 1, true);
rviz_updater = node_handle.advertise("/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()<
↧