See my pull request: https://github.com/ros-planning/moveit_core/pull/106 and let me know what you think.