  This study proposes an efficient Newton-type method for the optimal control
of switched systems under a given mode sequence. A mesh-refinement-based
approach is utilized to discretize continuous-time optimal control problems
(OCPs) and formulate a nonlinear program (NLP), which guarantees the local
convergence of a Newton-type method. A dedicated structure-exploiting algorithm
(Riccati recursion) is proposed to perform a Newton-type method for the NLP
efficiently because its sparsity structure is different from a standard OCP.
The proposed method computes each Newton step with linear time-complexity for
the total number of discretization grids as the standard Riccati recursion
algorithm. Additionally, the computation is always successful if the solution
is sufficiently close to a local minimum. Conversely, general quadratic
programming (QP) solvers cannot accomplish this because the Hessian matrix is
inherently indefinite. Moreover, a modification on the reduced Hessian matrix
is proposed using the nature of the Riccati recursion algorithm as the dynamic
programming for a QP subproblem to enhance the convergence. A numerical
comparison is conducted with off-the-shelf NLP solvers, which demonstrates that
the proposed method is up to two orders of magnitude faster. Whole-body optimal
control of quadrupedal gaits is also demonstrated and shows that the proposed
method can achieve the whole-body model predictive control (MPC) of robotic
systems with rigid contacts.
