stomp_moveit_interface: planning based on FCL works