small changes, failed to call moveit compute_cartesian_path