\section{Hybrid Modeling}


\end{document}


\section{Introduction} 
\label{sec:intro}

Safety is of significant importance in many modern control applications, with complex systems demanding the rigorous encoding of safety properties in the controller design process. One particularly challenging example is the safe locomotion of bipedal robots in difficult terrain; a concrete example of this is the ``stepping-stone problem'' where the feet must be placed in precise locations. In practice, the models used to design safety-critical controllers are imperfect---especially in the context of highly dynamic systems like walking robots---with model uncertainty arising due to parametric error and unmodeled dynamics. This uncertainty can lead to unsafe or dangerous behavior, thus requiring that safety-critical control be understood in the presence of model uncertainty.

In this work we consider a machine learning based approach for attenuating the impact of model uncertainty on the safe behavior of a system. The application of learning to safety-critical control of systems with uncertain models has recently exhibited significant success \citep{taylor2019learning2,cheng2019accelerating, choi2020reinforcement, lee2020learning, castaneda2020gaussian}. We look to achieve safety defined in terms of \textit{set invariance} \citep{blanchini1999set}, which is an area of active research at the intersection of data-driven methods and nonlinear control theory \citep{berkenkamp2016safe2, fisac2018general, wang2018safe, fan2019bayesian, dean2020guaranteeing}. 

We will leverage control barrier functions (CBFs) for synthesizing safety controllers \citep{ames2014control, ames2017control} that achieve set invariance. CBFs have become a popular method for ensuring safety \citep{nguyen2016exponential, wang2018safe}, but they require an accurate model of the system dynamics and model uncertainty can result in a loss of safety guarantees \citep{kolathaya2018input}. Efforts have been made to make the safety guarantees of CBFs robust to the effects of model uncertainty, but in doing so these methods can cause the system's behavior to be overly conservative \citep{gurriet2018towards, xu2015robustness, taylor2019adaptive}. 

One challenge in incorporating learning methods with nonlinear control is the need for diverse data capturing system behavior, and in particular the input-to-state relationship. While in simulated environments it is possible to collect high coverage data sets that accurately determine how inputs affect the evolution of the system, collecting such data on real physical systems may be prohibitively costly or damaging to the system. The lack of this data can lead to challenges with under-determination in supervised learning problems that seek to preserve the underlying structure of dynamic systems \citep{taylor2019episodic, taylor2019learning2}. Often this leads to model fits with low training loss that result in poor control performance when integrated in closed-loop controllers.

One setting for which collecting diverse data is infeasible is bipedal locomotion, as consistent walking requires well defined input profiles to ensure stability \citep{grizzle2001asymptotically}. The stepping-stone task is a historical benchmark for evaluating the safety-critical control of biped platforms \citep{nguyen20163d, nguyen2018dynamic, nguyen2020dynamic}. As compared to the statically stable motion which has been successfully employed to accomplish this task on a variety of legged robotic platforms \citep{griffin2019footstep, fankhauser2018robust}, the underactuated nature of the bipedal robot under consideration in this work requires dynamic gaits to successfully traverse a set of stepping-stones. During such dynamic motion, satisfaction of safety constraints is predicated on having accurate model information, a requirement that cannot be met on hardware platforms, thus suggesting the need for model uncertainty reduction via learning.

\textbf{Our Contribution:} The main contributions of this work are two-fold. First, we propose an episodic learning framework for iteratively reducing the impact of disturbances on the safety-critical behavior of a system. Compared to previous work, our method resolves challenges with input data diversity by recursively defining disturbance estimators and controllers that are agnostic to the underlying control-affine structure. We successfully deploy this method in the context of the classical bipedal robotic stepping-stone problem. We validate that this method is able to effectively reduce model uncertainty and improve the safe behavior of a complex system both in simulation and experiment. The second contribution is the first experimental demonstration of CBFs for safety-critical control on a bipedal robot. This challenge has been the focus of significant prior work and we demonstrate that it can be achieved through the introduction of learning.






\section{Background} 
\label{sec:background}
In this section we provide a review of CBFs and projection-to-state safety (PSSf), which will be utilized in Section \ref{sec:learning} to formulate a learning approach for mitigating model uncertainty.

\subsection{Safety via Control Barrier Functions}
Consider the nonlinear control affine system given by:
\begin{equation}
    \label{eqn:eom}
    \dot{\mb{x}} = \mb{f}(\mb{x})+\mb{g}(\mb{x})\mb{u},
\end{equation}
where $\mb{x}\in\mathbb{R}^n$, $\mb{u}\in\mathbb{R}^m$, and $\mb{f}:\mathbb{R}^n\to\mathbb{R}^n$ and $\mb{g}:\mathbb{R}^n\to\mathbb{R}^{n\times m}$ are locally Lipschitz continuous on $\mathbb{R}^n$. Given a Lipschitz continuous state-feedback controller $\mb{k}:\mathbb{R}^n\to\mathbb{R}^m$, the closed-loop system dynamics are:
\begin{equation}
    \label{eqn:cloop}
    \dot{\mb{x}} = \mb{f}_{\textrm{cl}}(\mb{x}) \triangleq  \mb{f}(\mb{x})+\mb{g}(\mb{x})\mb{k}(\mb{x}).
\end{equation}
The assumption of local Lipschitz continuity of $\mb{f}$, $\mb{g}$, and $\mb{k}$ implies that $\mb{f}_\textrm{cl}$ is locally Lipschitz continuous. Thus, for any initial condition $\mb{x}_0 := \mb{x}(0) \in \mathbb{R}^n$ there exists a maximum time interval $I(\mb{x}_0) = [0, t_{\textrm{max}})$ such that $\mb{x}(t)$ is the unique solution to \eqref{eqn:cloop} on $I(\mb{x}_0)$ \citep{perko2013differential}. In the case that $\mb{f}_{\textrm{cl}}$ is forward complete, $t_{\textrm{max}}=\infty$.

The notion of safety that we consider in this paper is formalized by specifying a \textit{safe set} in the state space that the system must remain in to be considered safe. In particular, consider a set $\mathcal{C}\subset \mathbb{R}^n$ defined as the 0-superlevel set of a continuously differentiable function $h:\mathbb{R}^n\to \mathbb{R}$, yielding:
\begin{align}
    \mathcal{C} &\triangleq \left\{\mb{x} \in \mathbb{R}^n : h(\mb{x}) \geq 0\right\}, \label{eqn:safeset}
\end{align}
with $\partial\mathcal{C} \triangleq \{\mb{x} \in \mathbb{R}^n : h(\mb{x}) = 0\}$ and
    $\textrm{Int}(\mathcal{C}) \triangleq \{\mb{x} \in \mathbb{R}^n : h(\mb{x}) > 0\}$. We assume that $\mathcal{C}$ is nonempty and has no isolated points, that is, $\textrm{Int}(\mathcal{C}) \not = \emptyset \textrm{ and }\overline{\textrm{Int}(\mathcal{C})} = \mathcal{C}$. We refer to $\mathcal{C}$ as the \textit{safe set}. This construction motivates the following definitions of forward invariance and safety:

