Trajectory Planning for Heterogeneous Robot Teams