Check reachability returns the trajectory plan in addition to the boolean