\begin{definition}[\textit{Forward Invariance \& Safety}]
A set $\mathcal{C}\subset\mathbb{R}^n$ is \textit{forward invariant} if for every $\mb{x}_0\in\mathcal{C}$, the solution $\mb{x}(t)$ to \eqref{eqn:cloop} satisfies $\mb{x}(t) \in \mathcal{C}$ for all $t \in I(\mb{x}_0)$. The system \eqref{eqn:cloop} is \textit{safe} on the set $\mathcal{C}$ if the set $\mathcal{C}$ is forward invariant.
\end{definition}

Before defining CBFs, we denote a continuous function $\alpha:[0,a)\to\mathbb{R}_+$, with $a>0$, as \textit{class $\cal{K}$} ($\alpha\in\cal{K}$) if $\alpha(0)=0$ and $\alpha$ is strictly monotonically increasing. If $a=\infty$ and $\lim_{r\to\infty}\alpha(r)=\infty$, then $\alpha$ is \textit{class $\cal{K}_\infty$} ($\alpha\in\cal{K}_\infty$). A continuous function $\alpha:(-b,a)\to\mathbb{R}$, with $a,b>0$, is said to belong to \textit{extended class $\cal{K}$} ($\alpha\in\cal{K}_e$) if $\alpha(0)=0$ and $\alpha$ is strictly monotonically increasing. If $a,b=\infty$, $\lim_{r\to\infty}\alpha(r)=\infty$, and $\lim_{r\to-\infty}\alpha(r)=-\infty$, then $\alpha$ is said to belong to \textit{extended class $\cal{K}_\infty$} ($\alpha\in\cal{K}_{\infty,e}$).

Certifying the safety of the closed-loop system \eqref{eqn:cloop} with respect to a set $\mathcal{C}$ may be impossible if the controller $\mb{k}$ was not chosen to enforce the safety of $\mathcal{C}$. CBFs can serve as a synthesis tool for attaining the forward invariance, and thus the safety of a set:
\begin{definition}[\textit{Control Barrier Function (CBF)} \citep{ames2017control}]
Let $\mathcal{C}\subset\mathbb{R}^n$ be the 0-superlevel set of a continuously differentiable function $h:\mathbb{R}^n\to\mathbb{R}$ with $0$ a regular value. The function $h$ is a \textit{control barrier function} (CBF) for \eqref{eqn:eom} on $\mathcal{C}$ if there exists $\alpha\in\mathcal{K}_{\infty,e}$ such that for all $\mb{x}\in\mathbb{R}^n$:
\begin{align}
\label{eqn:cbf}
     \sup_{\mb{u}\in\mathbb{R}^m} \dot{h}(\mb{x},\mb{u}) \triangleq \nabla h(\mb{x})\left(\mb{f}(\mb{x})+\mb{g}(\mb{x})\mb{u}\right) = L_{\mb{f}}h(\mb{x}) + L_{\mb{g}}h(\mb{x}) \u \geq-\alpha(h(\mb{x})),
\end{align}
where $L_{\mb{f}}h(\mb{x})$ and $L_{\mb{g}}h(\mb{x})$ are Lie derivatives.
\end{definition}
\noindent Given a CBF $h$ for \eqref{eqn:eom} and a corresponding $\alpha\in\cal{K}_{\infty,e}$, we define the point-wise set of all control values that satisfy \eqref{eqn:cbf}:
\begin{equation*}
    K_{\textrm{cbf}}(\mb{x}) \triangleq \left\{\mb{u}\in\mathbb{R}^m ~\left|~ \dot{h}(\mb{x},\mb{u})\geq-\alpha(h(\mb{x})) \right. \right\}.
\end{equation*}
We have the following result relating controllers taking values in  $K_{\textrm{cbf}}(\mb{x})$ to the safety of \eqref{eqn:eom} on $\mathcal{C}$:
\begin{theorem}[\cite{ames2014control}]
Given a set $\mathcal{C}\subset\mathbb{R}^n$ defined as the 0-superlevel set of a continuously differentiable function $h:\mathbb{R}^n\to\mathbb{R}$, if $h$ is a CBF for \eqref{eqn:eom} on $\mathcal{C}$, then any Lipschitz continuous controller $\mb{k}:\mathbb{R}^n\to\mathbb{R}^m$, such that $\mb{k}(\mb{x})\in K_{\textrm{cbf}}(\mb{x})$ for all $\mb{x}\in\mathbb{R}^n$, renders the system \eqref{eqn:eom} safe with respect to the set $\mathcal{C}$.
\end{theorem}
Given a nominal (but not necessarily safe) locally Lipschitz continuous controller $\mb{k}_d:\mathbb{R}^n\to\mathbb{R}^m$, a controller taking values in the set $K_\textrm{cbf}(\mb{x})$ is the safety-critical CBF-QP:
\begin{align}
\label{eqn:CBF-QP}
\tag{CBF-QP}
\mb{k}(\mb{x}) =  \,\,\underset{\mb{u} \in \mathbb{R}^m}{\argmin}  &  \quad \frac{1}{2} \| \mb{u} -\mb{k}_d(\mb{x})\|_2^2  \\
\mathrm{s.t.} \quad & \quad \dot{h}(\mb{x},\mb{u})
\geq -\alpha(h(\mb{x})). \nonumber
\end{align}

\subsection{Model Uncertainty and Projection-to-State Safety}
\label{sec:uncertainty}

In practice, the system dynamics \eqref{eqn:eom} are not known during  control design  due to parametric error and unmodeled dynamics. Instead, a nominal model of the system is utilized:
\begin{equation}
\label{eqn:eomnominal}
    \widehat{\dot{\mb{x}}} = \widehat{\mb{f}}(\mb{x}) + \widehat{\mb{g}}(\mb{x})\mb{u},
\end{equation}
where $\widehat{\mb{f}}:\mathbb{R}^n\to\mathbb{R}^n$ and $\widehat{\mb{g}}:\mathbb{R}^n\to\mathbb{R}^{n\times m}$ are assumed to be Lipschitz continuous on $\mathbb{R}^n$. By adding and subtracting the right hand side of \eqref{eqn:eomnominal} to \eqref{eqn:eom}, the dynamics of the system are:
\begin{equation}
    \dot{\mb{x}} = \widehat{\mb{f}}(\mb{x}) + \widehat{\mb{g}}(\mb{x})\mb{u} + \overbrace{\underbrace{\mb{f}(\mb{x})-\widehat{\mb{f}}(\mb{x})}_{\mb{b}(\mb{x})}+\underbrace{\left(\mb{g}(\mb{x})-\widehat{\mb{g}}(\mb{x})\right)}_{\mb{A}(\mb{x})}\mb{u}}^{\mb{d}(\mb{x},\u)},
