Ask Your Question
2

Moveit. Plan path with relaxed orientation tolerances on the goal pose.

asked 2021-11-30 10:24:11 -0500

Humberto Estrada gravatar image

updated 2021-12-03 08:16:54 -0500

Hi everyone, I am using moveit in python through the moveit_commander module. I am using the normal RRT planner from the OMPL planners.

I want to plan and execute a motion to a goal with spherical obstacles nearby. An accurate final position is desired, but the orientation can vary a bit, specially in the angle alpha of the end effector from the top view. Actually, in some cases alpha has to be different to the one commanded as a goal in order to avoid obstacles.

In order to achieve this, I thought of giving wide tolerances for the orientation constraint corresponding to the angle alpha. I couldn't find exactly what orientation constraints refer to, but from my understanding they refer only to the final pose achieved in the plan.

These are the lines of my code relevant to this:

commander = moveit_commander.MoveGroupCommander(group_name)
...
constraint = moveit_msgs.msg.Constraints()

position_constraints = moveit_msgs.msg.PositionConstraint()
position_constraints.link_name = "tcp"
position_constraints.header.frame_id = "base_link"
position_constraints.target_point_offset = geometry_msgs.msg.Vector3(x=0.001, y=0.001, z=0.001)
position_constraints.weight = 1.0
constraint.position_constraints.append(position_constraints)

orientation_constraints = moveit_msgs.msg.OrientationConstraint()
orientation_constraints.link_name = "tcp"
orientation_constraints.header.frame_id = "base_link"
orientation_constraints.orientation = self.wpose.orientation
orientation_constraints.absolute_x_axis_tolerance = np.deg2rad(30)
orientation_constraints.absolute_y_axis_tolerance = np.deg2rad(30)
orientation_constraints.absolute_z_axis_tolerance = np.deg2rad(30)
orientation_constraints.weight = 1.0
constraint.orientation_constraints.append(orientation_constraints)

commander.set_path_constraints(constraint)

traj = commander.plan(goal)

I am not sure if the axis refer to the tcp or base_link coordinate system, therefore I just relaxed the constraints in all directions. I tested the code in a situation where the angle alpha given by the goal is 0, but there is a collision in that pose which can be prevented with an alpha angle of 10°, but the planner is not able to find any valid path. Then I set alpha in the goal to 10° and it finds a solution. I don't know why the relaxation of the constraints don't allow the planner to find that solution in the first case.

Edit: Now that I can add images, these are 2 images of the valid plan described in the previous parragraph

I would like help with these 3 questions:

  1. What coordinate system do the axes in the orientation constraints refer to? The link the constraint refer to or the reference coordinate system in the header?

  2. Do the orientation constraints refer only to the final pose achieved in the plan?

  3. Is if possible to do what i intend to in the way I am approaching it?

Than you very much in advance.

edit retag flag offensive close merge delete

Comments

1

Hi @Humberto Estrada. Your question is very interesting and well structured. I don't have a definite answer but after reading your question, I did a little research to find out more.

Refer to this discussion: https://github.com/PointCloudLibrary/...

In MoveIt, the bounds on the orientation constraints are evaluated as bounds on intrinsic XYZ Euler angles. The bounds are relative to a given target orientation (the "center" of the constraint region)

In other words you need to consider that the end effector reaches the projected vector as well but stays within bounds. Relaxing the constraints I think will just to add errors and affect the planner as you have seen. So perhaps you could add some sort of feedback to improve the precision or pre-planning with meta planning

osilva gravatar image osilva  ( 2021-11-30 17:09:23 -0500 )edit

Thank you very much for the comment. I think you meant this discussion https://github.com/ros-planning/movei..., but it was easy to find with the provided text. I understand better how the constraints work. That text answers the question 2 I made and help with the rest. I will look more into it and into the suggested meta planning and add an answer if I get to something working. Thank you

Humberto Estrada gravatar image Humberto Estrada  ( 2021-12-01 05:41:33 -0500 )edit

Sorry about the wrong link. Glad you found it and it helped.

osilva gravatar image osilva  ( 2021-12-01 05:45:29 -0500 )edit

1 Answer

Sort by » oldest newest most voted
2

answered 2021-12-03 08:15:04 -0500

Humberto Estrada gravatar image

updated 2021-12-03 08:26:42 -0500

As stated in the comments of the question, the orientation constraints bound the states allowed in the path, but does not affect the tolerances on the goal.

I used instead MoveGroupCommander.set_goal_orientation_tolerance (with a default value of 0.001) to relax the orientation tolerances on the goal, accepting paths ending in poses with different orientation.

I tested this in the same situation described in the question giving a tolerance of 20° (in radians) as shown in the next lines of code:

commander = moveit_commander.MoveGroupCommander(group_name)
...
commander.set_goal_orientation_tolerance(np.deg2rad(20))

And it worked. It found the path shown in the following image:

Disadvantage is that I can't give different tolerances for different axis, but it worked as I intended.

edit flag offensive delete link more

Comments

Well done!

osilva gravatar image osilva  ( 2021-12-03 08:37:26 -0500 )edit

Your Answer

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

Add Answer

Question Tools

2 followers

Stats

Asked: 2021-11-30 10:24:11 -0500

Seen: 49 times

Last updated: Dec 03 '21