In one embodiment, a method, apparatus, and system for planning a trajectory for an autonomous driving vehicle (ADV) is disclosed. The operations comprise: receiving a plurality of optimization inputs, the plurality of optimization inputs comprising a trajectory time length, a time discretization resolution, an autonomous driving vehicle (ADV) starting state, a road shape function, a maximal jerk, and a maximal lateral acceleration; receiving a plurality of optimization constraints, the plurality of optimization constraints comprising constraints relating to the maximal jerk and the maximal lateral acceleration; receiving a cost function associated with an optimization objective, the cost function comprising a first term relating to cumulative jerk, a second term relating to an end longitudinal position, a third term relating to an end longitudinal speed, and a fourth term relating to an end longitudinal acceleration; generating a plurality of planned ADV states as optimization results with nonlinear optimization, wherein the optimization results minimize a value of the cost function; and generating control signals to control the ADV based on the plurality of planned ADV states.