\end{equation}
where the unknown disturbance $\mb{d}(\mb{x},\u)=\mb{b}(\mb{x})+\mb{A}(\mb{x})\mb{u}$ is assumed to be time invariant, but depends on the state and input to the system. If the function $h:\mathbb{R}^n\to\mathbb{R}$ is a CBF for the nominal model \eqref{eqn:eomnominal} on $\mathcal{C}$, the uncertainty in the dynamics directly manifests in the time derivative of $h$:
\begin{align}
\label{eqn:hdotdecomp}
    \dot{h}(\mb{x},\mb{u})  =  \underbrace{\nabla h(\mb{x})(\widehat{\mb{f}}(\mb{x})+\widehat{\mb{g}}(\mb{x})\mb{u})}_{\widehat{\dot{h}}(\mb{x},\mb{u})} 
    + \underbrace{\nabla h(\mb{x})\mb{b}(\mb{x})}_{b(\mb{x})} + \underbrace{\nabla h(\mb{x})\mb{A}(\mb{x})}_{\mb{a}(\mb{x})^\top}\mb{u}.
\end{align}
Given that $h$ is a CBF for \eqref{eqn:eomnominal} on $\mathcal{C}$, let $\mb{k}:\mathbb{R}^n\to\mathbb{R}^m$ be a Lipschitz continuous state-feedback controller such that $\widehat{\dot{h}}(\mb{x},\mb{k}(\mb{x})) \geq -\alpha(h(\mb{x}))$. Defining the \textit{projected disturbance} as: 
\begin{equation}
\label{eqn:projdist}
\delta(\mb{x}) \triangleq \dot{h}(\mb{x},\mb{k}(\mb{x})) - \widehat{\dot{h}}(\mb{x},\mb{k}(\mb{x})) = b(\mb{x})+\mb{a}(\mb{x})^\top\mb{k}(\mb{x}),
\end{equation}
yields:
\begin{equation}
\label{eqn:hdottrueineq}
    \dot{h}(\mb{x},\mb{k}(\mb{x})) \geq -\alpha(h(\mb{x})) - \delta(\mb{x})
\end{equation}
Assuming that $\delta$ is essentially bounded in time (there exists $M\in\mathbb{R}$, $M>0$, such that $\vert\delta\vert_\infty  \triangleq\esssup_{t\geq 0}\vert\delta(\mb{x}(t))\vert< M$), we may make use of the following definition:
\begin{definition}[\textit{Projection-to-State Safety (PSSf)} (\cite{taylor2020control})]
Given a feedback controller $\mb{k}$, the closed-loop system \eqref{eqn:cloop}, $\dot{\mb{x}} = \mb{f}_{\textrm{cl}}(\mb{x}) = \widehat{\mb{f}}(\mb{x}) + \widehat{\mb{g}}(\mb{x})\mb{k}(\mb{x}) + \mb{d}(\mb{x})$ with $\mb{d}(\mb{x})=\mb{b}(\mb{x})+\mb{A}(\mb{x})\mb{k}(\mb{x})$,  is \emph{projection-to-state safe} (PSSf) on $\mathcal{C}$ with respect to the function $h$ and projected disturbances $\delta:\mathbb{R}^n\to\mathbb{R}$ if there exists $\overline{\delta}>0$ and $\gamma\in\mathcal{K}_\infty$ such that the set $\mathcal{C}_{\delta}\supset\mathcal{C}$,
\begin{align}\label{eq:delta_safe_set}
    \mathcal{C}_{\delta} &\triangleq \left\{\mb{x} \in \mathbb{R}^n : h(\mb{x})+\gamma(\vert\delta\vert_\infty) \geq 0\right\},
\end{align}
is forward invariant for all $\delta$ satisfying $\vert\delta\vert_\infty\leq\overline{\delta}$.
\end{definition}


PSSf captures the fact that in the presence of model uncertainty, satisfying the CBF condition \eqref{eqn:cbf} for the estimated time derivative $\widehat{\dot{h}}$ is not sufficient for safety, as the projected disturbance $\delta$ appears in the lower bound on true time derivative of $h$ as in \eqref{eqn:hdottrueineq}. This results in a larger forward invariant set, given by $\mathcal{C}_\delta$, that grows with the magnitude of the projected disturbance.


\begin{figure}
    \centering
    \includegraphics{Figures/PssfCBF.pdf}
    \caption{Geometric visualization of Projection-to-State Safety. The system is able to leave the safe set $\mathcal{C}$, but remains within the larger set $\mathcal{C}_\delta$. Maximum possible deviation from the safe set grows larger with $\vert\delta\vert_\infty$.}
    \label{fig:pssf}
    \vspace{-3mm}
\end{figure}


   
   
   





\section{Learning Projected Disturbances}
\label{sec:learning}
In this section we explore how an estimate of the projected disturbance $\delta$ can be learned episodically from data and incorporated into control synthesis to improve PSSf behavior. 

As seen in Section \ref{sec:background}, the projected disturbance $\delta$ appears in the time derivative of the barrier function $\dot{h}$, and potentially leads to unsafe behavior since it compromises the CBF condition as in \eqref{eqn:hdottrueineq}. If an upper bound $\overline{\delta}$ on $\vert\delta\vert_\infty$ is known (or determined heuristically), it could be directly incorporated into the inequality enforced in the controller:
\begin{align}
    \label{eqn:dumbreject}
    \tag{$\overline{\delta}$-CBF-QP}
    \mb{k}(\mb{x}) =  \,\,\underset{\mb{u} \in \mathbb{R}^m}{\argmin}  &  \quad \frac{1}{2} \| \mb{u}-\mb{k}_d(\mb{x}) \|_2^2  \\
   
    \mathrm{s.t.} \quad & \quad \widehat{\dot{h}}(\mb{x}, \u) - \overline{\delta}
    \geq -\alpha(h(\mb{x})). \nonumber
\end{align}
While this will enforce safety of the original set $\mathcal{C}$, it can be exceedingly conservative if $\overline{\delta}$ is larger than the actual projected disturbance. Furthermore, as the projected disturbance is a function of the state, its magnitude (and possibly sign) may change along a trajectory, leading to additional conservativeness in this approach.

Instead, we consider a learning approach to resolve the impact of $\delta$. To motivate such an approach, consider the following setting: in an experiment, the system is allowed to evolve forward in time from a particular initial condition and under a given state-feedback controller. During this experiment, data is collected which provides a discrete-time history of the CBF, $h$. This time history is smoothed and numerically differentiated to compute an approximate time history of the true value of the time derivative of the CBF, $\dot{h}$. This yields a collection of input-output pairs: 
\begin{equation}
    {D}_i = ((\mb{x}_i, \mb{k}(\mb{x}_i)), \dot{h}_i) \in (\mathbb{R}^n \times \mathbb{R}^m) \times \mathbb{R}
