moveit path planning ignores constraints

asked 2015-01-08 00:46:50 -0500

andsok75 gravatar image

updated 2015-01-08 15:31:34 -0500

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.

edit retag flag offensive close merge delete

Comments

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.

johnyang gravatar image johnyang  ( 2016-04-06 03:02:56 -0500 )edit