Orientation constraint violated for link 'j2s7s300_end_effector'

edit

Hi everyone,

I'm using: Kinova Jaco2 7DoF ROS-Melodic

I'm trying to set orientation constraints in order to execute a trajectory without rotation of the end-effector around "x" and "z" Axes with reference to EE-Frame. Like in the immage attached.

I wish to go from grasp_pose to bin_pose only with a rotation of the end-effector around "y" axis with reference to EE-Frame ("z axis with reference to Base-Frame).

tf::Quaternion q_1;

q_1 = EulerZYZ_to_Quaternion(-90*M_PI/180,-90*M_PI/180,-90*M_PI/180);

grasp_pose.header.frame_id = "root";
grasp_pose.pose.position.x=0.3;
grasp_pose.pose.position.y=0.3;
grasp_pose.pose.position.z=0.70;
grasp_pose.pose.orientation.x = q_1.x();
grasp_pose.pose.orientation.y = q_1.y();
grasp_pose.pose.orientation.z = q_1.z();
grasp_pose.pose.orientation.w = q_1.w();

tf::Quaternion q;

q = EulerZYZ_to_Quaternion(90*M_PI/180,-90*M_PI/180,-90*M_PI/180);

bin_pose.header.frame_id = "root";
bin_pose.pose.position.x=0.0;
bin_pose.pose.position.y= -0.40;
bin_pose.pose.position.z= 0.0;
bin_pose.pose.orientation.x = q.x();
bin_pose.pose.orientation.y = q.y();
bin_pose.pose.orientation.z = q.z();
bin_pose.pose.orientation.w = q.w();
ocm.weight = 1; //0.5;

// setup constrains
moveit_msgs::OrientationConstraint ocm;
ocm.link_name = robot_type_ + "_end_effector";
//current_pose_= group_->getCurrentPose(ocm.link_name);
//ocm.header.frame_id = current_pose_.header.frame_id;
ocm.orientation = current_pose_.pose.orientation;

ocm.header.frame_id = "j2s7s300_link_base";

ocm.absolute_x_axis_tolerance = 0.15;
ocm.absolute_y_axis_tolerance = 0.15;
ocm.absolute_z_axis_tolerance =2*M_PI;

moveit_msgs::Constraints test_constraints;
test_constraints.orientation_constraints.push_back(ocm);
group_->setPathConstraints(test_constraints);

I read something like this:

[ INFO] [1641915262.603077343]: Orientation constraint violated for link 'j2s7s300_end_effector'. Quaternion desired: 0.000000 0.000000 0.000000 1.000000, quaternion actual: 0.710450 0.001373 -0.001685 0.703744, error: x=1.580279, y=0.000462, z=0.004322, tolerance: x=0.150000, y=0.150000, z=6.283185

How can I fix this problem?