Numerical solution of state constrained optimal control problems