ur5_barrett_reachability