\end{equation}
whereby a dataset $\mathfrak{D}=\{{D}_i\}_{i=1}^N$ can be constructed. Given a nonlinear function class $\mathcal{H}: \mathbb{R}^n \to \mathbb{R}$ and a loss function $\mathcal{L}: \mathbb{R} \times \mathbb{R} \to \mathbb{R} $, a learning problem can be specified as finding a function $\widehat{\delta} \in \mathcal{H}$ to estimate $\delta$ via empirical risk minimization: 
\begin{equation}
    \label{eqn:erm}
    \tag{ERM}
    \inf_{\widehat{\delta} \in \mathcal{H}}\frac{1}{N}\sum_{i=1}^N\cal{L}\left(\widehat{\dot{h}}(\mb{x}_i,\mb{k}(\mb{x}_i))+ \widehat{\delta}(\mb{x}_i),\dot{h}_i\right).
\end{equation}
A controller can be synthesized which incorporates $\widehat{\delta}$ as follows: 
\begin{align}
    \label{eqn:delta-CBF-QP}
    \tag{$\widehat{\delta}$-CBF-QP}
    \mb{k}(\mb{x}) =  \,\,\underset{\mb{u} \in \mathbb{R}^m}{\argmin}  &  \quad \frac{1}{2} \| \mb{u}-\mb{k}_d(\mb{x}) \|_2^2  \\
   
    \mathrm{s.t.} \quad & \quad \widehat{\dot{h}}(\mb{x}, \u) + \widehat{\delta}(\mb{x}) 
    \geq -\alpha(h(\mb{x})). \nonumber
\end{align}
Note that compared with \ref{eqn:CBF-QP}, the extended safe set with \ref{eqn:delta-CBF-QP} shrinks from \eqref{eq:delta_safe_set} to  $\mathcal{C}_{\delta} = \left\{\mb{x} \in \mathbb{R}^n : h(\mb{x})+\gamma(\vert\delta-\hat{\delta}\vert_\infty) \geq 0\right\}$. 

We directly build upon the episodic learning framework from \citep{taylor2019episodic, taylor2019learning2} by seeking to learn $\delta$. Our approach is outlined in Algorithm 1.  In each episode, the  algorithm runs the current controller to collect data, learns a new $\widehat{\delta}$ using the newly collected data, and synthesizes a new controller. In this prior work, which was applied to less complex dynamical systems, the collected data was rich enough to determine a control affine structure. In many contexts, such as bipedal robots, such a degree of diversity is infeasible without damaging the system. We instead directly learn $\delta$ as a function of the previous controller $\mb{k}$ via a recursive relationship, as updating the estimator leads to the definition of a new projected disturbance $\delta' = b(\mb{x})+\mb{a}(\mb{x})^\top\mb{k}'(\mb{x})$. This yields a projected disturbance $\delta$  learned iteratively by modifying $\widehat{\delta}$ over the course of multiple episodes. This episodic approach to safety-critical control is captured in Algorithm \ref{alg:episodic_learning}.

\begin{algorithm}[H]
    \SetAlgoLined
    \caption{Projected Disturbance Learning (PDL)}
    \SetKwFunction{Sample}{sample}\SetKwFunction{Experiment}{experiment} \SetKwFunction{ERM}{ERM}\SetKwFunction{Augment}{augment}
    \SetKwFunction{LCBFQP}{$\widehat{\delta}$-CBF-QP}
    \SetKwInOut{Input}{input} \SetKwInOut{Output}{output}
    \label{alg:episodic_learning}
    \Input{CBF $h$, CBF derivative estimate $\widehat{\dot{h}}$, model class $\mathcal{H}$, loss function $\mathcal{L}$, nominal state-feedback controller $\mb{k}_0$, number of episodes $T$, initial condition $\mb{x}_0$}
    \Output{Augmented Controller $\mb{k}_T$}
    \BlankLine
    \For{$j = 1, \ldots, T $}{
       
        $\mathfrak{D}_j \leftarrow$\Experiment{$\mb{x}_0,\mb{k}_{j-1}$}  \hspace{2.438cm}\tcp*[h]{Execute experiment}\;\\
        $\widehat{\delta}\leftarrow$\ERM{$\cal{H}, \cal{L},\mathfrak{D}_j,\widehat{\dot{h}}_0$}  \hspace{3.7cm}\tcp*[h]{Fit estimator}\;\\
        $\widehat{\dot{h}}_j\leftarrow \widehat{\dot{h}}_0+\widehat{\delta}$  \hspace{5.49cm}\tcp*[h]{Update derivative estimator}\;\\
        $\mb{k}_j\leftarrow$ \LCBFQP{$\widehat{\dot{h}}_j$} \hspace{3.9cm}\tcp*[h]{Synthesize new controller}\;
    }
\end{algorithm}






\section{Bipedal Robotics} 
\label{sec:bipeds}
In this section we specify the notion of learning projected disturbances to the setting of bipedal locomotion. We briefly introduce the theory of bipedal locomotion and then describe the barrier function formulations which allow us to achieve safe bipedal locomotion across stepping-stones. A deeper exploration of this material may be found in \citep{westervelt2003hybrid}.

\subsection{Bipedal Robotic Dynamics}
\label{sec:bipedsdynamics}
The bipedal robotic system we consider is the AMBER-3M robotic platform seen in Figure \ref{fig:amber_and_barrier}, modeled as an underactuated, planar five-link robot with point feet \citep{ambrose2017toward} whose physical parameters are reported in \cite[Table 1]{ma2014human}. The configuration coordinates $\mb{q}\in \mathcal{Q}\subset \mathbb{R}^5$ are given by
$
   
    \mb{q} = [q_{sf},\ q_{sk},\ q_{sh},\ q_{nsh},\ q_{nsk}]^\top,
$
with stance foot angle $q_{sf}$, stance knee angle $q_{sk}$, stance hip angle $q_{sh}$, non-stance hip angle $q_{nsh}$ and non-stance knee angle $q_{nsk}$. The continuous-time equations of motion, derived from the Euler-Lagrange equations, are given by: 
\begin{align}
    \mb{D}(\mb{q})\ddot{\mb{q}}+\mb{C}(\mb{q},\dot{\mb{q}})\dot{\mb{q}}+\mb{G}(\mb{q})&=\mb{Bu}, \label{roboDyn}
\end{align}
where $\mb{D}(q)\in{\mathbb{S}}_{++}^{5}$ is the positive definite mass-inertia matrix,  
$\mb{C}(q,\dot{q}) \in \mathbb{R}^{5\times 5}$ contains the centrifugal and Coriolis forces, $\mb{G}(q)\in \mathbb{R}^{5}$ contains the gravitational forces, $\mb{B}\in \mathbb{R}^{5\times 4}$ is the actuation matrix, and $\mb{u}\in\mathcal{U}\subset\mathbb{R}^4$ is the input. Note that this is the pinned model of the robot dynamics; for the unpinned model, refer to \citep{hereid2018dynamic}. For AMBER-3M, the number of inputs is one fewer than the degrees of freedom, meaning the system has one degree of underactuation.

