Initial version of stomp_moveit_interface