the extremal problem solution [@mayne66; @todorov05; @sideris05]. Motivated by their efficiency in solving regular optimal control problems, in this paper we have extended an SLQ algorithm to solve the optimal control problem for nonlinear switched system with predefined mode sequence. To this end, we adopt an approach introduced by [@xu04] where the primary switched problem is transcribed into an equivalent problem. We then introduce a two-stage optimization method to optimize the continuous inputs and the switching times. While [@xu04] use a computationally demanding approach based on solving a set of two-point BVPs, we propose a new SLQ algorithm to efficiently solve the optimal control problem for nonlinear switched systems.

The main contributions of this paper are: (1) it uses an efficient SLQ algorithm to synthesize the optimal control law for the continuous inputs. (2) it calculates the cost function derivative with respect to switching times using an LQ approximation of the problem. This approximation is obtained without any additional computation from the SLQ solution. (3) it introduces a new practical application of the optimal control for switched systems in the field of motion planning of legged robots.

Problem Formulation {#sec:problem}
===================

In this section, we briefly introduce the optimal control 