Publish robot movement to moveit