Optimal control of nonlinear systems with dynamic programming