Next point is introduce the optimization of trajectory by the differential kinematic
Next point is introduce the optimization of trajectory by the differential kinematic