Optimal control algorithms for second order systems