One of the oldest problems in the study of dynamical systems is the calculation of an optimal control. Though the determination of a numerical solution for the general non-convex optimal control problem for hybrid systems has been pursued relentlessly to date, it has proven difficult, since it demands nominal mode scheduling. In this paper, we calculate a numerical solution to the optimal control problem for a constrained switched nonlinear dynamical system with a running and final cost. The control parameter has a discrete component, the sequence of modes, and two continuous components, the duration of each mode and the continuous input while in each mode. To overcome the complexity posed by the discrete optimization problem, we propose a bi-level hierarchical optimization algorithm: at the higher level, the algorithm updates the mode sequence by using a single-mode variation technique, and at the lower level, the algorithm considers a fixed mode sequence and minimizes the cost functional over the continuous components. Numerical examples detail the potential of our proposed methodology.