Removed misplaced call to `(clear-collision-objects)'
Added function to check a path of different poses for planability

The `pose-path-valid-p' function checks whether a list of poses are executable subsequently without colliding with the collision environment (according to the planning environment server). Since CHOMP doesn't support pose constraints (yet), only OMPL is used for this checking.