Orientation constraints from current pose

edit

Hello,

I'm are trying to use orientation constraints to keep the end effector in the same orientation as the current pose, however I can't get it to work. It always gives the following error:

Orientation constraint violated for link 'ee_link'.

How is this possible? Because I set the constraint orientation from the current pose, and it already errors in the current pose.

I'm setting the orientation constraint as follows:

this->ocm.link_name = "ee_link";
this->ocm.header.frame_id = "base_link"
this->ocm.weight = 1.0;

this->ocm.orientation = this->move_group.getCurrentPose().pose.orientation;
this->ocm.absolute_x_axis_tolerance = 0.1;
this->ocm.absolute_y_axis_tolerance = 0.1;
this->ocm.absolute_z_axis_tolerance = 2.0 * 3.14;

this->constraints.orientation_constraints.push_back(this->ocm);
this->move_group.setPathConstraints(this->constraints);

Our goal is to keep the end effector up, so it won't drop the object (a plant) the end effector is holding. I want to restrict the orientation, so it may only use yaw to rotate in it's path.

Maybe something is wrong with our tf tree?:

Thanks in advance.