Taking $p^v:\mathcal{Q}\to \mathbb{R}$ to represent the vertical position (height) of the swing foot, the admissible states are given by the \textit{domain} $\mathcal{D} = \{(q,\mb{\dot{q}})\in T\mathcal{Q}\ |\ p^v(\mb{q}) \ge 0\}$. The switching surface on which the impact events occur, also known as the \textit{guard}, is defined by:
\begin{align}
    \mathcal{S} = \{(\mb{q},\dot{\mb{q}})\in T\mathcal{Q} ~|~ p^v(q) = 0, \dot{p}^v(q,\dot{q}) < 0\}\subset \mathcal{D}.
\end{align}
With the full system state given by $\mb{x}=(\mb{q},\dot{\mb{q}})\in T\mathcal{Q}$, the impact dynamics \citep{westervelt2007feedback} are defined by a \textit{reset map} $\bs{\Delta}:\mathcal{S}\to \mathcal{D}$ relating pre-impact states $\mb{x}^-(t) \triangleq \lim_{\tau \nearrow t} \mb{x}(\tau)$ and post-impact states  $\mb{x}^+(t) = \lim_{\tau \searrow t} \mb{x}(\tau)$ via $\mb{x}^+(t) = \bs{\Delta}(\mb{x}^-(t))$. Combining these concepts and rearranging (\ref{roboDyn}) into control affine form yields the following \textit{hybrid control system}:
\begin{equation}\label{eq:hybrid_model_1}
\mathcal{HC} = \begin{cases} \dot{\mb{x}} = \mb{f}(\mb{x}) + \mb{g}(\mb{x}) \mb{u} \quad &\mb{x}^- \in \mathcal{D} \setminus \mathcal{S},\\  
\mb{x}^+= \bs{\Delta}(\mb{x}^-) &\mb{x}^- \in \mathcal{S}.\end{cases}    
\end{equation}
\subsection{Bipedal Robotic Control}
\label{sec:bipedcontrol}
Control of bipedal robotic systems centers around a \textit{phasing variable}, $\tau:\mathcal{Q}\to [0,1]$, given by:
\begin{equation} \label{eq:phase_variable}
    \tau(\mb{q}) = \frac{\delta_{hip}(\mb{q}) - \delta_{hip}^+}{\delta_{hip}^- -\delta_{hip}^+},
\end{equation}
where $\delta_{hip}:\mathbb{R}^5\to\mathbb{R}$ defined as $\delta_{hip}(\mb{q}) = [-l_t-l_f,\ -l_f,\ 0,\ 0,\ 0]\mb{q}$ is the linearized hip position with $l_t$ and $l_f$ the length of the robots tibia and femur, respectively. The constants \(\delta_{hip}^+\) and \(\delta_{hip}^-\) are the linearized hip positions at the beginning and the end of a step, ensuring that $\tau(\mb{q})$ increases monotonically in time within a step. Desired trajectories resulting in walking gaits for the robot can be rapidly synthesized via a hybrid zero dynamics framework (\cite{westervelt2003hybrid,ames2014rapidly}).
We are now well equipped to define the relative degree 2 (\cite{sastry1999nonlinear}) outputs $\mb{y}:\mathcal{Q}\to \mathbb{R}^4$ as the difference between the actual output $\mb{y}_a$ and the desired output trajectory $\mb{y}_d$:
\begin{equation}
    \mb{y}(\mb{q},\bs{\alpha}) \triangleq \mb{y}_a(\mb{q}) - \mb{y}_d(\tau(\mb{q}),\bs{\alpha}),
\end{equation}
with $\bs{\alpha}$ being the coefficients of a Bézier polynomial coming from the trajectory generation step. The actual output is given by the actuated coordinates:
$
    \mb{y}_a(\mb{q}) = \begin{bmatrix} \mb{0}_{4\times 1} & \mb{I}_{4\times 4} \end{bmatrix}\mb{q}.
$
The nominal controller for this system is then given by the proportional-derivative controller $\mb{k}_d(\mb{x}) = \mb{k}_{PD}(\mb{x}) \triangleq -\mb{K}_P \mb{y}(q) - \mb{K}_D \dot{\mb{y}}(q)$ with proportional gain $\mb{K}_P\in\mathbb{S}_{++}^4$ and derivative gain $\mb{K}_D\in\mathbb{S}_{++}^4$. 
   






\begin{figure}[ht!]
\centering
\includegraphics[width=\textwidth]{Figures/confBarrier.pdf}
	\caption{\textbf{(Left):} Schematic diagram of the AMBER-3M robot with position coordinates. \textbf{(Center):} Schematic of the foot placement in the stepping-stone problem. The boundaries of virtual stepping-stones are captured via the blue and orange vertical lines. \textbf{(Right):} Virtual stepping stone width as function of the phase variable $\tau(\mb{q})$.}
	\label{fig:amber_and_barrier}
\end{figure}

\subsection{Control Barrier Functions for Stepping-Stones}







\noindent The stepping-stone problem is captured through the use of \textit{virtual} stepping-stones, which shrink over the course of a step to confine foot placement to a safe region defined on a targeted stone \citep{grandia2020multi}. The CBFs used to specify these foot position constraints are given by:
\begin{align}\label{eqn:ss-CBF1}
    h_1(q) &= R(\tau(q))-(O_x-F_x(q)),\\
    h_2(q) &= R(\tau(q))+(O_x-F_x(q)),\label{eqn:ss-CBF2}
\end{align}
where $F_x(q)$ is the horizontal position of the swing foot and $O_x>0$ is the horizontal position of the center of stepping-stone. The virtual stone width is given by the function $R:\mathbb{R}\to \mathbb{R}$:
\begin{equation}
    R(\tau(q)) = \frac{ar-1}{1  +ar(e^{-m(\tau(q)-1)}-1)}+1+r
\end{equation}
where  $m>0$ determines the decay rate of the barrier function, $(1+a)r$ is half of the targeted stone width, and $1+r$ defines the half the width of the virtual stepping-stone when $\tau = 0$. These functions are visualized in Figure \ref{fig:amber_and_barrier}.
The safety constraints can be interpreted as keeping the swing foot horizontal position in an interval centered at the middle of the stepping-stone, where the interval shrinks as $\tau$ increases.
As this formulation of CBFs is position-based and therefore relative degree two, we employ the exponential control barrier function (ECBF) extension technique \citep{nguyen2016exponential} to both CBFs to attain the relative degree 1 CBFs: $h_{e,i}(\mb{x}) \triangleq L_{\mb{f}}h_i(\mb{x})  + \alpha_{e}h_i(q).$ 

