Position constraint violated #1507
Description
Description
When using OMPL constrained planning, there are cases where the OMPL planner creates a path that satisfies a position constraint according to OMPL but does not satisfy it according to MoveIt's check. The reason for this is that kinematic_constraint uses a stricter check than OMPL's constraint implementation: kinematic_constraint uses the dimensions of a box to check if a point satisfies the constraint, while OMPL allows a tolerance beyond that (making the box slightly larger).
Your environment
- ROS Distro: main
- OS Version: Ubuntu 20.04
Steps to reproduce
For example, create a position constraint that represents a straight line by a box with dimensions [0.0005m, 0.0005m, 0.1m] and the default constraint tolerance (1e-4), and use the constrained planner to solve it.
Expected behaviour
Because the path is valid according to OMPL, it should also be considered valid by MoveIt.
Actual behaviour
The following error message is printed.
Position constraint violated on link 'left_tool_changer_robot_side'. Desired: 2.270036, -0.947986, 1.472917, current: 2.270319, -0.948200, 1.445913
[move_group-8] [INFO] [1660312455.351392589] [moveit_kinematic_constraints.kinematic_constraints]: Differences -0.000282794 0.000213868 0.0270041
The first two coordinates violate the constraint according to moveit because it uses the dimensions of the box to check if the constraint is satisfied. At the same time, these two coordinates are valid according to the check in OMPL because they are smaller than the constraint tolerance (1e-4).
Potential Solution
Add a tolerance field to the PositionConstraint and OrientationConstraint message definitions in moveit_msgs. Add padding equal to tolerance to the constraint region(s) of the position constraint and increment the three axes tolerances by tolerance in the orientation constraint in kinematic_constraints.cpp.