moveit path planning ignores constraints
I'm using moveit planning pipeline (indigo-devel) to generate a path for ur5 with some obstacles in the environment, but the generated path goes through the obstacles. The obstacles are represented by a mesh, which defines a collision object imported into the planning scene like this
planning_scene->processCollisionObjectMsg( collision_object );
When I publish the planning scene, I can see the obstacles in rviz. A path is then generated with
planning_pipeline->generatePlan( planning_scene, request, response );
which uses the planning scene with the defined constraints. However, the path visualised in rviz goes right through the obstacles as if they were not there. ROS outputs the following
[ INFO] [1420698166.546178298]: Planner configuration 'manipulator[ESTkConfigDefault]' will use planner 'geometric::EST'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1420698166.546475384]: manipulator[ESTkConfigDefault]: Attempting to use default projection.
[ INFO] [1420698166.549255239]: manipulator[ESTkConfigDefault]: Starting with 1 states
[ INFO] [1420698166.689949799]: manipulator[ESTkConfigDefault]: Created 3 states in 3 cells
[ INFO] [1420698166.690029336]: Solution found in 0.143339 seconds
[ INFO] [1420698166.690206928]: Path simplification took 0.000054 seconds
[ERROR] [1420698166.708755458]: Computed path is not valid. Invalid states at index locations: [ 4 5 ] out of 10. Explanations follow in command line. Contacts are published on /path_planning/display_contacts
[ INFO] [1420698166.710135263]: Found a contact between 'obstacle' (type 'Object') and 'wrist_2_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1420698166.710176768]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1420698166.713790237]: Found a contact between 'obstacle' (type 'Object') and 'wrist_2_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1420698166.713845565]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ERROR] [1420698166.716069182]: Completed listing of explanations for invalid states.
So, collision checking works fine and reports a collision. But the same obstacles are ignored by the path planning algorithm. What am I missing? What else do I need to do to make path planning take my collision objects into account? I was under the impression that just importing a collision object into the planning scene is sufficient. Obviously, something else needs to be done.
Have you found any solutions? I'm trying to figure this out as well. There is a related post ( https://groups.google.com/forum/#!top... ). It seemed that the planner initially found a solution to avoid obstacles but then the simplification process rejected it.