Combining the results in Section \ref{sec:learning} and \ref{sec:bipeds}, with $\hat{\mb{f}}$ and $\hat{\mb{g}}$ the nominal model in \eqref{eq:hybrid_model_1}, the final Stepping Stone QP \eqref{SS-QP} controller combines the robustifying term of \eqref{eqn:delta-CBF-QP} with the stepping-stone ECBF extensions of \eqref{eqn:ss-CBF1} and \eqref{eqn:ss-CBF2}:
\begin{align}
    \mb{k}(\mb{x}) =  \,\,\underset{\mb{u} \in \mathbb{R}^m}{\argmin}  &  \quad \frac{1}{2} \| \mb{u}-\mb{k}_{PD}(\mb{x}) \|_2^2 \tag{SS-QP} \label{SS-QP}\\
   
    \mathrm{s.t.} \quad & \quad L_{\hat{\mb{f}}}^2h_1(\mb{x})+L_{\hat{\mb{g}}}L_{\hat{\mb{f}}}h_1(\mb{x})\u + \alpha_e L_{\hat{\mb{f}}}h_1(\mb{x}) + \widehat{\delta}_1(\mb{x}) 
    \geq -\alpha(h_{e,1}(\mb{x})) \label{finalCBF1}\\
    & \quad L_{\hat{\mb{f}}}^2h_2(\mb{x})+L_{\hat{\mb{g}}}L_{\hat{\mb{f}}}h_2(\mb{x})\u + \alpha_e L_{\hat{\mb{f}}}h_2(\mb{x}) + \widehat{\delta}_2(\mb{x}) 
    \geq -\alpha(h_{e,2}(\mb{x})). \label{finalCBF2}
\end{align}




\noindent We assume that this \eqref{SS-QP} is feasible and we encountered no infeasibilities in simulation or experimentation. 
\section{Simulation and Experimental Validation} 
\label{sec:exp} 
In this section we apply our episodic learning framework (Algorithm \ref{alg:episodic_learning}) to the AMBER-3M platform in both simulation with injected model uncertainty and on hardware with the model error inherent to real-world systems. In each instance the estimator $\widehat{\delta}$ was implemented as a neural network with two hidden layers of 50 hidden units using the ReLU activation function. The network was trained minimizing mean absolute error using mini-batch gradient descent. Mean absolute error was chosen over other loss functions for its robustness to outliers. The same controller (\ref{SS-QP}) was deployed in the RaiSim \citep{raisim} simulation environment and on the AMBER-3M hardware platform, as seen in the supplementary video (\cite{videoVideo}).
The complete learning code used in simulation and experiments can be found at \citep{git}.
\begin{figure}[b!]
\centering
\includegraphics[width=\textwidth]{Figures/SimHardHComb.pdf}
\caption{Simulation (S) and Hardware (H) data where model mismatch causes violations. \textbf{(Far-Left)}: Simulation where the barrier functions $h_1$ (solid blue) and $h_2$ (solid orange) are enforced via a \ref{eqn:CBF-QP}. The \ref{eqn:dumbreject} is also shown for $\overline{\delta}_1$ (dashed blue) and $\overline{\delta}_2$ (dashed orange), which results in more conservative behavior over many steps. \textbf{(Mid-Left)}: After three episodes of learning the \ref{SS-QP} in simulation, the maximum barrier violation decreases from 2.0 to 0.3 cm. \textbf{(Mid-Right)}: Hardware where the barrier functions $h_1$ (blue) and $h_2$ (orange) enforced via a \ref{eqn:CBF-QP}. \textbf{(Far-Right)}: After two episodes of learning on hardware, the maximum barrier violation decreases from 9.2 to 1.9 cm via the \ref{SS-QP}.}
\label{fig:eps}
\end{figure}

\subsection{Simulation}
The controllers and learning algorithm were first validated in simulation. Model error was introduced by increasing the inertia of all limbs on the true model by a factor of ten while maintaining constant mass. Due to the underactuated nature of the robot and the relationship between step length and zero dynamics stability, not every set of stepping stones is navigable, even if safety is perfectly enforced with respect to the CBFs. Therefore, a feasible stepping stone configuration was first generated for the robot to traverse with stones of 4 cm in width. Without knowledge of the modified model ($\widehat{\delta}_1(\mb{x}) = \widehat{\delta}_2(\mb{x}) = 0$), the controller did not satisfy the CBF constraints (\ref{finalCBF1}-\ref{finalCBF2}), resulting in a maximum violation at foot placement of 2.0 cm, causing the robot to miss the stepping stone and fall over. Three episodes of the PDL algorithm were run, after which the maximum violation was reduced to be 0.3 cm, only 15\% of the original violation. Additionally, the \ref{eqn:dumbreject} controller was implemented, which ensured safety but resulted in extremely conservative behavior, resulting in poor qualitative walking, i.e. harsh foot strikes and an over-bending torso. A comparison of the barrier functions $h_1$ and $h_2$ over the steps with these controllers can be seen in Figure \ref{fig:eps}. 

\subsection{Hardware}
The same nominal model for the robot was used in the hardware experiments as in simulation, with model uncertainty presenting itself as significant friction in the joints, as well as imperfect mass and inertia measurements. The PDL algorithm was implemented on the AMBER-3M robot across a sequence of two episodes. The controllers ran on an off-board i7-6700HQ CPU @ 2.6GHz with 16 GB RAM, which computed desired torques and communicated them with the ELMO motor drivers on the 137 cm tall, 22 kg robot. The motor driver communication ran at 2kHz, and the \ref{SS-QP} ran at 1kHz.
The stepping stone configuration was specified to the controller with stones of 8 cm in width. 
As with simulation, the CBF-QP resulted in a maximum violation of the barriers of 9.2 cm due to model error. After running the PDL algorithm for two episodes, the maximum violation of the barriers was 1.9 cm, only 21\% of the original violation, as depicted in Figure \ref{fig:eps}. Although learning improves our estimate of the safe set size, as discussed in Section \ref{sec:learning}, there is still uncertainty in the stone size. This was accommodated by utilizing physical stones which were 10 cm in width.
The 7.3 cm reduction in stone size mismatch captures the change from $|\delta|_\infty$ to $|\delta-\widehat{\delta}|_\infty$. Gait tiles for this improved traversal of the stepping stones are shown in Figure \ref{fig:gaitTiles}.

\begin{figure}[t!]
    \centering
    \includegraphics[width=\textwidth]{Figures/Hardware/GaitTiles.png}
    \caption{Gait tiles for Episode 2 of learning showing the AMBER-3M robot safely traversing a set of stepping stones. Notice the change in step width and added lean of the torso induced by the barrier functions.}
    \label{fig:gaitTiles}
    \vspace{-7mm}
\end{figure}

\section{Conclusion}
\label{sec:conclude}
In this paper we presented an episodic learning approach for reducing the impact of model uncertainty on safety-critical control using control barrier functions. Our method is able to learn a projected disturbance and incorporate the learned model information into an optimization-based controller, as demonstrated in both a high-fidelity simulation and on the AMBER-3M robot hardware platform. Future work includes theoretical analysis of the convergence properties of Algorithm \ref{alg:episodic_learning}, modification of the controller to ensure trajectories remain close to previously-seen data points to reduce the effects of generalization error, and an extension to three dimensional bipedal walking with vision sensing for stepping-stone identification. 



\section{Hybrid motion planning}

