0
$\begingroup$

Rosanswers logo

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.


Originally posted by Humberto Estrada on ROS Answers with karma: 41 on 2021-11-30

Post score: 2


Original comments

Comment by osilva on 2021-11-30:
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/pcl/issues/4661

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

Comment by Humberto Estrada on 2021-12-01:
Thank you very much for the comment. I think you meant this discussion https://github.com/ros-planning/moveit_msgs/issues/89, 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

Comment by osilva on 2021-12-01:
Sorry about the wrong link. Glad you found it and it helped.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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.


Originally posted by Humberto Estrada with karma: 41 on 2021-12-03

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by osilva on 2021-12-03:
Well done!

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.