Merge pull request #184 from marioprats/groovy-devel

First version of benchmark tool
Merge branch 'groovy-devel' of https://github.com/ros-planning/moveit_ros into groovy-devel
Added TrajectoryConstraintsStorage for the warehouse