Orientation constraints from current pose
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.