added cost term for lambdas penalty on the trajectory points