Hi all,

I would like my UR5's end-effector to have an orientation constraint. Here is the code I used to try to achieve that:

//Adding Path Constraints
    planning_interface::MotionPlanRequest req;
    std::vector<double> tolerance_pose(3, 0.01);
    std::vector<double> tolerance_angle(3, 0.01);
    geometry_msgs::PoseStamped pose;

moveit_msgs::Constraints target_pose =
kinematic_constraints::constructGoalConstraints("wrist_3_joint", pose, tolerance_pose, tolerance_angle);

geometry_msgs::QuaternionStamped quaternion;
quaternion.header.frame_id = "ee_link"; //"wrist_3_link";
quaternion.quaternion.x = 0;
quaternion.quaternion.y = 0;
quaternion.quaternion.z = 0;
quaternion.quaternion.w = 1;
req.path_constraints = kinematic_constraints::constructGoalConstraints("wrist_3_joint", quaternion);

I am pretty sure there is something wrong in this code, because the end effector sometimes changes his orientation during the execution of the plan.

Any help?

ROS kinetic, ubuntu 16.04

edit: full code here:

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit/planning_interface/planning_interface.h>
#include <moveit/kinematic_constraints/utils.h>

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

  static const std::string PLANNING_GROUP = "manipulator";
  moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

  namespace rvt = rviz_visual_tools;
  moveit_visual_tools::MoveItVisualTools visual_tools("world");

  moveit::planning_interface::PlanningSceneInterface planning_scene_interface; 

  visual_tools.loadRemoteControl(); 

  move_group.setMaxVelocityScalingFactor(0.05);
  move_group.setMaxAccelerationScalingFactor(0.10);
  move_group.setPlanningTime(0.1);
  move_group.allowReplanning(true);
  move_group.setPlannerId("RRTConnectkConfigDefault");

  geometry_msgs::Pose target_pose1;
  target_pose1.orientation.w = 1;
  target_pose1.position.x = 0.50;                   
  target_pose1.position.y = 0.00;                 
  target_pose1.position.z = 0.34;

  geometry_msgs::Pose target_pose2;
  target_pose2.orientation.w = 1;
  target_pose2.position.x = 0.00;                   
  target_pose2.position.y = 0.50;                   
  target_pose2.position.z = 0.34;

  // Adding Path Constraints
  planning_interface::MotionPlanRequest req;
  std::vector<double> tolerance_pose(3, 0.01);
  std::vector<double> tolerance_angle(3, 0.01);
  geometry_msgs::PoseStamped pose;

  moveit_msgs::Constraints target_pose =
  kinematic_constraints::constructGoalConstraints("ee_link", pose, tolerance_pose, tolerance_angle);

  geometry_msgs::QuaternionStamped quaternion;
  quaternion.header.frame_id = "base_link";
  quaternion.quaternion.x = 0;
  quaternion.quaternion.y = 0;
  quaternion.quaternion.z = 0;
  quaternion.quaternion.w = 1;
  req.path_constraints = kinematic_constraints::constructGoalConstraints ("wrist_3_joint", quaternion);

  req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y =
      req.workspace_parameters.min_corner.z = -2.0;
  req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y =
      req.workspace_parameters.max_corner.z = 2.0; 


  while (ros::ok()) {

    visual_tools.prompt("Press 'next'");

    move_group.setPoseTarget(target_pose1);

    move_group.asyncMove();

    move_group.getCurrentState();

    visual_tools.prompt("Press 'next'");

    move_group.setPoseTarget(target_pose2);

    move_group.asyncMove();

    move_group.getCurrentState(); 

  }


  ros::shutdown();
  return 0;
}