\begin{figure}[!t]
	\begin{center}
	\vspace{3mm}
		\includegraphics[width=0.43\columnwidth]{Figures/Vision/V35_2.png}
		\includegraphics[width=0.46\columnwidth]{Figures/Vision/model.png}
		\caption{On the left is the rendering of the robot design, and on the right is the illustration of the configuration coordinates for the robot. The leg indices $\mathrm{l}_*$ are shown on the vertices of the \textit{body} link. Each leg has three actuated joints and equipped with a point foot.}
		\label{fig:config}
	\end{center}
	\vspace{-5mm}
\end{figure}

We consider the nonlinear model of quadrupedal locomotion as a hybrid dynamical system, which is an alternating sequence of continuous- and discrete-time dynamics. The order of the sequence is dictated by contact events. In comparison with bipedal walking, the increased number of contact points of quadrupeds increased the complexity of the hybrid model substantially. In this section, we introduce a unified model for quadrupedal behaviors including walking, ambling and trotting, based on which we design full model based optimal controllers as well as simulation validation.


\subsection{The robot}
The robot we study in this paper is the Vision 60-v3.2 (see \figref{fig:robot}), a quadrupedal robot built by Ghost Robotics, with the total weight of $26$ kg and maximum standing height of $0.5$ m. As shown in \figref{fig:config}, we model the quadruped as a $13$-link system: a \textit{body} link and four legs, each of which has three children links ---the \textit{hip}, \textit{upper} and \textit{lower} links. The configuration variables of Vision 60 are denoted by $q\in\mathcal{Q}\subset\mathbb{R}^n$ where $n=18$ is the total number of degrees of freedom (DOF) without considering any contact constraints. Utilizing the floating base convention \cite{Grizzle2014Models}, we can have $q^T =( q_b^T, q_l^T )$, in which $q_b\in\mathbb{R}^3\times\mathrm{SO}(3)$ represents the global Cartesian position and orientation of a frame attached to the \textit{body} linkage, and the local coordinates $q_l\in\mathbb{R}^{12}$ denote the $12$ joint variables: hip roll, hip pitch and knee angles. These angles are denoted by $\theta_{hr_j}, \theta_{hp_j}, \theta_{k_j}$ for the $j$-th leg, all of which are actuated by Brushless (BL) DC motors. This yields the system's total DOF to be $18$ and control inputs $u\in\mathbb{R}^{12}$.
With different scenarios of foot contacts with the ground, we have a mixture of overactuated, fully-actuated, and underactuated domains (i.e., phases) for the dynamics. Futher, we can define the state space $\mathcal{X}=T\mathcal{Q}\subseteq \mathbb{R}^{2n}$ with the state vector $x^T = (q^T,\dot q^T)$, where $T\mathcal{Q}$ is the tangent bundle of the configuration space $\mathcal{Q}$. 

We now define the different modes of the quadrupedal dynamics, separated by discrete events. This fusion of continuous- and discrete-time dynamics yields a hybrid automaton model \cite{derIntro}. For more detailed definitions of hybrid modeling for bipedal robots, we refer the readers to \cite{Ames2013Humana}. 


\subsection{The continuous-time domain: constrained dynamics}
Given the floating base coordinates, we can derive the unconstrained dynamics, i.e. without any contact constraints, by the Euler-Lagrange equations as:
\begin{align*}
    D(q)\, \ddot q + H(q,\dot q) &= B u
\end{align*}
where $D(q)\in\mathbb{R}^{n\times n}$ is the mass-inertia matrix, $ H(q,\dot q)\in\mathbb{R}^n$ includes the Coriolis, centrifugal and gravity terms and $B(q)\in\mathbb{R}^{n\times m}$ is the actuation matrix and $u\in\mathbb{R}^m$ is the torque applied at joints with $m=12$.

Now consider $k\in\{1,2,3,4\}$ feet standing on the ground, which means $\bar k=4-k$ feet are swinging in the air. This creates a variety of contact scenarios, hence generates different types of quadrupedal behaviors (see \figref{fig:graph}). For each contact situation, we associate a \textit{continuous domain}:

\vspace{-3mm}
\small
\begin{align}
	\label{eq:domain}
	\mathcal{D}_v := \{ (x,u) | h_v(q)=\dot h_v(q,\dot q)=0, N_v \succeq 0, h_{\mathrm{s},v}(q)\succeq 0\}
\end{align}
\normalsize
where $v\in\{1,2,3...\}$ is the domain index and $h_v(q)\in\mathbb{R}^k$ is the height of all standing feet with ground reaction force $N_v(x,u)\in\mathbb{R}^k$. The height of the other feet, referred as the swing feet, is shown by $h_{\mathrm{s}, v}(q)\in\mathbb{R}^{\bar k}$. 
We then have the constrained dynamics for $\mathcal{D}_v$ as:
\begin{align}\begin{cases}
D(q)\, \ddot q + H(q,\dot q) = B_v\, u + J_{v}^T(q)\, \lambda_v \\
J_v(q)\, \ddot q  + \dot J_v(q,\dot q)\, \dot q =0
\end{cases}
\label{eq:eom-general}
\end{align}
where $J_v(q):={\partial p_v(q)} / {\partial q}$ represents the Jacobian matrix of the Cartesian position of the standing feet $p_v(q)\in\mathbb{R}^{3k}$, with the corresponding constraint wrench $\lambda_v\in\mathbb{R}^{3k}$. 
Note that the actuation matrix $B_v$ is domain dependent. This is because the double and triple support phases (\figref{fig:graph}) create closed-chain structures that induce redundancy in control and constraints.
This is an underdetermined problem which often appears in multi-contact locomotion \cite{zhaomulti} that yields nonunique controllers for $u$. Therefore, we manually turn off the rear standing leg's hip pitch motor for double support phase and turn off the diagonal standing legs' hip pitch motors and the other standing leg's hip roll motors for triple supporting phase. This implementation in return yields underactauted dynamics for the full system. Now we can convert the constrained EOM \eqref{eq:eom-general} into a controlled ODE:
\begin{align}
\label{eq:ode}
	\dot x = f_v(x) + g_v(x) u.
\end{align}
To track a given set of time-based trajectories $\mathcal{B}_v(t)$, which will be detailed in a later section, we deployed an \textit{input output feedback linearization} controller:
\begin{align}
\label{eq:io}
	u_{\mathrm{io}}(x,t) = \mathcal{A}(x) \big(  \mathcal{L}(x,t) - 2\varepsilon y(q,t) - \varepsilon^2 \dot y(q,\dot q, t)  \big).
\end{align}
with the outputs $y(q,t) = y_a(q)-\mathcal{B}_v(t)$ and $\varepsilon>0$. In this formulation, we chose the \textit{actual outputs} $y_a(q)$ as all of the actuated joints. The notations follow directly from Eq. (28) of \cite{Ames2013Humana}. The time-based controller has been justified for improved robustness over state-based methods both in theory \cite{DURUS_timedependent} and experiments \cite{ma2017bipedal}. As a result, \eqref{eq:io} forces the system to converge to a desired gait exponentially, that is, $y_a(q)\rightarrow \mathcal{B}_v(t)$. 
The output dynamics become
\begin{align}
    \label{eq:ydyn}
	\ddot y = -2\varepsilon \dot y -\varepsilon^2 y
