Before revision:  First, we use kinodynamic path finding method to obtain smooth, dynamically feasible, time-optimal initial paths and added collision cost as a soft constraint to ensure safety.
After revision:  First, we use kinodynamic path finding method to obtain smooth, dynamically feasible, time-optimal initial paths and add collision cost as a soft constraint to ensure safety.