Ask Your Question
1

Orientation constraints from current pose

asked 2017-10-11 02:58:21 -0500

jeffreyp23 gravatar image

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?:

image description

Thanks in advance.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2017-10-11 07:39:54 -0500

v4hn gravatar image
this->ocm.link_name = "ee_link";
this->ocm.header.frame_id = "base_link";
this->ocm.orientation = this->move_group.getCurrentPose().pose.orientation;

Are you sure getCurrentPose returns the pose in "base_link" and your current end-effector is "ee_link"? Try always to keep frame_ids and coordinates together and if you specify a hard-coded link name in one place, do the same for the rest of the code:

auto current_pose= this->move_group.getCurrentPose(this->ocm.link_name);
this->ocm.header.frame_id = current_pose.header.frame_id;
this->ocm.orientation = current_pose.pose.orientation;

It's hard to debug this further without more information on how you trigger the error.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2017-10-11 02:58:21 -0500

Seen: 556 times

Last updated: Oct 11 '17