t_{I-1}) | t_o \leq t_1 \leq \dots \leq t_{I-1} \leq t_I \right\}$.

The optimal control problem based on the parameterization of switching times can be defined as the following two-stage optimization problem $$\begin{aligned}
 \label{eq:two_stage_opt}
\min_{\vt \in \mathbb{T}} J[\vt,\vx^*,\vu^*] \quad \text{s.t. } \{\vx^*,\vu^*\} = \argmin J[\vt,\vx,\vu]. \end{aligned}$$ with $$\begin{aligned}
J[\vt,\vx,\vu] &= \Phi(\vx_{I})+ \sum_{i=1}^I{(t_{i}-t_{i-1}) \int_{i-1}^{i} {L_i(\vx(z),\vu(z))dz}}, \label{eq:equivalent_cost} \\
\frac{d\vx(z)}{dz} &= (t_{i}-t_{i-1}) \vf_i \left(\vx(z),\vu(z) \right) \qquad \text{\textit{for }} i-1 \leq z < i 
\label{eq:equivalent_system} \\
t &= (t_{i}-t_{i-1})(z-i) + t_{i}, \label{eq:z}\end{aligned}$$ in which we have replaced the independent time variable $t$ with a normalized time variable $z$ defined by . With this change of variable, while the switching times are still part of the decision variables, they are fixed parameters for the bottom-level optimization in . Therefore, the reformulated optimal control problem does not have variable switching times and it reduces to a conventional optimal control problem parameterized over the switching times (Theorem 1 in [@xu04]). In the next section, we introduce our algorithm for calculating the optimized cost function and its gradient.

Solution Approach {#sec:ocs2}
=================

In this section, we use a gradient-based method to solve the top-level optimization introduced in and a dynamic programming approach to synthesize the continuous inputs control 