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

edit

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.