\end{align}
for which the origin $(y,\dot{y})=(0,0)$ is exponentially stable. 

\begin{figure}[!t]
\vspace{2mm}
	\begin{center}
		\includegraphics[width=0.9\columnwidth]{Figures/hybrid/directedGraph.png}
		\caption{The cyclic directed graph for the multi-domain hybrid dynamics of walking, ambling and trotting gaits. 
		The dashed lines represent a \textit{relabeling} map \cite{Ames2013Humana} that flips the left and right legs' contact attributes.
	
	
		}
		\label{fig:graph}
	\end{center}
	\vspace{-5mm}
\end{figure} 

Prior to introducing the edges and the discrete dynamics, some assumptions are necessary to construct a feasible model both for computation and experiment:
\begin{itemize}
	\item There is no ground slippage. This is partially guaranteed by enforcing a friction cone condition. However, slipping locomotion has been observed on quadrupedal animals for energy efficiency \cite{gamus2013analysis, Maslipery}.
	

	
	\item The ethology studies \cite{Muybridgeanimal} have observed a pattern of $4\times2$-domain\footnote{The term $m \times 2$ denotes a gait with $m$-domain. See \figref{fig:graph} for the directed graph of the gaits with $m$ domains. The second half of the motion is directly a left-right mirror (i.e., symmetry) of the gait. See \figref{fig:gaits} for the full motion with $m \times 2$ domains.}
	locomotion on quadrupedal animals, and the authors have investigated this modeling method in \cite{Hamed_Ma_Ames_Vision60}. But in this paper, we assume the \textit{stance leg transition domains ---one leg strikes while another leg lifts} are instantaneous and passive for walking and ambling. Hence $\mathbf{s}_1$ and $\mathbf{s}_2$ in \figref{fig:graph} become edges and we can have a $2\times 2$-domain behavior for \textit{walk} and \textit{amble}. 	
	But a $4\times 2$-domain model is still used for \textit{trot}.

\end{itemize}


\subsection{The discrete-time domain: impact and lift-off}
On the edge of $\mathcal{D}_v$ in \eqref{eq:domain}, one of the conditions reaches its bound. Thus we have two switching mechanisms:
\begin{itemize}

	\item \textit{Lift off}: a standing foot of leg ${l_*}$ lifts off from the ground, meaning 
	$N_v^{l_*}(x,u) = 0$.
	

	\item \textit{Impact}: a swing foot of leg $l_*$ impacts the ground, meaning $h_{s,v}^{l_*}(q) = 0$, $\dot h_{s,v}^{l_*}(q,\dot q) < 0$.
\end{itemize}
For \textit{lift off}, an identity map $x^+_{v+1} = x^-_{v}$ is sufficient to represent the transition from current to the next domain, where $x^-_{v}$ is the state at the end of the domain $\mathcal{D}_v$ and  $x^+_{v+1}$ is the state at the beginning of $\mathcal{D}_{v+1}$. However, the \textit{impact} shall cause a jump in the velocity terms. To describe this, we make use of a plastic impact model \cite{Grizzle2014Models},
\begin{align}
\label{eq:impact}
	\begin{bmatrix} D & -J_{v+1}^T\\ J_{v+1} &0 \end{bmatrix}
	\begin{bmatrix} \dot q^+_{v+1}\\ \Lambda \end{bmatrix}
	= 
	\begin{bmatrix} D \dot q^-_v\\ 0  \end{bmatrix}
\end{align}
by using the conservation of momentum while satisfying the next domain's holonomic constraints.



\input{sections/opt}

\subsection{Gait Synthesis}
\label{sec:gaitsyn}
The desired trajectory $\mb{y}_d(\tau(\mb{q}),\alpha)$ is given by a degree $N$ Bézier polynomial
\[\mb{y}_d^i(\tau) = \sum_{k=0}^{N}\alpha_k^i \frac{N!}{k!(M-k)!}\tau^k (1-\tau)^{M-k}, \quad i\in \{1,...,m\} \]
With the relative degree 2 output defined for the hybrid control system $\Sigma$, the zero dynamic surface is defined as:
\begin{equation*} \label{eq:zdsurface}
    \mathcal{Z}(\delta_{hip}^-,\delta_{hip}^+,\bs{\alpha}) = \{ \mb{x} \in \mathcal{X}  \big| \mb{y}(\mb{q}) = 0 \mbox{ and } L_\mb{f} \mb{y}(\mb{q},\dot{\mb{q}}) = 0 \} 
\end{equation*}

For underactuated systems, \cite[Thm. 2]{ames2014rapidly} proved that with sufficiently rapidly exponentially stabilizing outputs, an exponentially stable periodic orbit in zero dynamics surface is an exponentially stable periodic orbit for the full-order dynamics.  The goal of the gait optimization is to find a periodic gait in the zero dynamics surface. 

The gait optimization is then performed with a feedback linearizing control law
\begin{equation}
    \u(\mb{x}) = -\left(L_\mb{g} L_\mb{f} \mb{y}(\mb{x})\right)^{-1} \left(\frac{1}{\epsilon}K_D L_\mb{f} \mb{y}(\mb{x}) +\frac{1}{\epsilon^2}K_P \mb{y}(\mb{x}) + L^2_\mb{f}\mb{y}(\mb{x})\right)
\end{equation}
which results in rapidly exponentially stabilizing output dynamics as $\Ddot{\mb{y}} = -\frac{1}{\epsilon}K_D \dot{\mb{y}} -\frac{1}{\epsilon^2}K_P \mb{y}$.

The optimization is performed to find a periodic orbit on the two-dimensional zero dynamics submanifold of $\mathcal{X}$, i.e. to find the set of parameters $\delta_{hip}^-,\delta_{hip}^+,\bs{\alpha}$ for desired outputs such that 
\[ \Delta(\mb{x}^-) \in \mathcal{Z}(\delta_{hip}^-,\delta_{hip}^+,\bs{\alpha}) ,\quad  \forall \mb{x}^- \in \mathcal{S} \cap \mathcal{Z}(\delta_{hip}^-,\delta_{hip}^+,\bs{\alpha}) \tag{HZD}\]
under a list of physical constraints including foot clearance, torso moving range, friction cone constraints and actuation limits. 

The gait optimization is a nonlinear program (NLP) performed under a direct collocation framework \cite{hereid2017frost}:
\begin{align*}
    \mathcal{X}^*  = \operatorname*{argmin}_{\chi} \quad & \Phi(\mathcal{X})\\
    \textrm{s.t.   }& \text{HZD}\\
    &\text{physical constraints}\\
\end{align*}
where $\mathcal{X}$ is a set of parameters in the NLP including $\delta_{hip}^-,\delta_{hip}^+$, and $\bs{\alpha}$.
