 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.
<fluency>  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.
 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.