text:
\section{Introduction}
\label{sec:introduction}
The main advantage of legged robots with respect to their wheeled counterpart is their
ability to traverse unstructured environment such as stairs, obstacles,
and debris. However, the control of legged robots poses complex problems related to
 underactuation (the body is controlled only indirectly through the legs), and to the hybrid
nature of the forces required to generate motion, since the robot needs
to establish and interrupt contact between its feet and the ground.
The control design for legged robots was initially dealt with by using heuristic
approaches which yielded remarkable results such as in the walking machines from
 Raibert \cite{raibert1986legged}, the virtual model control
of Pratt et al. \cite{pratt2001} and in the heuristic locomotion planning for quadrupedal
robots by Focchi et al. \cite{Focchi2020}. However,  heuristic approaches have several limitations, for example:
1) they cannot be easily generalized to all kinds of terrain and motion;  2) they
cannot account for the future state of the robot hence, they have no possibility to guarantee physical
feasibility of the planned trajectories. The challenge of avoiding
these undesirable  \textit{myopic} behaviors in heuristic planning
approaches has motivated the research towards new optimization-based predictive locomotion planning.
\begin{figure}[t]
	\centering
	\includegraphics[scale=0.14]{figures/pallet_walk_nmpc.png}
	\caption{\small IIT's quadruped robot HyQ traversing a pallet with the mobility enhanced real-time \gls{nmpc}.}
	\label{fig:pallet_walk_main}
\end{figure}

Formulating the locomotion planning problem as an optimization problem
allows one to represent high-level locomotion tasks and system dynamics
using cost functions and constraints, respectively.
Besides robot dynamics, the locomotion tasks should also respect the contact dynamics
such as unilateral force and friction cone constraints, that
are critical to stabilize the locomotion.
The use of optimization techniques to design \gls{wbc} has
enabled legged robots to overcome challenging terrains \cite{Focchi2016},
to traverse soft terrains~\cite{Fahmi2020TRO} and to be
versatile in terms of type of gait and motions that a legged robot
can achieve~\cite{Bellicoso2017}. The aforementioned
examples are based on the solution of a Quadratic Program
that only considers the instantaneous \cite{kuindersma2014} effects of the joint
torques on the robot's base.
Further, similar to heuristic approaches mentioned earlier, these  approaches
do not consider the information about the future states of the robot and
hence cannot assure recursive feasiblity.

In order to address this issue, several approaches studied \gls{to} based locomotion
planning considering the full
dynamics of the robot~\cite{Neunert2017,Posa2016}.
However, these approaches often suffer from high computational
time hence they are restricted to \textit{offline} use.
In general, offline (open loop) planners~\cite{Bernardo2018,Melon2020}
neither adapt to quick terrain
changes nor cope with state drifts and uncertainties.
To address this issue the concept of \textit{online} re-planning can be used.
Online re-planning can intrinsically cope with the problem of error
accumulation in planned motion that is common in real scenarios.
For online re-planning, MPC has gained broad interest in
the robotics community for legged locomotion.
Moreover, the intrinsic feedback mechanism offered by MPC can compensate for modeling
errors and disturbances acting on the system provided that
the MPC is executed at a sufficiently high rate in closed-loop.
An MPC formulation with a careful choice of the dynamic model is
typically required to achieve a desired re-planning frequency in
closed-loop, given the limited computational resources available for online computations.
For example, using a full dynamics model
of legged robots inside MPC \cite{Neunert2018, koenemann15iros} with long prediction horizon results in an optimization
 problem which requires excessive computations for real-time deployment at high sampling rates.
Using approximate models is a way to reduce the complexity of the
optimization, trading the accuracy with computational efficiency.
Following this line, \cite{Dai2014} used a \gls{cdm} plus a full-kinematic
model to enforce the kinematic limits in \gls{to} to plan complex behaviors on the humanoid robot Atlas.
The \gls{cdm} model considers contact forces as input and
links the linear and angular momentum of the
robot to the external wrench~\cite{Orin2013a}.

A simplified version of the \gls{cdm} model is the \gls{srbd} model where the inertia of the legs is neglected (this model assumes massless legs)
and the robot's body and legs are lumped into a single rigid body.
This model is well suited for quadrupeds, since they
 usually concentrate their mass and inertia in the robot base, unlike humanoids.
The \gls{srbd} model was used for \gls{to}~\cite{Winkler2018a}
and  MPC~\cite{wensigHeuristics2017} to jointly optimize
for footholds, \gls{com} trajectories and contact forces.
By further linearizing the angular part of the
dynamics  of the \gls{srbd}, \cite{DiCarlo2018} was able to
achieve a variety of quadrupedal gaits in experiments
but their approach was not suitable for motions that involve
large variations from the horizontal orientation.
The simplest among all the approximate models mentioned earlier is the \gls{lipm} and it has been used inside MPC for quadruped~\cite{Horvat2017} and biped~\cite{herdt2015} locomotion.
However, there are two main limitations in \gls{lipm},
namely it neglects  angular dynamics and assumes constant robot height.
Additionally, it does not account for friction cones,
so that contact stability
on non-flat terrain  can not be guaranteed.

While models play an important role in obtaining computationally light MPC formulations,
the choice of solution method is also paramount to achieve fast online re-planning with MPC.
A \gls{ddp} based approach demonstrated the real-time performance
with  whole-body MPC \cite{koenemann15iros} on HRP-2 humanoid.
Recently, \cite{Grandia2019} proposed a \gls{ddp}-based MPC using a kinodynamic model
which re-plans at a low frequency of  $15\,\mathrm{Hz}$ with a time horizon of  $1\,\mathrm{s}$ on a quadruped.
The main drawbacks of \gls{ddp} based approaches are: 1) the need of writing custom-made solvers,
2) some difficulty in implementing hard-inequality and switching  constraints.
 Hard-inequality constraints need to be  implemented as penalties (e.g., with  relaxed-barriers)
\cite{Hauser2007} while the switching constraints are formulated using Augmented Lagrangian methods
\cite{Lantoine2012, Li2020}. Though not obvious at first sight, these methods are essentially equivalent to direct optimal control based on multiple-shooting~\cite{Bock1984} in combination with some form of \gls{nlp} solvers using barrier functions in the real-time iteration scheme~\cite{Diehl2005}. One such framework is provided by $\texttt{acados}$~\cite{Verschueren2019}.

In addition to \gls{ddp}-based MPC, there also exist a few implementations of \gls{nlp}-based MPC for legged robots.
One such \gls{nmpc} implementation with \gls{com} dynamics plus full kinematic model was demonstrated
in \cite{Farshidian2017b} using a \gls{slq} algorithm for a trotting gait
on flat terrain. Neunert  et al. \cite{Neunert2018} achieved a fast re-planning
frequency of 80-170 $\mathrm{Hz}$ for a small time horizon of 0.5 $\mathrm{s}$
 with their \gls{nmpc} using the full dynamics of the robot, and
 optimizing foot locations, swing timing, and locomotion sequences along
with full body dynamics.  However, in the real experiments they have only demonstrated
the locomotion of quadrupeds in place and slow trotting on flat terrain.
Moreover, since their approach does not consider the map of the terrain, it has  limited
application on uneven terrain conditions.  An interesting observation is that
they did not see a noticeable degradation in the closed-loop performance of the \gls{nmpc}
when the re-planning frequency was dropped until 30 $\mathrm{Hz}$, demonstrating
that the predictive nature of the  MPC empowers the robot to tolerate much lower
re-planning frequency. A similar observation has been made in \cite{Bledt2019} with
an MPC scheme which optimizes foot locations, but requires a heuristic conditioning
of the cost function. In the experiments, the robot is stable if the re-planning occurs at $20 \ \mathrm{Hz}$,
unstable for lower frequencies and the performance improvement is observed over
  $40 \ \mathrm{Hz}$.

The aforementioned approaches have been successful in controlling legged robots,
but neglected an important aspect of these robots, which is usually referred to
as \emph{mobility}. In this paper, we define the mobility as the attitude
 of the robot leg to arbitrarily change its foot position.
To achieve kinematically suitable configurations for the legs, a common heuristic
is to align the robot base with the terrain
inclination (estimated in \cite{Focchi2020} via
fitting an averaging plane through the stance feet).
This approach aims to bring the legs as close as possible to the
middle of their workspaces in order to avoid the violation of the kinematic limits.
Optimization of mobility allows to achieve a similar behaviour in an automatic way.
Fankhauser et al. in \cite{Fankhauser2018}
 maximized mobility by encoding it in a cost function that
penalizes the distance with respect to a default foot position. Recently,
Cebe et al. \cite{Cebe2020} implemented \gls{to} using an \gls{srbd}
model and also incorporating the feet positions in the optimization.
They re-plan only at the feet touchdowns due to the high computation demand of
their \gls{to} algorithm and showed experimental results on uneven
terrain. Since their planner does not plan during the swing phase of the legs, they do not run their planner in an MPC fashion. Apart from the previously mentioned work, to the best of our
knowledge no contribution has been made to address the mobility
with MPC in legged locomotion.

\subsection{Proposed Approach and Contribution}
In this work, we introduce a real-time \gls{nmpc}
planner for rough terrain locomotion that accounts for leg mobility
to drive the base orientation of our \gls{hyq} robot \cite{semini11hyqdesignjsce} in real experiments.
In our implementation, we employ a simplified \gls{srbd} model
that describes the angular and translational dynamics
of the robot base but neglects the dynamics of legs.
\begin{figure*}[htb]
	\centering
	\includegraphics[width=\textwidth]{figures/plannerblockDiagram.pdf}
	\caption{\small Block diagram of the planning pipeline with the \gls{nmpc} in our locomotion framework.}
	\label{fig:blockDiagram}
\end{figure*}
The contributions of our work are as follows:
\begin{itemize}[leftmargin=*]
\item We formulate an \gls{srbd} model enforcing contact sequences
in an optimization-friendly way through model parameters that are updated online, rather
 than through complementarity constraints.
This results in an \gls{nmpc} formulation of lower complexity compared to alternative model formulations.
We introduce a cost term which accounts for
\textit{mobility} by penalizing hip-to-foot positions
that do not provide the highest mobility.
This allows our \gls{nmpc} to devise a robot base orientation and height that ensures
high leg mobility during locomotion on rough terrain.
To date, this is the first time mobility has been addressed with MPC to the best of our knowledge.
We also introduce improvement features regarding \textit{force regularization} for
 locomotion stability and \textit{kinematic robustness} to account for estimation
 errors on the contact normals inside our \gls{nmpc} that are essential for locomotion on rough terrain.
\item
Running an \gls{nmpc} online with a long prediction horizon poses
the challenge of either a high computation time or to accept a suboptimal
solution. We have addressed this problem through the \gls{rti} scheme \cite{Diehl2005,Diehl2009,Gros2020}
that allowed us to run our \gls{nmpc} online with a time horizon of $2\,\mathrm{s}$
as opposed to the $0.5 \,\mathrm{s}$ used by \cite{Neunert2018}. Differently from
\cite{Cebe2020} (that re-plans at each foot touchdown event),
we continuously re-plan at the rate of $25\,\mathrm{Hz}$.
We run our \gls{nmpc} on a single computer along with the rest of our locomotion
framework\footnote{Except the perception related modules that run on a dedicated computer}
unlike in \cite{Cebe2020, Neunert2018} where they use dedicated computers
for their \gls{to} and \gls{nmpc}, respectively.
\item Simulations and Experiments: We show in simulation the robot traversing a set of
pallets of different dimensions placed relatively at varying distances,
walking into a V-shaped chimney and lastly over a randomly generated rough terrain.
We present \emph{Experimental} results that demonstrate the capability of our \gls{nmpc} to generate an
omni-directional walk and to traverse a pallet for our quadruped
robot \gls{hyq}, by exploiting an online evaluation of the map of the terrain
 and on-board state estimation. We test the re-planning capability of our approach
by pushing a pallet in front of the robot while walking, such that the
perception algorithms have to estimate the new pallet position and the control
algorithm has to re-plan online in order to adapt to a dynamically changing environment.
\end{itemize}

The optimization is carried out with minimal heuristics from the
user i.e., no reference trajectory for the robot's height, its base roll and pitch orientation
is required. The generation of the reference trajectory for the \gls{nmpc} takes
into account premature and delayed touchdown of the feet as
well as continuously adjusting the footholds according to the robot body motion
and the terrain features.

\subsection{Outline}
The paper is organized as follows: Section \ref{sec:overview}
gives an of our planning pipeline whereas
Section \ref{sec:nmpc} describes the  \gls{nmpc} setup.
The contributions about leg mobility and robustness
are discussed in Section \ref{sec:features}
whereas the generation of the references and the \gls{wbc} are detailed
in Sections \ref{sec:ref_gen} and \ref{sec:whole_body_controller}, respectively.
We then summarize the \gls{rti} scheme for our \gls{nmpc} in Section \ref{sec:rti_scheme}.
Further, Section \ref{sec:results} illustrates simulation and experimental results
with the \gls{hyq} robot. Finally, we draw the conclusions in Section \ref{sec:conclusion}.

\section{Locomotion Framework} \label{sec:overview}
\label{subsec:planning_pipeline}
In Fig.~\ref{fig:blockDiagram}, we illustrate the planning pipeline of our
locomotion framework. The \emph{reference generator}, as discussed in Section
\ref{sec:ref_gen}, takes the user input (longitudinal, lateral and angular velocity),
the gait (e.g., a crawl), the initial state of the robot, and a
map of the terrain to generate reference trajectories for the state $\vect{x}^{\mathrm{ref}}$
and control input $\vect{u}^{\mathrm{ref}}$ required by the \gls{nmpc}.
The reference generator also provides a vector of parameters $\vect{a}$ to the \gls{nmpc},
that includes foot locations and contact sequences.
The \gls{nmpc} running at  $25 \,\mathrm{Hz}$,
delivers the optimal trajectories of the
state $\vect{x}^\mathrm{p}$ and control input
$\vect{u}^\mathrm{p}$, as detailed in Section \ref{sec:nmpc}. All the components
of the Whole-Body controller (highlighted with dashed box in Fig.~\ref{fig:blockDiagram}) are discussed in Section \ref{sec:whole_body_controller}.
The \emph{\gls{wbc} interface}  interpolates the optimal state $\vect{x}^\mathrm{p}$ at a
rate of  $250 \,\mathrm{Hz}$ to generate a desired  signal $\vect{x}^\mathrm{d}$
for a Cartesian virtual impedance controller \cite{Focchi2016}.
The \gls{wbc} interface also  computes the feedforward wrench
$\vect{W}^\mathrm{d}_\mathrm{ff}$ that is added to a feedback wrench
$\vect{W}^\mathrm{d}_\mathrm{fb}$ that renders
the Cartesian  impedance.
Moreover, the \gls{wbc} interface provides  the joint position $\vect{q}_\mathrm{d}$
and velocities $\vect{\dot{q}}_\mathrm{d}$ to a Joint Space PD controller running at  $1\,\mathrm{kHz}$.
After acquiring the feedback and
feed-forward wrenches, a \gls{qp} optimization computes the
vector of desired \gls{grfs} $\vect{f}^\mathrm{d}$ accounting for the friction cone constraints
and penalizing the the difference between $\vect{f}^\mathrm{d}$ and
$\vect{u}^\mathrm{p}$ coming from the \gls{nmpc} solution.
Then, $\vect{f}^\mathrm{d}$
is mapped to the torque vector $\bm{\tau}^*$ that is  added to
the Joint Space PD torques $\bm{\tau}_\mathrm{fb}$ resulting into the total
desired torque $\bm{\tau}_\mathrm{d}$. The value $\bm{\tau}_\mathrm{d}$ is  passed to a low-level joint torque controller
as reference \cite{boaventura15tro}.
An online  state estimator~\cite{nobili_camurri2017rss} that runs at  $500\,\mathrm{Hz}$ provides the estimation of the
robot state to all the  components inside
our locomotion framework that require it.
A dedicated on-board computer takes inputs from an RGB-D camera (RealSense)
mounted in front of the robot and generates a 2.5D heightmap  at the rate
of  $30\,\mathrm{Hz}$ using the \emph{Grid Map} library from \cite{Fankhauser2016GridMapLibrary}.
This heightmap is later sent to the reference generator.
\section{NMPC}\label{sec:nmpc}
In our planning algorithm, we choose a real-time \gls{nmpc} formulation because it has the ability to handle
both the nonlinear system dynamics and the constraints, explicitly.
\gls{nmpc} is based on solving an \gls{ocp} given the current
state $\vect{\hat{x}}_0$ of the system.
Only the first element of the optimized input trajectory
is applied to the system, then the state is measured and the \gls{ocp} is solved again
based on the new state measurement to close the loop.

We define the decision variables as the predicted state and control input with $\vect{x}^\mathrm{p}=\{\vect{x}_{0}, \ldots, \vect{x}_N\}$ and  $\vect{u}^\mathrm{p}=\{\vect{u}_{0}, \ldots, \vect{u}_{N-1}\}$, respectively,
such that an \gls{nlp} formulation can be stated as:
\begin{subequations}\label{eq:nlp_formulation}
	\begin{align}
	\displaystyle{\min_{\vect{x}^\mathrm{p},\vect{u}^\mathrm{p}}}\quad
	&  \sum_{k=0}^{N-1} \ell\left(\vect{x}_{k}, \vect{u}_{k}, \vect{a}_{k}\right)+\ell_\mathrm{T}\left(\vect{x}_{N}\right) \label{eq:cost_function} \\
	\text{s.t.} \quad 		& \vect{x}_{0}=\boldsymbol{\hat{\vect{x}}_{0}}, \label{eq:initial_condition}\\
	&\vect{x}_{k+1}=f\left(\vect{x}_{k}, \vect{u}_{k}, \vect{a}_{k}\right),&& k\in\mathbb{I}_0^{N-1}, \label{eq:equality}\\
	&h\left(\vect{x}_{k}, \vect{u}_{k}, \vect{a}_{k}\right) \leq 0,&& k\in\mathbb{I}_0^{N-1}, \label{eq:inequality}

	\end{align}
\end{subequations}
where, $\ell : \Rnum^{n_x} \times \Rnum^{n_u} \times \Rnum^{n_a} \rightarrow \Rnum$ is the stage
cost function; $\ell_\mathrm{T} : \Rnum^{n_x} \rightarrow \Rnum$ is the terminal
cost function. Initial condition \eqref{eq:initial_condition}
is expressed by setting $\vect{x}_0$ equal to the state estimate
$\boldsymbol{\hat{\vect{x}}_{0}}$ received from the state estimator.
The vector of model parameters $\vect{a}_{k}$ is not optimized
but is computed externally by the reference generator and
provided to the optimization problem formulation. The nonlinear system
dynamics are introduced by the equality constraints \eqref{eq:equality}.
Finally, the path constraints are included with \eqref{eq:inequality}
which, for example, can be bounds on the decision variables.
The \gls{nlp} \eqref{eq:nlp_formulation} is defined
for a time horizon  $T$ that is divided into $N$ discrete time intervals
of lengths $T_\mathrm{s} = \frac{T}{N}$. Hereafter, we will refer to $T_\mathrm{s}$
as the \textit{sampling time}, while we call $N$ as \textit{prediction horizon}.

\subsection{Cost}
In our \gls{nmpc} formulation we use a cost function of the form:
\begin{subequations}\label{eq:quadratic_cost}
\begin{align}
	\ell\left(\vect{x}_{k}, \vect{u}_{k},\vect{a}_k\right)=&\parallel \vect{x}_k-\vect{x}^{\mathrm{ref}}_k\parallel_{\mx{Q}}^2+
	\parallel \vect{u}_k-\vect{u}^{\mathrm{ref}}_k\parallel_{\mx{R}}^2 \label{eq:tracking_cost}\\
	&+\parallel {}_\mathcal{C}\vect{p}_{\mathrm{hf}_{k}}-{}_\mathcal{C}\vect{p}_{\mathrm{hf}_{k}}^{\mathrm{ref}}\parallel_{\mx{M}}^2 \label{eq:mobility_cost}\\
	&+\rho\parallel {{}_\mathcal{K}\vect{u}_k}\parallel_{\mx{P}}^2\label{eq:force_robustness}
\end{align}
\end{subequations}
where:
\begin{itemize}[leftmargin=*]
\item The tracking cost \eqref{eq:tracking_cost} is associated to state and control input
and the references trajectories $\vect{x}^{\mathrm{ref}}_k, \vect{u}^{\mathrm{ref}}_k$
are provided by the reference generator for each sampling instance $k$ (refer Section \ref{sec:ref_gen}).
\item The mobility cost \eqref{eq:mobility_cost} is one of the contributions
of this work that accounts for improving the leg mobility
by penalizing the difference between the hip-to-foot relative position
${}_\mathcal{C}\vect{p}_\mathrm{hf}$ and the reference value ${}_\mathcal{C}\vect{p}_\mathrm{hf}^{\mathrm{ref}}$
of maximum mobility. This cost allows the  \gls{nmpc} to optimize the robot base
orientation (e.g. align it to the terrain) in order to  increase
the leg mobility of the robot without reaching the kinematic limits during locomotion.
The derivation of ${}_\mathcal{C}\vect{p}_\mathrm{hf}^\mathrm{ref}$ is detailed
separately in Section \ref{subsec:mobility}.
\item In some locomotion scenarios \cite{Focchi2016}, to cope with uncertainties
in the contact normal estimation and increase robustness to external disturbances,
it is desirable to have the \gls{grfs} $\vect{f}_i$ as close as possible to
the center of the friction cone. This can be achieved
by penalizing $\vect{u}$ in a frame $\mathcal{K}$ (see Fig. \ref{fig:hyqScheme}) that
is aligned to the normal of the contact and it is included in our cost function
by a control input regularization term \eqref{eq:force_robustness}, refer Section \ref{subsec:force_robustness} for the details.
\end{itemize}
The positive definite weight matrices $\mx{Q} \in \Spd{n_x}, \mx{R} \in \Spd{n_u}$,
$\mx{M} \in \Spd{12}$, $\mx{P} \in \Spd{n_u}$ act as important tuning parameters
in the \gls{nmpc} formulation. The regularization factor $\rho$ decides the trade-off
between force robustness cost \eqref{eq:force_robustness} and both the tracking
\eqref{eq:tracking_cost} and mobility \eqref{eq:mobility_cost} cost. Finally, we define
the terminal cost $\ell_\mathrm{T} = \parallel \vect{x}_N-\vect{x}^{\mathrm{ref}}_N\parallel_{\mx{Q}_N} $ and use the weight matrix ${\mx{Q}_N} ={\mx{Q}}$ for this terminal cost.
\subsection{Robot Model} \label{sec:robot_model}

The inertial frame $\mathcal{W}$ and the \gls{com} frame $\mathcal{C}$ are shown in Fig.~\ref{fig:hyqScheme}. The \gls{com} frame is aligned with the base of the robot and
its origin is located at the \gls{com}. A variable with left subscript denotes its
frame of reference. For example  ${}_\mathcal{C}\bm{\omega}$ represents
the angular velocity of the robot base
expressed in the \gls{com} frame $\mathcal{C}$.
Note that, unless explicitly specified,  all the relevant quantities in this paper
are defined in the inertial frame $\mathcal{W}$. Throughout this paper we define $(a,\ldots,b)$ as the column vector stacking any generic column vectors $a,\ldots,b$.
\begin{figure}[h]
	\centering
	\includegraphics[scale=0.3]{figures/hyqScheme.pdf}
	\caption{\small \gls{hyq} schematic showing the inertial frame ($\mathcal{W}$), the
		\gls{com} frame ($\mathcal{C}$) attached to the \gls{com} of the robot, and the contact frame ($\mathcal{K}$). The robot legs are shown in the \textit{default} configuration.}
	\label{fig:hyqScheme}
\end{figure}

We use a simplified reduced-order \gls{srbd} model \cite{Winkler2018a} defined in a 6D space that describes
the translational and angular dynamics of the robot while neglecting
the dynamics of the its legs. This is a valid approximation for the \gls{hyq} robot  because most
of its mass is concentrated in the base, as mentioned in~\cite{semini11hyqdesignjsce} (mass of the base is $61.4\, \mathrm{kg}$ and mass of each leg is $6.5\, \mathrm{kg}$).
The robot is approximated as a rigid body with the inertia
computed considering the robot in a default leg configuration as shown in Fig. \ref{fig:hyqScheme}.
We choose to define the \gls{srbd} model in the \gls{com} frame (specifically the
angular dynamics) because this choice yields a constant inertia tensor.
This allows our \gls{nmpc} formulation to benefit from a lower computational
complexity because  the angular dynamic equations are much simpler i.e., less non-linear because the inertia tensor is not time varying
and, consequently, less computationally demanding.
In \gls{srbd} model, the \gls{grfs} are applied as inputs to control the position and orientation of the robot base.
The \gls{srbd} model is:
\begin{subequations} \label{eq:newtonEuler}
	\begin{align}
	m\dot{\vect{v}}_\mathrm{c}&=m\vect{g}+\sum_{i=1}^{4}\delta_i\vect{f}_i \label{eq:trans_dynamics}\\

	{}_\mathcal{C}\mx{I}_\mathrm{c}\,{}_\mathcal{C}\dot{\bm{\omega}}+{}_\mathcal{C}\bm{\omega} \times {}_\mathcal{C}\mx{I}_\mathrm{c} {}_\mathcal{C}\bm{\omega} &=\sum_{i=1}^{4} \delta_i   {}_\mathcal{C}\vect{p}_{\mathrm{cf},i} \times {}_\mathcal{C}\vect{f}_i \label{eq:ang_dynamics}
	\end{align}
\end{subequations}
where $m$ is the robot mass, $\dot{\vect{v}}_\mathrm{c} \in \Rnum^3$ is the \gls{com} acceleration,
$\vect{g}$ is the gravitational acceleration,
$\vect{f}_i \in \Rnum^3$ is the ground reaction force of foot $i$, ${}_\mathcal{C}\mx{I}_\mathrm{c}\in \Rnum^{3\times3}$
is the inertia tensor computed at the \gls{com} frame origin, ${}_\mathcal{C}\dot{\bm{\omega}}\in \Rnum^3$
is the angular acceleration of the robot's base, $\vect{p}_{\mathrm{cf},i}\in \Rnum^3$ is the distance between
the \gls{com} position $\vect{p}_\mathrm{c} \in \Rnum^3$ and the position $\vect{p}_{\mathrm{f},i}\in \Rnum^3$ of foot $i$. We introduce binary parameters $\delta_i = \{0, 1\}$ to define
whether foot $i$ is in contact with the ground and can therefore generate contact
forces or not.

The robot dynamics governed by \eqref{eq:newtonEuler} can be
expressed as the continuous-time state-space model:
\begin{equation}\label{eq:stateSpaceEq}
\mat{\dot{\vect{p}}_\mathrm{c}\\
	\dot{\vect{v}}_\mathrm{c}\\
	\dot{\bm{\Phi}}\\
	{}_\mathcal{C}\dot{\bm{\omega}}}
=
\mat{ {\vect{v}}_\mathrm{c} \\
	1/m \sum_{i=1}^4 \delta_i \vect{f}_i + \vect{g} \\
	\mx{E}'^{-1}(\bm{\Phi}) {}_\mathcal{C}\bm{\omega} \\
	-{}_\mathcal{C}\mx{I}^{-1}_\mathrm{c}({}_\mathcal{C}\bm{\omega} \times {}_\mathcal{C}\mx{I}_\mathrm{c}) {}_\mathcal{C}\bm{\omega} +  \sum_{i=1}^4 \delta_i {}_\mathcal{C}\mx{I}^{-1}_\mathrm{c}{}_\mathcal{C}\vect{p}_{\mathrm{cf},i} \times {}_\mathcal{C}\vect{f}_i
}
\end{equation}
where $\vect{v}_\mathrm{c}$ is the \gls{com} velocity of the robot.  The robot base orientation is represented by the vector of $Z$-$Y$-$X$ Euler
angles \cite{Diebel2006} $\bm{\Phi}=(\phi,\, \theta,\, \psi)$ i.e., roll
($\phi$), pitch ($\theta$) and yaw ($\psi$), respectively. The relation between the Euler Angles
rates $\dot{\bm{\Phi}}$ and angular velocity ${}_\mathcal{C}\bm{\omega}$ is well-known and discussed in Appendix~\ref{adx:eulerRate} for the sake of completeness. We define the state and control vectors as
$\vect{x}=(\vect{p}_\mathrm{c}, \,\vect{v}_\mathrm{c} ,  \, \bm{\Phi}, \, {}_\mathcal{C}\bm{\omega})$, and
$\vect{u}=(\vect{f}_1, \ldots, \vect{f}_4)$.
Equation \eqref{eq:stateSpaceEq}
can be concisely written as:
\begin{equation}\label{eq:compactStateSpace}
\dot{\vect{x}}(t)= g(\vect{x}(t),\vect{u}(t),\vect{a}(t)),
\end{equation}
where $\vect{a} = (\vect{p}_f, \, \bm{\delta})$
is a  vector of parameters that includes the feet positions $\vect{p}_f$ and the contact sequences
$\bm{\delta} \in \Rnum^4$.

The rigid-body dynamics \eqref{eq:compactStateSpace} are discretized using numerical integration~\cite{Quirynen2017,Butcher2003,Hairer1993,Hairer1996} to obtain the
discrete-time model:
\begin{align}
	\vect{x}_{k+1}=f\left(\vect{x}_{k}, \vect{u}_{k}, \vect{a}_{k}\right),
\end{align}
which defines equality constraints~\eqref{eq:equality} imposed at every stage $k$ in MPC to ensure that the state trajectory satisfies the system dynamics for the given control inputs.

One specific feature of legged robots is the need to ensure that the values of the \gls{grfs} equal to zero for a swinging leg. This is typically done by introducing complementarity constraints \cite{Cebe2020, Villarreal2020}. These constraints, however, pose several difficulties in the solution of the optimization problem, since the vast majority of the \gls{nlp} algorithms cannot handle them and tailored solvers are required. Ultimately, this results in a significant increase in computation time.
An alternative to complementarity constraints consists in providing the contact sequences $\bm{\delta}$ as input parameters in the
state space model \eqref{eq:compactStateSpace}.
In this manner, a contact mode $\delta_i$
is multiplied with the terms involving force $\vect{f}_i$ in \eqref{eq:stateSpaceEq}
and the contribution of that force is nullified during the swing
phase of the corresponding leg $i$.
Hence, there is no more need to include complementarity constraints
separately in \eqref{eq:nlp_formulation} which results in
fewer constraints and, consequently, in a relatively smaller \gls{nmpc} formulation.

\textit{Remark:} Note that Euler angles can suffer from singularities that occur in certain configurations \cite{Younes2012}. Because in this work we do not consider motions that involve such configurations, using Euler angles does not pose any issue.
A singularity-free implementation is out of the scope
of this work and is left for future research.
\subsection{Friction cone and unilateral constraints}

Friction cone constraints are encoded with their square pyramid approximation:
\begin{subequations}\label{eq:friction_cone}
	\begin{align}
		-\mu_i \vect{f}_{\mathrm{z},i}&\leq \vect{f}_{\mathrm{x},i}\leq \mu_i \vect{f}_{\mathrm{z},i}\\
		-\mu_i \vect{f}_{\mathrm{z},i}&\leq  \vect{f}_{\mathrm{y},i}\leq \mu_i \vect{f}_{\mathrm{z},i}\\
		\underline{\vect{f}}_\mathrm{z}&\leq \vect{f}_{\mathrm{z},i}\leq \overline{\vect{f}}_\mathrm{z} \label{eq:unilaterality}
	\end{align}
\end{subequations}
where, $\underline{\vect{f}}_\mathrm{z}$ and $\overline{\vect{f}}_\mathrm{z}$
are upper and lower bounds on \gls{grfs} $Z$ component, respectively, and
$\mu_i$ is the friction coefficient of the contact surface.
Choosing
$\underline{\vect{f}}_z$ greater than or equal to zero enforces unilateral
 constraints on the normal forces $\vect{f}_\mathrm{z}$. The friction cone and unilateral constraint are represented by $h\left(\vect{x}_{k}, \vect{u}_{k}, \vect{a}_{k}\right) \leq 0$ in the \gls{nmpc} formulation.
\section{Locomotion-Enhancing Features} \label{sec:features}

In this section we discuss the main distinctive features of our approach, which provide a quadruped robot with an enhanced locomotion ability.
These features are \emph{mobility}, \emph{force robustness} and \emph{kinematic robustness}.

\subsection{Mobility and Mobility Factor} \label{subsec:mobility}
A way to enable our \gls{nmpc} to choose robot orientation adaptively to  any terrain
is to employ the concept of \emph{mobility}~\cite{focchi17iros}.
In order to rigorously discuss mobility in mathematical terms, we first define it in words as the attitude of a manipulator (leg) to arbitrarily change end-effector position/orientation~\cite{sciavicco00}.

In order to penalize low leg mobility
in the cost function \eqref{eq:mobility_cost} we need to compute the reference value of hip-to-foot distance ${}_\mathcal{C}\vect{p}_{\mathrm{hf}_{k}}^{\mathrm{ref}}$. Our goal in this section is to define a convenient metric to represent mobility and
compute ${}_\mathcal{C}\vect{p}_{\mathrm{hf}_{k}}^{\mathrm{ref}}$ corresponding to the maximum value of such
a metric.
Among several ways to compute mobility \cite{focchi17iros},  the velocity transformation ratio \cite{Yoshikawa1984}
allows one to evaluate mobility in a particular direction.
However, the velocity transformation ratio cannot be used in our setting
because it requires prior knowledge of the evolution of the relative
foot position with respect to \gls{com} which is not available in advance in our case.

As an alternative approach, we consider the volume of the \textit{manipulability
ellipsoid} $\left( \vect{v} (\mx{J}\mx{J}^\top)^{-1}\vect{v}=1 \right)$ \cite{sciavicco00} as a metric to evaluate mobility.
A change in the volume of the manipulability ellipsoid with different
leg configurations is visualized in Fig. \ref{fig:3Dellipsoids} (left).
\begin{figure}
	\centering
	\includegraphics[width=\columnwidth]{figures/mobility_ellipsoid.pdf}

	\caption{\small Manipulability ellipsoid changing with
		leg configuration (left) of the right front leg. Volume of the ellipsoid (top right) and
		Eccentricity of the ellipsoid (bottom right).}
	\label{fig:3Dellipsoids}
\end{figure}
Inspecting Fig. \ref{fig:3Dellipsoids} (top right), it can be seen that
the maximum volume $V$ is in the vicinity of the most extended
leg configuration because the mobility becomes very big in the $X$ and $Y$ direction,
even if it is still very limited in the $Z$ direction. However, because it is desirable
to achieve a good mobility in all the directions of the leg configuration, a better metric to do so is the one that accounts for
the \textit{isotropy} of the manipulability ellipsoid. A measure of the isotropy of an
ellipsoid can be expressed as the inverse of its \textit{eccentricity}.
Hence, a new  manipulability index that we call \textit{mobility factor}
\eqref{eq:mobility_factor} can be defined in terms of both the eccentricity
and the volume of manipulability ellipsoid.
Again from Fig. \ref{fig:3Dellipsoids}, it can be visualized that
to keep a good mobility (left plot) in all directions, the volume (top right plot) should be maximized
while the eccentricity (bottom right plot) as small as possible.
Defining a foot Jacobian $\mx{J}(\vect{q}) \in \Rnum^{3\times3}$ computed at a particular joint configuration $\vect{q}$, the volume $V$ of a manipulability ellipsoid is evaluated as the product
of the eigenvalues of $\left(\mx{J}(\vect{q})\mx{J}(\vect{q})^\top\right)^{-1}$
while the eccentricity is the ratio between its maximum and minimum eigenvalue
\cite{focchi17iros}.   First, the volume and eccentricity of manipulability ellipsoid
 are normalized by their ranges $\bar{V}$ and $\bar{E}$. Then we define the mobility factor as:
\begin{equation}\label{eq:mobility_factor}
l_\mathrm{m} =  \beta \frac{V}{\bar{V}} - \gamma \frac{E}{\bar{E}}
\end{equation}
The minus sign in \eqref{eq:mobility_factor} represents conflicting contributions
of the $V$ and $E$ in the definition of the mobility factor. Parameters
$\beta$ and $\gamma$ are introduced to find a best trade-off between volume and eccentricity
while deciding a mobility factor.

The mobility factor is a convex nonlinear function $l_\mathrm{m}: \Rnum^3\to \Rnum$
that can be numerically evaluated inside the workspace of each leg.
By selecting
$\beta = 1$ and $\gamma = 4$, and after conducting a numerical analysis for all the feet positions in the
workspace of a leg of the HyQ robot we found that hip-to-foot distance ${}_\mathcal{C}\vect{p}_\mathrm{hf} = (0, 0 , -0.55)\,\mathrm{m}$
maximizes $l_\mathrm{m}$.
In  Fig. \ref{fig:mobility3D} (left) we show a slice of the scalar
function $l_\mathrm{m}$ in the $X$-$Y$ plane for  ${}_\mathcal{C}\vect{p}_\mathrm{hf,z}=-0.55\,\mathrm{m}$
obtained for the \gls{rf} leg. Instead, in  Fig. \ref{fig:mobility3D} (right)
we plot $l_\mathrm{m}$ against the change of foot position in the $Z$ direction
considering the hip under the foot ($X=0$, $Y=0$) which clearly
highlights ${}_\mathcal{C}\vect{p}_\mathrm{hf,z}=-0.55\,\mathrm{m}$ corresponding to
the maximum value of the mobility factor $l_\mathrm{m}$ (i.e., around 0.41).
We use the output of  this analysis as a reference
for the hip-to-foot distance
 ${}_\mathcal{C}\vect{p}_{\mathrm{hf}}^{\mathrm{ref}}$
 in the mobility cost \eqref{eq:mobility_cost} for all the legs.
\begin{figure}
	\centering
	\includegraphics[width=\columnwidth]{figures/mobility_factor.pdf}
	\caption{\small Slices of the mobility factor function for the \gls{rf} leg: the left figure
		 plots it  against the $X$-$Y$ components keeping $Z$ constant.
		The red dot in the left plot represents the maxima.
		The right figure plots it against the $Z$ component
		for a constant $X$-$Y$ foot position. }
	\label{fig:mobility3D}
\end{figure}

In the mobility cost, we multiply $\delta_i$ to the term corresponding to the $i^\mathrm{th}$ leg. Thus, the mobility cost solely accounts for stance legs because the robot can only use them to control its base orientation.
Since, including the mobility cost enables \gls{nmpc} to provide
the optimal base orientation for a particular locomotion that retains mobility, there is no need to separately specify tracking cost for roll and pitch in the \gls{nmpc}.
This relieves a user from the burden of implementing a customized heuristic (e.g., to align the robot base to the terrain),
as was necessary in, e.g., \cite{Focchi2020, Gehring2015, Villarreal2020}.
The relative tracking task for the \gls{com} $Z$ position is no longer required either,
because maximizing the mobility in the $Z$ direction automatically takes care of
keeping an average distance of hips from the terrain to ${}_\mathcal{C}\vect{p}_\mathrm{hf,z}$,
consequently keeping the robot base at a certain height.

Moreover, the yaw motion
results by penalizing the mobility cost along the $X$-$Y$ directions. This has the effect of driving the hips of the robot base over the feet,
naturally aligning the base to the feet, similar to what was
done in \cite{frontiers20raiola}. However, a tracking cost on yaw was still necessary in the NMPC to avoid oscillations.

\subsection{Kinematic Robustness} \label{subsec:kinematic_robustness}
In legged locomotion, the robot is often operated close to unstable configurations which require a controller to continuously compensate for model inaccuracies and external disturbances while maintaining locomotion stability. However, a configuration in which the \gls{zmp} \cite{Wieber2009} is close to the boundary of the support polygon \cite{Bretl2008a} could cause instability even with small perturbations.

In our case, the reference generator computes
references for the \gls{grfs} by dividing the robot mass with the
number of legs as explained in Section \ref{sec:ref_gen}.
Penalizing \gls{grfs} $Z$ component heavily in the tracking cost \eqref{eq:tracking_cost} ensures that they stay close to the reference, consequently maintaining a higher loading on the diagonally opposite leg to the swinging one, and therefore maintaining some margin for the locomotion stability.
To evaluate the locomotion stability, we define the \emph{\gls{zmp} margin} which is computed as the minimum of the distance of the \gls{zmp} from each support polygon edge, i.e.,
\begin{equation}
	m_\mathrm{c} = \min(\vect{d})
\end{equation}
where $\vect{d}$ is a vector of the distances of \gls{zmp} projection from the support polygon edges.

\subsection{Force Robustness} \label{subsec:force_robustness}
Similar to the considerations on mobility, in order to  effectively compensate for disturbances acting on the system, robustness in the \gls{grfs} is required. The closer the GRF is to the friction cone boundary, the less lateral force can be generated to compensate for perturbations.
An approach penalizing \gls{grfs} that are in the vicinity of the cone boundaries has been proposed in \cite{Focchi2016, Fahmi2020TRO} inside the \gls{wbc}. These \gls{wbc} based approaches instantaneously generate \gls{grfs} that are as close as possible to the normals of the cones while yielding the prescribed resultant wrench on the robot base. However, \gls{wbc} does not account for the future state of the robot and hence, it leaves some room for the \gls{nmpc} to compensate for the contact normal estimation error and external disturbances.
Introducing these margins on \gls{grfs} from the cone boundaries is especially important in some scenarios, such as the one reported in simulation in Section \ref{sim:vshaped_wall}.
In this paper, we adopt a similar idea to \cite{Focchi2016, Fahmi2020TRO} and introduce the additional cost term \eqref{eq:force_robustness} in the \gls{nmpc}, which penalizes the tangential components of \gls{grfs} in the contact frame $\mathcal{K}$ (see Fig. \ref{fig:hyqScheme}) to obtain the resultant \gls{grfs}  as close as possible to the contact normals. The weight matrix $\mx{P}$ used in this cost is defined in Table \ref{tab:nmpc_weights} (Section \ref{sec:results}).

\section{Reference Generator}
\label{sec:ref_gen}
In our approach, the \gls{nmpc} requires a reference trajectory of
the state and control input along with the model parameters i.e.,
foot positions and contact sequences.
For the very first run of the NMPC, this reference trajectory also serves  as an initial guess.
The references are generated for the length of prediction horizon $N$,
since the reference generator is called before every iteration of
the NMPC in order to obtain prompt adaptation to terrain changes and user set-point.
Our reference generator is based on heuristics and it takes as inputs:
\begin{itemize}
	\item the user commanded longitudinal and lateral \gls{com} velocity
	${}_\mathcal{H} \vect{v}_\mathrm{c}^{\mathrm{usr}} \in \mathbb{R}^2$ in the
	horizontal frame $\mathcal{H}$,\footnote{The horizontal frame is placed like the \gls{com} frame
	but with the  $Z$-axis aligned with the gravity}
    \item user commanded heading velocity $\omega^{\mathrm{usr}}_\mathrm{z} \in \Rnum$,
	\item current pose of the robot $(\vect{p}_\mathrm{c}, \bm{\Phi})$,
	\item current feet positions  $\vect{p}_\mathrm{f} \in \Rnum^{12}$,
	\item heightmap of the terrain
\end{itemize}
The reference generator outputs:
\begin{itemize}
	\item the references for the \gls{nmpc} cost: states
	$\vect{x}^\mathrm{ref} \in  \Rnum^{N \times n_x}$,
	control $\vect{u}^\mathrm{ref} \in  \Rnum^{N \times n_u}$,
	\item parameters $\vect{a}$ of the model: contact sequences $\bm{\delta} \in  \Rnum^ {N \times 4}$
	and foot locations $\vect{p}_\mathrm{f} \in  \Rnum^{N \times 12}$,
	\item normals of the terrain at the foothold locations, which are
	 provided as inputs to the \gls{nmpc} for the cone constraints.
\end{itemize}

First we compute the $X$-$Y$ components of the total velocity
$\vect{v}^\mathrm{ref}_\mathrm{c} \in \mathbb{R}^3$ as
the sum of $\vect{v}_\mathrm{c}^{\mathrm{usr}}$ obtained by mapping  ${}_\mathcal{H} \vect{v}_\mathrm{c}^{\mathrm{usr}}$ in the world frame and
the $X$-$Y$ components of the tangential velocity resulting from the angular rotation
of the robot base due
to heading velocity $\bm{\omega}^{\mathrm{usr}}$ = $(0, 0, \omega^{\mathrm{usr}}_\mathrm{z})$.
\begin{equation}
\vect{v}^\mathrm{ref}_{\mathrm{c,(x,y)}} = \vect{v}_\mathrm{c}^{\mathrm{usr}} + (\bm{\omega}^{\mathrm{usr}} \times \vect{p}_\mathrm{c}^\mathrm{ref})_\mathrm{(x,y)}
\end{equation}
The $X$-$Y$ \gls{com} position $\vect{p}^\mathrm{ref}_\mathrm{c,(x,y)}$ is obtained by
integrating the $\vect{v}^\mathrm{ref}_\mathrm{c,(x,y)}$ with  the explicit Euler scheme.
The references for \gls{com} $Z$, roll and pitch are set to $0$
because we do not track them in the \gls{nmpc} cost \eqref{eq:tracking_cost}.

Instead, the reference for the yaw is obtained  by integrating the user defined
yaw rate $\dot{\psi}^\mathrm{usr}$. To do so, first
$\dot{\bm{\Phi}}^{\mathrm{usr}}$ is computed from the heading
velocity as explained in Appendix \ref{adx:eulerRate}:
$\dot{\bm{\Phi}}^{\mathrm{usr}} = \vect{E}^{-1}
(\bm{\Phi}^{\mathrm{ref}}) \bm{\omega}^{\mathrm{usr}}$ and
then yaw rate is extracted from this vector.
Therefore, the equation that represents the heuristics
to update the state references is:
\begin{equation}
  \vect{x}^\mathrm{ref}_{k+1} = \left (
  \vect{p}_{\mathrm{c}_{k+1}}^{\mathrm{ref}},\
  \vect{v}_{\mathrm{c}_{k+1}}^{\mathrm{ref}},\
  \bm{\Phi}_{k+1}^{\mathrm{ref}},\
  \bm{\omega}_{k+1}^{\mathrm{ref}}\right )
\end{equation}
where,
  $\vect{p}_{\mathrm{c}_{k+1}}^{\mathrm{ref}} = (\vect{p}^{\mathrm{ref}}_{\mathrm{c}_k, \mathrm{(x,y)}} +
  T_\mathrm{s} \vect{v}^\mathrm{ref}_{\mathrm{c}_k, \mathrm{(x,y)} }  ,  0)$,

 $\vect{v}_{\mathrm{c}_{k+1}}^{\mathrm{ref}} = (\vect{v}_{\mathrm{c}}^{\mathrm{usr}} + (\bm{\omega}^{\mathrm{usr}} \times \vect{p}_{\mathrm{c}_k}^{\mathrm{ref}})_\mathrm{(x,y)}    , 0)$,
 %
 $\bm{\Phi}_{k+1}^{\mathrm{ref}}= (0, 0, \psi_k^\mathrm{ref} ) + T_\mathrm{s} (0, 0, \dot{\psi}^{\mathrm{usr}} )$ and
 $\bm{\omega}^{\mathrm{ref}}_{k+1} =\bm{\omega}^{\mathrm{usr}}$.
The references for \gls{grfs} $\vect{u}^\mathrm{ref}$ are calculated by simply dividing the
total mass of the robot by the number of legs in stance.

The contact sequences $\bm{\delta}$ and footholds are computed by the
\textit{gait scheduler} and \textit{robocentric stepping}, respectively.
It is important to mention that the reference generator does
 not compute the swing trajectories and they are obtained from the
  \gls{wbc} interface discussed in Section \ref{subsec:wbc_interface}.

\subsubsection{Gait scheduler}
The gait scheduler is logically decoupled from the generation of reference trajectories and
determines if each leg is either in swing or in stance $({\delta}_i)$ at
each time instance for the entire gait cycle as shown in Fig. \ref{fig:gaitScheduler} (left).

The leg duty factor $D_i$ and offsets $o_i$ can be used to encode
different gaits such as crawl, trot and pace. The gait scheduler implements
a time parametrization $s$ (\textit{stride phase}) which is normalized
about the cycle time duration $T_\mathrm{c}$ such that the leg duty
factor $D_i$ and offsets $o_i$ are independent from the cycle time.
Time parametrization $s$ is reset at the end of each cycle resulting in a sawtooth pattern.
Once we have defined $s$, we can expresses the value of $\delta$ for leg $i$ as
\begin{equation}
	\delta_i =\begin{cases}
	1 , &   (s < o_i)  \vee s > (o_i + (1-D_i))\\
	0, & \text{otherwise}
	\end{cases}
\end{equation}
Changing the gait cycle duration changes the slope  of $s$ resulting
in different frequencies  of the swing events.
\begin{figure}[!tb]
	\centering
	\includegraphics[width=0.68\columnwidth]{figures/gaitScheduler.pdf}
		\includegraphics[width=0.3\columnwidth]{figures/gaitCounter.pdf}
	\caption{\small Gait schedule for walk. Offsets $\vect{o} =\left[0.05, 0.3, 0.55,0.8\right]$,
		duty-factors $\vect{D} =\left[0.85, 0.85, 0.85,0.85\right]$.
		The red arrows represent the triggers $l^\mathrm{tr}_{i}$ for the
		swing legs. Right part shows the fast-forwarding or the re-winding
		of the gait counter to recover synchronization between actual (haptic) and planned touchdown.}
	\label{fig:gaitScheduler}
\end{figure}
The gait scheduler also computes the default swing duration,
$T_{\mathrm{sw}_i}^\mathrm{d}$ according to the duty factor $D_i$. Note that since
the  gait schedule of leg $i$ is normalized between 0 and 1,
to recover the real swing duration $T_\mathrm{sw}^\mathrm{d}$ one
needs to multiply it by the cycle duration $T_\mathrm{c}$.
Each trigger $l^\mathrm{tr}_i$ corresponds to a new lift-off event.
Every time the reference generator is called, it extracts $N$
points from the gait schedule obtaining the contact sequence
for the entire prediction horizon (that could last also more than one gait cycle),
starting from an index called \textit{gait counter} that
keeps \textit{memory} of the index of the gait schedule achieved by the previous
call of the reference generator.
The synchronization between the
first point of a contact sequence $\delta_{i_k}$ computed by the reference generator and the actual contact state of the robot is crucial for locomotion.  This is to avoid the reference generator to compute a zero reference force while the leg is in stance and vice-versa.
In the case of premature or delayed touchdown events on uneven terrains this synchronization is lost.
To recover the synchronization,
it is possible to shift the gait counter backwards or forward
to re-conciliate the planned touchdown with the actual
touchdown as shown in Fig.  \ref{fig:gaitScheduler} (right).

\subsubsection{Robocentric stepping}
\label{sec:robocentric_stepping}
The choice of the foothold is a key element in locomotion,
since it deals with the kinematic limits of the robot.
We use an approach that continuously computes
footholds consistent with the current position of the robot.
To compute a foothold for the swinging leg $i$, we
consider its hip position  $\vect{h}_i$ instead of
using the foot position at the moment of  lift-off.
In this way a disturbance acting on the robot or a tracking error
occurred during a swing  can be recovered in the following swing.
For a leg $i$, dropping the index to simplify the notation and defining
the lift-off trigger as $l^\mathrm{tr}_k=  \delta_k  \land \overline{\delta}_{k+1}$, the foot position is computed as:
\begin{equation} \label{eq:footposition}
\vect{p}_{\mathrm{f}_{k+1}}=
\begin{cases}
\vect{p}^{\mathrm{td}}_{\mathrm{f}_k} & l^\mathrm{tr}_k= 1 \ \\
\vect{p}_{\mathrm{f}_k} & l^\mathrm{tr}_k= 0
\end{cases}
\end{equation}
We can notice that at the lift-off condition  $l^\mathrm{tr}= 1$ at instance $k$, $\vect{p}_\mathrm{f}$ is set equal
to the touchdown point  $\vect{p}^{\mathrm{td}}_\mathrm{f}$ and it is kept constant until the
next lift-off event occurs. The $X$-$Y$ component of the touchdown point is given by
\begin{equation} \label{eq:touchdownpoint}
	\vect{p}^\mathrm{td}_{\mathrm{f}_k,\mathrm{(x,y)}} =
	\vect{h}_k +   T_\mathrm{sw}^\mathrm{d} \alpha (\vect{v}_\mathrm{c}^{\mathrm{usr}}  + (\bm{\omega}^{\mathrm{usr}} \times \vect{p}_\mathrm{bh})_\mathrm{(x,y)}   )
\end{equation}
where $\vect{h}_k = \vect{p}_{\mathrm{b}_k,\mathrm{(x,y)}}^\mathrm{ref} +  \vect{p}_\mathrm{bh,(x,y)}$
is the $X$-$Y$ components of the hip position, $\vect{p}_{\mathrm{b}_k}^{\mathrm{ref}} \in \Rnum^3$
is the position of the geometric center of the base\footnote{In order
to compute $\vect{p}_{\mathrm{b}_{k}}^\mathrm{ref}$ we assume that its velocity is equal to
the one of the \gls{com}, so $\vect{p}_{\mathrm{b}_{k},\mathrm{(x,y)}}^\mathrm{ref}=
\vect{p}_{\mathrm{b}_{k-1},\mathrm{(x,y)}}^\mathrm{ref}+ T_\mathrm{s}
\vect{v}_\mathrm{c}^\mathrm{ref}$}
and $\vect{p}_\mathrm{bh}\in \Rnum^3$
is the distance between hip and center of the base. The second term in (14) represents the step length (red arrow in Fig.~\ref{fig:robocentricStepping}) which is computed with respect to the hip (non the previous foot location) so that the foot velocity incorporates the desired base velocity from the user. Parameter $\alpha$ is a
scaling factor between foot and \gls{com} velocity chosen empirically,  and
the term $\bm{\omega}^{\mathrm{usr}} \times \vect{p}_\mathrm{bh}$ is the contribution of $\bm{\omega}^{\mathrm{usr}}$ to the
foot velocity.
\begin{figure}[!tb]
	\centering
	\includegraphics[scale=0.2]{figures/robocentricStepping.pdf}
	\caption{\small Representation of the robocentric stepping strategy and of the Swing Frame,
		located at the lift-off point. The red arrow shows the distance between the
		touchdown point $\vect{p}^\mathrm{td}_\mathrm{f}$ and the hip $\mathbf{h}$.
		The blue vector $\vect{L}_\mathrm{sw}$ connects lift-off and touchdown point.}
	\label{fig:robocentricStepping}
\end{figure}
A 2.5D heightmap of the terrain  is evaluated in correspondence
of the touchdown point $\vect{p}^\mathrm{td}_{\mathrm{f}_k,\mathrm{(x,y)}}$,
to obtain a $\vect{p}_{\mathrm{f}_k,\mathrm{z}}^\mathrm{td}$ that does not penetrate the terrain.

\subsubsection{\gls{vfa}} \label{subsub:vfa}
The robocentric stepping provides a touchdown location $\vect{p}_\mathrm{f}^{\mathrm{td}}$
that is coherent with the terrain.
However, if $\vect{p}_\mathrm{f}^{\mathrm{td}}$ is located near an edge or
leads to collisions (e.g., of the foot or the shin) during the step cycle,
this can be harmful for the robot's balance.
To prevent this from happening, we integrated the \gls{vfa} module presented in \cite{villarreal19ral}, which works as follows.
The robot acquires a local heightmap in the vicinity of the touchdown
location $\vect{p}_\mathrm{f}^{\mathrm{td}}$ extracting it from the full
2.5D heightmap surrounding the robot. The local heightmap corresponds
to a $15\times 15$ grid centered at
$\vect{p}_\mathrm{f}^{\mathrm{td}}$ with a resolution of 4 $\mathrm{cm}$.
The \gls{vfa} adjusts the foot landing location to avoid collisions,
sharp edges, narrow gaps/holes, and footholds that are outside the workspace of the swinging leg.
This module uses a pre-trained \gls{cnn} that takes as input the $15\times 15$ local
heightmap and outputs an adjustment for the touchdown
location $\vect{p}_\mathrm{f}^\mathrm{td}$ within the local heightmap.
The \gls{cnn} has low computational time ($\approx0.2\,\mathrm{ms}$ per adjustment),
which allows the robot to adapt the landing location of the feet without
compromising the computation of the control action
provided by the \gls{nmpc}. Details on how the \gls{cnn}
is trained can be found in \cite{villarreal19ral}.
\section{Whole-Body Controller} \label{sec:whole_body_controller}
In this section, we describe the \gls{wbc} that tracks planned trajectories
 $\vect{x}^\mathrm{p}$ and $\vect{u}^\mathrm{p}$ provided by the \gls{nmpc}.
 The \gls{wbc} first computes feed-forward $\mx{W}^\mathrm{d}_\mathrm{ff}$
 and feedback $\mx{W}^\mathrm{d}_\mathrm{fb}$ wrenches from the planned
 trajectories and then the sum of these wrenches are mapped into \gls{grfs}
 through the \gls{qp} optimization \eqref{eq:qp_wbc}. The \gls{wbc} also
  maps the \gls{grfs}
  into the joint torques $\bm{\tau}^*$. This joint torque along with
  low-impedance feedback torque $\bm{\tau}_\mathrm{fb}$ results in
  the total torque $\bm{\tau}_\mathrm{d}$ required by the low-level joint torque control block.
  Refer to Fig.~\ref{fig:blockDiagram} for the block representation of \gls{wbc} inside our locomotion framework.
\subsection{WBC interface}\label{subsec:wbc_interface}
In our planning framework, the \gls{nmpc} runs at  $25\,\mathrm{Hz}$
which we will refer to as \textit{re-planning frequency} whereas the \gls{wbc}
requires  state and control inputs at  $250\,\mathrm{Hz}$
(we will call this the \textit{\gls{wbc}  frequency}).
Hence, we introduce a \gls{wbc} interface block that re-samples state
and control inputs at the \gls{wbc} frequency.
Furthermore, the \gls{wbc} interface takes care of generating the
swing trajectory connecting subsequent footholds
and managing the premature/delayed touchdowns.
Particularly, the \gls{wbc} interface provides the desired state $\vect{x}^\mathrm{d}$,
desired \gls{grfs} $\vect{u}^\mathrm{d}$, joint position $\vect{q}^\mathrm{d}$,
joint velocity $\dot{\vect{q}}^\mathrm{d}$ and feed forward
wrenches $\mx{W}^\mathrm{d}_\mathrm{ff}$ at any given time instant.
\subsubsection{Re-sampling}
In order to obtain the desired $\vect{u}^\mathrm{d}$ at \gls{wbc} frequency we use a zero-order hold.
At a certain sampling instance $k$, the value of $\vect{u}^\mathrm{d}$ between interval $i$ and $i+1$ is kept equal to the value of $\vect{u}^\mathrm{p}$ obtained at the instance $k$,
\begin{equation}
	\vect{u}_{k,i}^\mathrm{d} = \vect{u}_k^\mathrm{p}, \qquad i\in\mathbb{I}_0^{f_\mathrm{r} -1}
\end{equation}
where $f_\mathrm{r} \in \mathbb{R}$ is the ratio between the \gls{wbc} frequency and re-planning frequency.
Moreover, the planned states $\vect{x}^\mathrm{p}$ from the \gls{nmpc}
are re-sampled with a linear interpolation to obtain $\vect{x}^\mathrm{d}$
at the \gls{wbc} frequency\footnote{The rigorous approach is to use the model \eqref{eq:newtonEuler}
	to predict the evolution of the system in the $T_\mathrm{s}$ time interval,
	considering the $\vect{u}^\mathrm{p}$ coming from the \gls{nmpc},
	but for the motions considered in this paper the result is very similar,
	so a linear interpolation is a fair approximation.}:
\begin{equation} \label{eq:state_interpolation}
\vect{x}^\mathrm{d}_{k,i} = \vect{x}^\mathrm{p}_k +
i  \frac{\vect{x}^\mathrm{p}_{k+1}  - \vect{x}^\mathrm{p}_k}{f_r},
\qquad i\in\mathbb{I}_1^{f_\mathrm{r}}
\end{equation}

\subsubsection{Joint trajectory generation}
To compute the joint trajectories, inverse kinematics is required which in turn
needs the swing trajectory $\vect{p}^{\mathrm{sw}}_\mathrm{f}$.
The latter substitutes  the constant foot position $\vect{p}_\mathrm{f}$ computed by
the reference generator for a swinging leg i.e., when $\delta_i = 0$.
We define the swing frame $\mathcal{S}$ \cite{frontiers20raiola},
whose $X$-axis is aligned with the vector that links lift-off and
touchdown point ($\vect{L}_\mathrm{sw}$), $Y$-axis is perpendicular to the $X$-axis
of the swing frame and to the $Z$-axis of the world frame.
Finally the $Z$-axis is such that $\mathcal{S}$ is a  counter-clockwise coordinate system.
The origin of the swing frame $\mathcal{S}$ coincides with the
lift-off point, see Fig. \ref{fig:robocentricStepping}.
In this way the swing trajectory lies on the $X$-$Z$ plane and we shape it as a semi-ellipse
with $L_\mathrm{sw}$ and $H_\mathrm{sw}$ as length of the axes:
\begin{align}
{}_\mathcal{S} \vect{p}^{\mathrm{sw}}_f = \mat{
 \frac{L_\mathrm{sw}}{2} (1- \mathrm{cos}(\pi f_\mathrm{sw} t_\mathrm{sw}))\\
 0.0\\
 H_\mathrm{sw} \mathrm{sin}(\pi f_\mathrm{sw} t_\mathrm{sw})}
\end{align}
where $t_\mathrm{sw}$ is the time elapsed from the beginning of a swing
and $f_\mathrm{s} = 1/T_\mathrm{sw}$ is the swing frequency computed from user-defined
 cycle time and the duty cycle of a leg. We map ${}_\mathcal{S}\vect{p}^{\mathrm{sw}}_\mathrm{f}$
 in the inertial frame $\mathcal{W}$ to obtain $\vect{p}^{\mathrm{sw}}_\mathrm{f}$. Similarly, the foot velocity
is computed as the derivative of $ {}_\mathcal{S}\vect{p}^{\mathrm{sw}}_\mathrm{f}$
and then mapped to $\mathcal{W}$.  Finally, after  evaluating the relative
foot position ${}_\mathcal{C}\vect{p}_\mathrm{cf}$ and velocity ${}_\mathcal{C}\dot{\vect{p}}_\mathrm{cf}$
we can obtain $\vect{q}^\mathrm{d}$ and $\dot{\vect{q}}^\mathrm{d}$ via inverse kinematics.
\subsubsection{Feed-forward wrench}
The feed-forward wrench $\mx{W}_\mathrm{ff}\in \Rnum^6$ is computed
from the desired \gls{grfs} $\vect{u}^\mathrm{d}$ as,
\begin{equation}\label{eq:ffwd_wrench}
\mx{W}^\mathrm{d}_\mathrm{ff}= \mat{\sum_{i=1}^4 \vect{u}^{\mathrm{d}}_i &
	\sum_{i=1}^4 \vect{p}_{\mathrm{cf},i}^\mathrm{d} \times \vect{u}^\mathrm{d}_i}^\top
\end{equation}

\subsection{Feedback wrench}\label{feedbackController}
Since most of the mass of \gls{hyq} is located in its base,
the average angular velocity of the robot is assumed to be equal
to the angular velocity of the base. We use the approach of \cite{Focchi2016}
to define desired feedback wrench obtained from a Cartesian impedance
and briefly recall it next for completeness:
\begin{equation}
\mx{W}^\mathrm{d}_{fb}=\mx{K}
\mat{\vect{p}_\mathrm{c}^{\mathrm{d}}-\vect{p}_\mathrm{c}\\
	\mx{e}({}_{\mathrm{w}}\mx{R}_\mathrm{b}^{\mathrm{\top}} {}_{\mathrm{w}}\mx{R}_\mathrm{d})}
+\mx{D}
\mat{\mx{v}^{\mathrm{d}}_\mathrm{c}-\mx{v}_\mathrm{c}\\
	\bm{\omega}^\mathrm{d}_\mathrm{b}-\bm{\omega}_\mathrm{b}}
\end{equation}
where $\vect{p}^{\mathrm{d}}_\mathrm{c}$, and $\vect{v}^{\mathrm{d}}_\mathrm{c}$ and
$\bm{\omega}^\mathrm{d}_\mathrm{b} \in \Rnum^3$
are the desired \gls{com} position, \gls{com} velocity and angular velocity of the base.
Matrices
${}_{\mathrm{w}} \mx{R}_\mathrm{b}$ and $ {}_{\mathrm{w}} \mx{R}_\mathrm{d} \in \Rnum^{3\times3} $
are the rotation matrices representing actual and desired orientation of the
 base with respect to the inertial frame, respectively, $\mx{e}(\cdot) : \Rnum^{3\times3} \to \Rnum^{3}$ is
  a mapping from a rotation matrix to the associated rotation vector, composed of the three off-diagonal terms in the (skew-symmetric) matrix logarithm of the argument.
 Matrices $\mx{K}$ and $\mx{D}$
   are diagonal matrices containing the proportional and derivative gains and they can be interpreted as impedances.

\textit{Remark:} At each re-planning instance of \gls{nmpc},
the state reference is computed from the current state of the robot $\vect{\hat{x}_0}$.
Thus,  at each  re-planning instance the feedback term is nullified.
\subsection{Projection of the \gls{grfs}}

While the feedforward wrenches $\mx{W}^\mathrm{d}_\mathrm{ff}$  provided by MPC satisfy the friction cone and unilateral constraints by construction, this guarantee is lost with the addition of the feedback term $\mx{W}^\mathrm{d}_\mathrm{fb}$  to the wrenches. Therefore, one needs to project the total wrenches $\mx{W}^\mathrm{d}_\mathrm{ff}+\mx{W}^\mathrm{d}_\mathrm{fb}$ onto the set of wrenches that satisfy the constraints.
The matrix representation
\begin{equation}\label{eq:linear_sys_wbc}
	\underbrace{\mat{\delta_1 \mx{I} & \ldots & \delta_e\mx{I} \\
			\delta_1\left[\vect{p}_{\mathrm{cf}, 1} \times\right] & \ldots & \delta_e\left[\vect{p}_{\mathrm{cf}, e} \times\right]}}_{\mx{A}}
	\underbrace{\left[\begin{array}{c}\vect{f}_{1} \\ \vdots \\ \vect{f}_{e}\end{array}\right]}_{\vect{f}}
	=\underbrace{\mx{W}^\mathrm{d}_\mathrm{ff}+\mx{W}^\mathrm{d}_\mathrm{fb}}_{\vect{b}}
\end{equation}
is derived from a
simplified \gls{srbd} model \cite{Focchi2016} and allows us to map the
desired wrenches into \gls{grfs}.
To compute the desired \gls{grfs} $\vect{f}^{\mathrm{d}}$
we solve the following \gls{qp}:
\begin{subequations} \label{eq:qp_wbc}
	\begin{align}
	\mx{f}^\mathrm{d} =  \argmin_{\vect{f}} \ & \parallel \mx{A} \vect{f}-\vect{b}\parallel_{\mx{S}}^2 +
	\parallel \vect{f}-\vect{u^\mathrm{d}}\parallel_{\mx{T}}^2 \\
	\text{s.t.} \quad &  	\underline{\vect{d}}\leq \vect{C}\vect{f}\leq \overline{\vect{d}} \label{eq:wbc_qp_constraints}
	\end{align}
\end{subequations}
The term $ \parallel \vect{f}-\vect{u^\mathrm{d}}\parallel_{\mx{T}}^2$ in the cost \eqref{eq:qp_wbc}
allows the tracking of the desired forces $\vect{u}^\mathrm{d}$ received from the \gls{nmpc}. This term is required because the information on the specific direction of the \gls{grfs} is lost when projected into the lower dimensional 6D wrench space in \eqref{eq:ffwd_wrench}.
Matrices
$\mx{S} \in \Spd{6}$ and $\mx{T} \in \Spd{12}$ are positive-definite weight matrices.
Inequality \eqref{eq:wbc_qp_constraints} encodes the friction cone and
unilateral constraints similar to \eqref{eq:friction_cone} for which
further details can be found in \cite{Focchi2016}.
It is important to note that in \eqref{eq:linear_sys_wbc} we do not need to include
gravity compensation because it is already incorporated in the \gls{nmpc}
formulation through the \gls{srbd} model.
\subsection{Mapping \gls{grfs} to Joint Torques}
Since the real actuators apply torques to the leg joints, the \gls{grfs}  $\mx{f}^\mathrm{d}$ must be mapped into joint torques
$\bm{\tau}^*$. We do so by exploiting the kinematic relation:
\begin{equation}
\bm{\tau}^* = -\mx{J}(\vect{q}) ^\top \vect{f}^\mathrm{d} +
\vect{h}(\vect{q},\dot{\vect{q}})
\end{equation}
where $\vect{J}(\vect{q}) \in \Rnum ^{n_u \times n_x}$ is the contact Jacobian
and  $\vect{h}(\vect{q},\dot{\vect{q}})$ the vector of gravity/Coriolis terms in the leg dynamics.

\textit{Remark}: A $1 \,\mathrm{kHz}$ Joint-Space PD is put in cascade with the \gls{wbc} before sending torques to the low-level controller. In this way, we track the desired trajectories of the swing legs and we increase the robustness in case a foot loses contact with the ground.

\section{\gls{rti} for \gls{nmpc}} \label{sec:rti_scheme}
One of the main drawbacks of \gls{nmpc} is its computational burden, thus
efficient tailored algorithms are necessary in order to achieve fast sampling
 rates for complex systems with fast dynamics.
While many approaches have been developed for optimal control, a complete
 discussion about all possible approaches is beyond the scope of this paper.
 We focus on direct multiple shooting using methods derived from \gls{sqp}
 that have been specifically developed for real-time \gls{nmpc}~\cite{Diehl2009}.

In multiple shooting methods both state $\vect{x}$ and control input $\vect{u}$ are
decision variables unlike in single shooting where the decision vector only includes
the control input. We ought to stress that this does not increase the computational
complexity with respect to single shooting (where computations are moved from linear
algebra to the evaluation of derivatives); on the contrary, it allows one to use
tailored sparse linear algebra, which can actually reduce the computational burden.
Furthermore, multiple-shooting allows one to provide an initial guess also for the
state trajectory, which is typically beneficial for unstable systems in an \gls{nmpc} context~\cite{Diehl2009}.

\gls{sqp} is a  popular algorithm which
solves an \gls{nlp} by iteratively solving local quadratic approximations (\gls{qp}s) of the problem~\cite{Gros2020}.
At each \gls{sqp} iteration, the solution from the previous step is recycled to define an initial guess
($\vect{x}_k^{\mathrm{L}}, \vect{u}_k^{\mathrm{L}}$), which is then used to construct a \gls{qp} approximation of
the \gls{nlp}~\eqref{eq:nlp_formulation}, given by
\begin{subequations}\label{eq:qp_subproblem}
	\begin{align}

	\min _{\Delta \vect{x}, \Delta \vect{u}} \quad & \sum_{k=0}^{N-1} \frac{1}{2}\left
	[\begin{array}{c}\Delta \vect{x}_k \\ \Delta \vect{u}_k\end{array}\right]^\top
	\mx{H}_k
	\left[
	\begin{array}{c}\Delta \vect{x}_k \\ \Delta \vect{u}_k\end{array}
	\right]+
	\vect{J}_k^\top
	\left[
	\begin{array}{c}\Delta \vect{x}_k \\ \Delta \vect{u}_k\end{array}
	\right] \label{eq:qp_cost_function} \\
	\mathrm{s.t.} \quad	& \Delta \vect{x}_0=\hat{\vect{x}}_0-
	\vect{x}_0^{\mathrm{L}}, \label{eq:qp_initial_condition}\\
	&\Delta \vect{x}_{k+1}=\mx{A}_k \Delta \vect{x}_k+\mx{B}_k \Delta \vect{u}_k+\vect{r}_k, \label{eq:qp_equality}\\
	&\mx{C}_k \Delta \vect{x}_k+\mx{D}_k \Delta \vect{u}_k+\vect{h}_k \geq 0, \label{eq:qp_inequality}
	\end{align}
\end{subequations}
where,
$\Delta \vect{x}_k = \vect{x}_k-\vect{x}^\mathrm{L}_k, \Delta \vect{u}_k = \vect{u}_k-\vect{u}^\mathrm{L}_k$,
$\hat{\vect{x}}_0$ is the current system state, and
\begin{subequations}\label{eq:qp_sensitivities}
\begin{align*}
	\mx{A}_k &=\left.\frac{\partial f(\vect{x}, \vect{u}, \vect{a}_k)}{\partial \vect{x}}\right|_{\vect{x}_k^{\mathrm{L}}, \vect{u}_k^{\mathrm{L}}},
	&\mx{B}_k &=\left.\frac{\partial f(\vect{x}, \vect{u}, \vect{a}_k)}{\partial \vect{u}}\right|_{\vect{x}_k^{\mathrm{L}}, \vect{u}_k^{\mathrm{L}}}, \\
	\mx{C}_k  &=\left.\frac{\partial h(\vect{x}, \vect{u}, \vect{a}_k)}{\partial \vect{x}}\right|_{\vect{x}_k^{\mathrm{L}}, \vect{u}_k^{\mathrm{L}}} ,
	&\mx{D}_k &=\left.\frac{\partial h(\vect{x}, \vect{u}, \vect{a}_k)}{\partial \vect{u}}\right|_{\vect{x}_k^{\mathrm{L}}, \vect{u}_k^{\mathrm{L}}}, \\
	\vect{r}_k&=g\left(\vect{x}_k^{\mathrm{L}}, \vect{u}_k^{\mathrm{L}}, \vect{a}_k\right)-\vect{x}_{k+1}^{\mathrm{L}},
	&\vect{h}_k&=h\left(\vect{x}_k^{\mathrm{L}}, \vect{u}_k^{\mathrm{L}}, \vect{a}_k\right)\\
	\vect{J}_k&=\mx{W}_k\left[\begin{array}{c}\vect{x}_k^{\mathrm{L}}-\vect{x}_k^{\text {ref }} \\ \vect{u}_k^{\mathrm{L}}-\vect{u}_k^{\text {ref }}\end{array}\right] \tag{\ref{eq:qp_sensitivities}}
\end{align*}
\end{subequations}
Matrix $\mx{H}_k$ is the diagonal blocks of a suitable approximation of the
Lagrangian Hessian. Since our problem relies on a least-squares cost, we adopt
 the popular Gauss-Newton Hessian approximation~\cite{Gros2020} that gives $\mx{H}_k = \mx{W}_k$. Matrix  $\mx{W}_k$ is  equal to the $\mathrm{diag}(\mx{Q}, \mx{R}, \mx{M}, \mx{P})$.

While in \gls{sqp} one solves several \gls{qp}s until convergence is reached,
the \gls{rti} scheme consists in solving a single \gls{qp} per sampling time.
This is motivated by the observation that in \gls{nmpc} two subsequent problems
have very similar solutions. Therefore, by reusing the solution of the previous
\gls{nmpc} problem, one obtains a very good initial guess for the next problem,
which essentially only needs to correct for external perturbations and model mismatch.
For all details on the \gls{rti} scheme, we refer to~\cite{Diehl2005,Gros2020}
and references therein. We limit ourselves to observe that,
since ($\vect{x}_k^{\mathrm{L}}, \vect{u}_k^{\mathrm{L}}$) is known \emph{before}
the next state measurement is available, one can already evaluate the functions and their derivatives~\eqref{eq:qp_sensitivities} before the initial state $\vect{\hat x}_0$
is available. Consequently, the \gls{qp} can be constructed and prepared beforehand;
note that this also includes the first factorization of the \gls{qp} Hessian.
Once $\vect{\hat x}_0$ is available, one only has to finish solving the \gls{qp}.
Therefore, while the overall sampling time must still be long enough to prepare
the next \gls{qp}, the latency between the time at which $\vect{\hat x}_0$
is available and the time at which the control input can be applied to the system is very small.

Note that in the \gls{rti} scheme proposed above, the functions and their
derivatives~\eqref{eq:qp_sensitivities} are evaluated along a guess obtained
from the previous solution, rather than along the reference trajectory.
Another important aspect to highlight is the fact that there exist several
approaches to compute~\eqref{eq:qp_sensitivities}. One choice consists of
first linearizing the continuous-time system dynamics and then using the
matrix exponential to obtain a discrete-time linear system. This approach
presents some advantages, but can be computationally demanding. For the time-varying and infeasible references, however, it is preferred to \textit{first discretize and then linearize}~\cite{Gros2020}. In this work we deal with time-varying and infeasible references, hence we opt for first discretize and then linearize approach. An advantage of this apporach is that after numerically approximating the discrete-time dynamics, the linearization can be obtained at a desired accuracy.

A very popular
way to obtain discret-time dynamics is with the explicit Euler
integrator, which is computationally inexpensive, but can be inaccurate and unstable.
Therefore, it is usually more efficient to resort to
higher-order integration schemes, such as, e.g., the popular Runge-Kutta methods.
Finally, we should further stress that there also exist implicit integration schemes,
which require more computations per step, but they are typically much more stable
and accurate than explicit schemes for some classes of systems. Unfortunately,
the selection of the least computationally demanding integrator which delivers
sufficient accuracy depends on the problem setting and typically requires some
trial-and error approach, which can be educated using some guidelines based on
the theoretical properties of each integrator~\cite{Quirynen2017,Quirynen2014a,Hairer1993,Hairer1996,Butcher2003}.

In this work, we relied on the \gls{rti} implementation provided by $\texttt{acados}$~\cite{Verschueren2019},
which consists of tailored efficient implementations of \gls{qp} solvers,
numerical integration schemes, and all other components of the \gls{rti} scheme.

\section{Results}\label{sec:results}
In this section we discuss the implementation details and results obtained
from the simulations and experiments with the \gls{nmpc} scheme proposed
in section \ref{sec:nmpc}.
\subsection{Implementation details}
\label{sec:implementation}
To check the efficacy of our \gls{rti} based \gls{nmpc} algorithm with the proposed
features mentioned in Section \ref{sec:features}, we performed
several simulations and experiments in challenging scenarios.
The simulation and experiments were performed on the \gls{hyq} robot of mass $m = 87.4 \,\mathrm{kg}$ and
inertia $ {}_\mathcal{C}\mx{I}_\mathrm{c} ( I_\mathrm{xx} = 4.0745, I_\mathrm{xy}=0.1458, I_\mathrm{xz} = -0.2245,
I_\mathrm{yy} = 11.3576, I_\mathrm{yz} = -0.0133, I_\mathrm{zz}  = 12.5675)$.
The feedback gains used in the \gls{wbc} are $\mx{K} = \mathrm{diag}(1500,1500,1500, 100,100,100)$ and $\mx{D} = \mathrm{diag}(1000,1000,1000,50,50,50)$. We chose the weights $\mx{S} = \mathrm{diag}(5,5,10,10,10,10)$ and
$\mx{T} = \mathrm{diag}(1000,\cdots,1000)$ for the \gls{qp} \eqref{eq:qp_wbc}.
The parameters and weights used by the \gls{nmpc} are reported in Table
\ref{tab:nmpc_parameters} and \ref{tab:nmpc_weights}, respectively.
In all of our simulations and experiments, we do not set any weights on the
\gls{com} $X$-$Y$, roll ($\phi$) and pitch ($\theta$) tasks because we wanted
the \gls{nmpc} to sort out these \gls{com} trajectories autonomously.
\begin{table}[h!]
	\caption{\small \gls{nmpc} parameters}
	\begin{center}
		\begin{tabular}{@{} l l l l @{}}
			\toprule[0.4mm]
			\textbf{Parameter} & {\textbf{Symbol}} & \textbf{Value}  & \textbf{Unit} \\
			\midrule
			Number of state & $n_x$&  12  & -   \\
			Number of control inputs & $n_u$&  12  & -    \\
			Number of model parameters & $n_a$&  16  & -    \\
			Time horizon & $T$&  2  & $\mathrm{s}$    \\
			Sampling time &$T_\mathrm{s}$&  0.04  & $\mathrm{s}$    \\
			Prediction horizon & $N$&  50  & -    \\
			Friction coefficient & $\mu$	&  0.7     &  - \\
			\gls{grfs} lower bound & $\underline{\vect{f}}_\mathrm{z}$&  0  & $\mathrm{N}$    \\
			\gls{grfs} upper bound &  $\overline{\vect{f}}_\mathrm{z}$&  500  & $\mathrm{N}$    \\
			Hip-to-foot position reference & ${}_\mathcal{C}\vect{p}_{\mathrm{hf},i}^{\mathrm{ref}}$&  (0.0, 0.1, -0.55)  & $\mathrm{m}$ \\
			Regularization parameter & $\rho$& $3\times 10^{-5}$  & -  \\
			\bottomrule[0.4mm]
		\end{tabular}
		\label{tab:nmpc_parameters}
	\end{center}
\end{table}
\begin{table}[h!]
	\caption{\small Weights used in \gls{nmpc}}

	\begin{center}
		\begin{tabular}{@{} l l l @{}}
			\toprule[0.4mm]
			\textbf{Cost} & \textbf{Weight} & \textbf{Value} \\
			\midrule
			\multirow{4}{4em}{State}
			&$\mx{Q}_{\vect{p}_\mathrm{c}}$  & $\mathrm{diag}$($0$, $0$, $0$)   \\
			&$\mx{Q}_{\vect{v}}$	 & $\mathrm{diag}$($100$, $100$, $100$) \\
			&$\mx{Q}_{\bm{\Phi}}$	  & $\mathrm{diag}$($0$, $0$, $100$) \\
			&$\mx{Q}_{\bm{\omega}}$ 	  & $\mathrm{diag}$($100$, $100$, $1000$)  \\
			\midrule
			\multirow{3}{4em}{Force}
			&$\mx{R}_\mathrm{x}$ 	  & $1\times 10^{-3}$    \\
			&$\mx{R}_\mathrm{y}$ 	  & $1\times 10^{-3}$    \\
			&$\mx{R}_\mathrm{z}$	  & $8\times 10^{-4}$ \\
			\midrule
			\multirow{3}{4em}{Mobility}
			&$\mx{M}_\mathrm{x}$ 	  & $1\times 10^{-4}$    \\
			&$\mx{M}_\mathrm{y}$ 	  & $2\times 10^{-3}$    \\
			&$\mx{M}_\mathrm{z}$	  & $1000$ \\
			\midrule
			\multirow{3}{4em}{Force robustness}
			&$\mx{P}_\mathrm{x}$ 	 &  $100$\\
			&$\mx{P}_\mathrm{y}$ 	 &  $100$ \\
			&$\mx{P}_\mathrm{z}$	 &  $1$  \\
			\bottomrule[0.4mm]
		\end{tabular}
		\label{tab:nmpc_weights}
	\end{center}
\end{table}
\subsubsection{Discretization}
For the discretization of the dynamic constraints,
we mainly investigated two integration schemes, i.e, a single step of the explicit
and implicit Euler, both of order $1$ due to their low computational complexity that
favors our real-time implementation needs. We compared the accuracy of these single-step
first-order integration schemes with more accurate integration schemes and verified
that our results were accurate enough for the purpose of our control task. We chose the
implicit Euler integration scheme of order $1$ because of its stability and accuracy properties.
The sampling time $T_\mathrm{s}=40 \,\mathrm{ms}$ was chosen and it was sufficient to conduct
\gls{nmpc} computation \textit{online} along with the other necessary computations for the re-planning.
\subsubsection{\gls{nmpc} software}
We use the $\texttt{acados}$ software package \cite{Verschueren2019} to implement
the \gls{rti} scheme described in Section \ref{sec:rti_scheme}. Since $\texttt{acados}$ comes
with a Python interface allowing rapid prototyping, we first tuned the algorithm in
 simulation and then used the generated $\texttt{C}$-code to perform real experiments.
We employ the \gls{qp} solver High-Performance Interior Point Method (HPIPM) \cite{Frison2020},
which exploits the sparsity structure of the MPC QP sub-problem \eqref{eq:qp_subproblem},
and supports inequality constraints.

The computation time required by the \gls{nmpc} was in the range of $5$-$7 \, \mathrm{ms}$
with the time horizon of $2 \, \mathrm{s}$ and prediction horizon $N$ equal to $50$ on the
on-board computer (a Quad Core Intel Pentium PC104 @ 1GHz) of \gls{hyq} for all the experiments. This computation time
corresponds to the feedback phase of the \gls{rti} scheme where
the \gls{qp} \eqref{eq:qp_subproblem} is solved after receiving the current state of the robot.
The preparation phase of the \gls{rti} takes about $2$-$3 \, \mathrm{ms}$ which
is a fraction of the sampling time we chose. Refer to Section \ref{sec:rti_scheme} for more details on these phases of the \gls{rti} scheme.

\subsubsection{Integration with the locomotion framework}
The \gls{nmpc} is integrated in a ROS node that publishes  $\vect{x}^\mathrm{p}$ and $\vect{u}^\mathrm{p}$
at a frequency of 25 $\mathrm{Hz}$. The on-board computer along with our locomotion framework (\gls{wbc}
Interface, \gls{wbc}, etc., illustrated in Fig. \ref{fig:blockDiagram})
runs a node that subscribes to the topic of the \gls{nmpc} ROS node.
ROS is not a real-time operating system, so it introduces quite a significant and unpredictable communication delay if the  \gls{nmpc}
is run on an external (e.g. more powerful) computer.
These delays are difficult to compensate for and they can cause the loss of
synchronization between the \gls{nmpc} ROS node
and the \gls{wbc} interface.
Therefore, we decided to launch the \gls{nmpc} node natively from the
on-board computer to avoid the communication delays between two different computers.
Even though we chose not to use a more powerful off-board computer for \gls{nmpc} ROS node,
we obtained a better performance in the overall implementation by avoiding the communication delays of ROS.

\subsection{Simulations}
We show our \gls{nmpc} planner in action on challenging terrain starting
with simulations. These simulations are pallet crossing, walking over
unstructured rough terrain and walk into a V-shaped Chimney.
\subsubsection{Pallet crossing}
\label{sec:pallet_cross}
In this simulation \gls{hyq} traverses pallets of
different heights, placed at varying distances form each other. This simulation highlights
the importance of including mobility in the \gls{nmpc} formulation \eqref{eq:mobility_cost}.
In particular, the simulation scenario includes a set of pallets, each one of $1 \, \mathrm{m}$ length,
with variable heights between $0.13$ and  $0.17\, \mathrm{m}$
and placed at unequal gap lengths ranging from $0.2$ and  $0.7\,\mathrm{m}$.
We performed multiple trials commanding the robot to move forward at
different velocities i.e.,  $0.05\,\mathrm{m/s}$ and  $0.1\, \mathrm{m/s}$ to show the repeatability of our approach.
To avoid stepping on undesired locations such as pallet edges and to prevent foot or shin collisions,
the nominal footholds were  adjusted by using the~\gls{vfa} (see Section \ref{subsub:vfa}).
Figure~\ref{fig:multi_pallet} shows the results of five different trials for
each of the commanded velocities. The middle plot shows the pitch angle $\theta$ of the robot as it
traverses the scenario.  Since the
robot is only commanded to move along its $X$ direction with a constant forward
velocity and the foot locations are provided as known quantities
to the \gls{nmpc}, the adjustment in pitch is the result of minimizing the deviation
 from the hip-to-foot position corresponding to high mobility for
all four legs.
 Without this feature,
the robot would maintain a constant horizontal orientation eventually reaching low mobility
 in some legs as shown in the attached video for a single pallet simulation.

We also showed in the accompanying video\footnote{https://www.dropbox.com/sh/mkr4pftcug6jlo7/AABNqu1AsGED2WSR\\8IqvaiUla?dl=0} the simulation of a walk on
randomly generated rough terrain (using a tool by \cite{Bulat2020})
further stressing the advantages of mobility cost mentioned earlier.
 %
 %
\begin{figure}[!h]
	\centering
	\includegraphics[width=\columnwidth]{figures/multi_pallet.pdf}
	\caption{\small Simulation of pallet crossing scenario for five different trials commanding the robot to cross at 0.1~$\mathrm{m/s}$~(blue) and 0.05~$\mathrm{m/s}$~(red). The top graph shows the
		actual velocities (solid lines) and their respective references (dashed lines).
		 The graph in the middle shows the pitch angle and the dashed vertical lines indicate the edges of the pallets for that specific
		  location in the plot. The bottom graph shows the \gls{com} $Z$ position
		   for all the trials and the feet trajectories for one of the trials performed at 0.1 $\mathrm{m/s}$. The color of swings are related to the different legs.}
	\label{fig:multi_pallet}
\end{figure}
\subsubsection{Walk into a V-shaped chimney}\label{sim:vshaped_wall}
In this simulation we show \gls{hyq} walking at $0.03\, \mathrm{m/s}$ commanded velocity in the $X$ direction into a
V-shaped chimney with friction coefficient $\mu= 0.7$ and walls inclined at $35^\circ$ to the ground.
This simulation exploits the cone constraints and force robustness cost defined inside the
 \gls{nmpc} formulation that is vital to achieve this walk successfully.
The robot receives online an update of the map of the environment through an on-board camera to get the
information about the normals at the location of the contact. These normals are used to formulate the force
 robustness cost \eqref{eq:force_robustness} in the contact frame $\mathcal{K}$.
With this cost, the NMPC provides optimal \gls{grfs} to stay close to the normals
of friction cones at the contacts.

As shown in the accompanying video, without the cone constraints the robot
slips while climbing the chimney and ultimately falls. When the force robustness cost is enabled, the
forces are regularized to stay in the middle of the cones, thanks to the robustness
feature described in Section \ref{subsec:force_robustness}. In this case the robot walks successfully
into the chimney.
In Fig. \ref{fig:friction_cone_forces}, it can be seen that the longitudinal and lateral
components of the GRF at the \gls{lf} foot stay within the bound $\mu f_\mathrm{z}$ (in red) imposed
by the cone constraints. Moreover, Fig. \ref{fig:friction_cone_violation}
plots the normal versus the resultant of $X$ - $Y$ components of the \gls{grfs}
together with the cone bound $\mu f_\mathrm{z}$ (red line). The picture shows that
the GRF stays well within the bound without any violation.
Therefore, including the force regularization term
enables the \gls{nmpc} to account for the estimation error in the orientation of
 contact normals and increase robustness to the external disturbances.
\begin{figure}[!h]
	\centering
	\includegraphics[width=\columnwidth]{figures/cone_forces.pdf}
	\caption{\small Walk into a V-shaped chimney simulation: \gls{grfs} of \gls{lf}
		leg for a single gait cycle with cone constraints
		and regularization cost. Both the longitudinal $f_\mathrm{x}$ and
		lateral $f_\mathrm{y}$ lie conservatively within the bound $\mu f_\mathrm{z}$ imposed by cone constraints.}
	\label{fig:friction_cone_forces}
\end{figure}
\begin{figure}[!h]
	\centering
	\includegraphics[width=\columnwidth]{figures/cone_violation.pdf}
	\caption{\small Walk into a V-shaped chimney simulation: Normal force $f_z$ versus resultant of X
		and Y forces $\sqrt{f_\mathrm{x}^2 + f_\mathrm{y}^2}$ of the \gls{lf} leg for a single gait cycle
		expressed in the contact frame. The red line is the cone bound $\mu f_\mathrm{z}$.}

	\label{fig:friction_cone_violation}
\end{figure}

Apart from the simulation mentioned above, we also have added in the attached video, the simulations regarding the kinematic robustness (see Section \ref{subsec:kinematic_robustness}) and importance of the re-planning at a higher rate.
For the kinematic robustness simulation, the robot is pushed with $200\, \mathrm{N}$ of lateral force for $1 \, \mathrm{s}$ both in case of with (higher weight on \gls{grfs} Z) and without (lower weight on \gls{grfs} Z) \gls{zmp} margin. The \gls{zmp} margin plots for this simulation can be seen in Fig. \ref{fig:stability_margin}
where the \gls{zmp} margin is improved in case of
the red line compared to the blue one because the \gls{grfs}
$Z$ components are penalized relatively ($100$ times) more for the red line.
Because of the improved margin the robot walks stably, whereas
it falls while walking when there is no margin.
\begin{figure}
	\centering
	\includegraphics[width=\columnwidth]{figures/stability_margin.pdf}
	\caption{\small Plot of the \gls{zmp} margin $m_\mathrm{c}$ used to measure locomotion stability. The robot is pushed immediately after $2\,\mathrm{s}$ with lateral force of $200\, \mathrm{N}$ while walking on a flat terrain at  $0.1\,\mathrm{m/s}$ \gls{com} $X$ velocity. The discontinuities are due to the switching between 3/4 stance legs in a crawl gait.}
	\label{fig:stability_margin}
\end{figure}

In second simulation, the robot is commanded with a constant \gls{com} X velocity and heading velocity simultaneously. In case of re-planning at lower rate of $0.8\, \mathrm{Hz}$, the robot becomes unstable and falls due to increase in the model uncertainties and tracking errors. We would like to stress that when the re-planning is done at a lower
frequency than $25\,\mathrm{Hz}$, the robot is in open-loop for the time interval between two consecutive re-planning instances, hence, it is not \gls{nmpc} anymore but an online open-loop trajectory optimization. On the other hand, at a higher re-planning frequency of $25\,\mathrm{Hz}$ the robot walks successfully because
the \gls{nmpc} compensates for the model uncertainties and tracking errors.

\subsection{Experiments}
We performed three different experiments to demonstrate the
real-time implementation of our \gls{nmpc} running on the
on-board computer of the robot as follows.
\subsubsection{Omni-directional walk} \label{subsec:omnidirectional}
With this experiment, we wanted to show the omni-directional walk
performed by HyQ with the \gls{nmpc} on a flat terrain. This experiment
validates that the \gls{nmpc} computes feasible trajectories after receiving
different velocity commands from the user while walking.
In this experiment, the robot was commanded with a longitudinal velocity
${}_\mathcal{H} \vect{v}_\mathrm{c,x}^{\mathrm{usr}}$ by the user to walk forward/backward and then a lateral velocity. Finally, a heading velocity $\omega^{\mathrm{usr}}_\mathrm{z}$ was commanded
to turn in the left/right direction.
Figure~\ref{fig:omni_walk_comPos}
shows the \gls{com} $X$-$Y$ position
and yaw angle of the robot base and it can be noticed that the actual values
follow very closely the planned trajectories provided by the \gls{nmpc}.
Figure~\ref{fig:omni_walk_velocity}
depicts the deviation of the actual velocities from the reference values
while following the planned trajectories form \gls{nmpc}.
It can be seen in Fig.~\ref{fig:omni_walk_forces} that
the \gls{grfs} generated by the \gls{wbc} are compliant with the planned values
$\vect{u}^\mathrm{p}$ and again the actual values of \gls{grfs}
follow closely the planned values. From these plots, it can be observed that the
continuous re-planning with \gls{nmpc} plays an important role to achieve
 good tracking of the planned trajectory.

\begin{figure}[!h]
	\centering
	\includegraphics[width=\columnwidth]{figures/omni_walk_comPos.pdf}
	\caption{\small \gls{com} X-Y position and
		yaw $\psi$ in omni-directional walk experiment. The blue, dotted red and dashed
		green line represent the actual, planned and reference values, respectively.}
	\label{fig:omni_walk_comPos}
\end{figure}
\begin{figure}[!h]
	\centering
	\includegraphics[width=\columnwidth]{figures/omni_walk_velocity.pdf}
	\caption{\small The longitudinal $\dot{\vect{p}}_\mathrm{c,x}$, lateral $\dot{\vect{p}}_\mathrm{c,y}$  and angular $\bm{\omega}_\mathrm{z}$ velocity  of the robot in omni-directional walk
		 experiment.  The blue, dotted red and the green line represent the actual,
		  planned and reference values, respectively.}
	\label{fig:omni_walk_velocity}
\end{figure}
\begin{figure}[!h]
	\centering
	\includegraphics[width=\columnwidth]{figures/omni_walk_forces.pdf}
	\caption{\small \gls{grfs} of one gait cycle in the omni-directional walk experiment
		 (We show only one cycle for better visibility of the data). The green,
		  dotted red and dashed blue line represent the output from \gls{wbc}
		   i.e., $f_{\mathrm{z},i}^\mathrm{d}$, planned and reference values, respectively.}
	\label{fig:omni_walk_forces}
\end{figure}
\subsubsection{Traversing a static pallet}\label{sub:traversing_pallet}

The purpose of this experiment was to demonstrate that the mobility cost \eqref{eq:mobility_cost}
incorporated in the \gls{nmpc} formulation provides the necessary body pitch  for the robot
to traverse over a static pallet while maintaining good leg mobility. The pallet used in this experiment was $0.13\,\mathrm{m}$ in height and $0.8\,\mathrm{m}$ in length.
Figure~\ref{fig:staticPallet_pitch} shows that the robot
pitches up while climbing up the pallet and pitches down consequently while climbing
down from the pallet. As shown in the attached video in simulation, when the mobility cost is deactivated,
the \gls{nmpc} keeps the base orientation horizontal. This causes a reduced hip-to-foot distance
 while stepping up/down on the pallet ultimately resulting in low leg mobility. When mobility
 cost is activated, it directs the \gls{nmpc} solution to achieve the necessary pitch that allows
  to maintain the hip-to-foot distance at the reference value and hence the leg mobility is improved.
Moreover, the \gls{vfa} provides the
corrected foot position (i.e., to avoid shin or feet collisions with the edges of the pallet) to the \gls{nmpc}
and this further enhances the overall locomotion.
\begin{figure}[!h]
	\centering
	\includegraphics[width=\columnwidth]{figures/staticPallet_pitch.pdf}
	\caption{\small Planned (red) and Actual (dashed blue) pitch of the robot
		base while traversing a static pallet in the experiment at
		a commanded \gls{com} $X$ velocity of $0.03 \, \mathrm{m/s}$.}
	\label{fig:staticPallet_pitch}
\end{figure}
\subsubsection{Traversing a repositioned pallet }
In this experiment we tested our \gls{nmpc} to plan the robot motion in real-time
by adapting the changes in the environment with the help of \gls{vfa}.
As it can be seen from the attached video, when the pallet ($0.13\,\mathrm{m}$ in height and $0.8\,\mathrm{m}$ in length) is pushed in
front of \gls{hyq} while walking, the heightmap detects the pallet
and the \gls{vfa} provides updated foot locations to the \gls{nmpc}.
The \gls{nmpc} after receiving these updated foot locations
delivers a solution by pitching up the robot base in order to adapt to
the change in the environment while maintaining the mobility.
 Even though the mobility cost is
defined for the stance legs, it is interesting to notice that the \gls{nmpc} decides to adjust the base pitch while swinging the \gls{rf} leg onto the pallet (see Fig. \ref{fig:pitch_while_swing})
by forecasting the hip-to-foot distance at the touchdown.
This experiment highlights the advantage of the predictive control
over the traditional control apporaches for its ability to incorporate the
knowledge of the future states. It also validates
the effectiveness of our mobility cost in the \gls{nmpc} coupled with
the \gls{vfa} to adapt to the changes in the locomotion environment.
\begin{figure}[!h]
	\centering
	\includegraphics[width=\columnwidth]{figures/pitch_while_swing.pdf}
	\caption{\small Robot base pitch achieved during the swing of \gls{rf}
		leg while traversing a repositioned pallet in the experiment a commanded \gls{com} $X$ velocity of $0.05 \, \mathrm{m/s}$.
		The red and dashed blue line are planned and actual values.}
	\label{fig:pitch_while_swing}
\end{figure}
\section{Conclusion}\label{sec:conclusion}
In this work we have developed a real-time \gls{nmpc} that enhances the leg
mobility of a quadruped robot for rough terrain locomotion with the help of a proposed
mobility metric  called mobility factor.
The contact sequence parameters embedded inside the \gls{srbd} model allowed
us to encode the complementarity constraints directly, and without a need to enforce these constraints separately in the \gls{nmpc}.
We exploited the \gls{rti} scheme for our \gls{nmpc} that enabled us to close the loop
at  $25\,\mathrm{Hz}$ on the \gls{nmpc} with a
time horizon of $2 \,\mathrm{s}$. Closing the loop on \gls{nmpc} at  $25\,\mathrm{Hz}$
allowed us to compensate for the state drifts due to model
uncertainties and tracking errors, and also adapt to the changes in
the environment while following user commands both in the simulations
and experiments.

In our \gls{nmpc}, the  mobility cost penalizes the hip-to-foot distance from a
reference value corresponding to a high mobility factor and hence it
directs the \gls{nmpc} to compute essential robot orientation to maintain
mobility while respecting the kinematic limits.
This is evident from the pallet experiments where we also included the \gls{vfa}
to correct undesired foot positions defined by the heuristics and avoid possible shin collision.
Including the kinematic robustness in our \gls{nmpc} improved the
locomotion stability of the robot in all of our experiments and
simulation by keeping a sufficiently large \gls{zmp} margin from support polygon boundaries.
Incorporating a force robustness term in the \gls{nmpc}
ensures that the \gls{grfs} stay close to the contact normals and hence, it enables the robot to cope with the estimation error of the orientation of the contact normals.

Our gait scheduler provides
the contact sequences considering the user defined
gait parameters and recovers the synchronization between planned and actual touchdown of the robot.
This is essential when traversing rough terrain where premature or delayed touchdown could occur very frequently.
The robocentric stepping strategy computes foot positions coherent with the base evolution avoiding error accumulation.
The nominal foot positions supplied by the reference generator
are further improved by  the \gls{vfa} that corrects the foot locations considering the terrain map.

With our \gls{nmpc} we demonstrated successful dynamic locomotion in simulation as
well as in the experiments on different rough terrains.
In our future work we would like
to extend our \gls{nmpc} to optimize the step timing and foot locations.  Additionally,
the reference generator does not provide references by rejecting the external disturbances
 acting on the robot state, hence the robot complies transparently
with these disturbances. Therefore, in the future we plan to  empower the reference
generator to reject disturbances to bring back the robot from a perturbed state to a state coherent with the user commands.

redpajama_set_name: RedPajamaArXiv


text:
Q: Certbot not creating .well-known/acme-challenges file My domain is:
monxas.ninja

I ran this command:
sudo certbot --apache --debug-challenges

It produced this output:
   Obtaining a new certificate
/usr/lib/python3/dist-packages/josepy/jwa.py:107: CryptographyDeprecationWarning: signer and verifier have been deprecated. Please use sign and verify instead.
  signer = key.signer(self.padding, self.hash)
Performing the following challenges:
http-01 challenge for monxas.ninja
Waiting for verification...

-------------------------------------------------------------------------------
Challenges loaded. Press continue to submit to CA. Pass "-v" for more info about
challenges.
-------------------------------------------------------------------------------
Press Enter to Continue
/usr/lib/python3/dist-packages/josepy/jwa.py:107: CryptographyDeprecationWarning: signer and verifier have been deprecated. Please use sign and verify instead.
  signer = key.signer(self.padding, self.hash)
Cleaning up challenges
Failed authorization procedure. monxas.ninja (http-01): urn:acme:error:unauthorized :: The client lacks sufficient authorization :: Invalid response from http://monxas.ninja/.well-known/acme-challenge/Wt_CvapZhIJt3EDdoIjop4Lun7V4B_JpWmnpyMxz7es: "<!DOCTYPE HTML PUBLIC \"-//IETF//DTD HTML 2.0//EN\">\n\n<title>404 Not Found</title>\n\n
Not Found
\n<p"

IMPORTANT NOTES:
 - The following errors were reported by the server:

   Domain: monxas.ninja
   Type:   unauthorized
   Detail: Invalid response from
   http://monxas.ninja/.well-known/acme-challenge/Wt_CvapZhIJt3EDdoIjop4Lun7V4B_JpWmnpyMxz7es:
   "<!DOCTYPE HTML PUBLIC \"-//IETF//DTD HTML
   2.0//EN\">\n\n<title>404 Not
   Found</title>\n\n
Not Found
\n<p"

My web server is (include version):
Apache version 2.4.25

The operating system my web server runs on is (include version):
Raspbian GNU/Linux 9

I can login to a root shell on my machine (yes or no, or I don't know):
yes

I'm using a control panel to manage my site (no, or provide the name and version of the control panel):
no

the dns records are
A *.monxas.ninja 83.56.8.166 300

A monxas.ninja 83.56.8.166 300

and i don't use AAAA
Also, manually created the well known path, it's accesible as you can see here:
http://monxas.ninja/.well-known/acme-challenge/

A: I think I may have found the solution. It was my solution anyway...
I had the inclination to look at the renewal file and found the solution within. I don't know how or why this was created incorrectly, but the webroot was wrong in renewal file on both servers.  When I corrected it, both renewals completed as expected.
/etc/letsencrypt/renewal/www.server.com.conf
Look for the entry under [[webroot_map]]
I hope this helps someone.  I spent two days scratching my head before thinking to look at the renewal file.

redpajama_set_name: RedPajamaStackExchange


Consolidation in the aerospace and defense supply chain can be big news when it involves publicly traded companies as prominent as United Technologies and Goodrich.
Tactair Fluid Controls Inc. has been selected by General Atomics to provide select components for wheel brake control on the Spaceship Company's WhiteKnightTwo (WK2) launch vehicle for the SpaceShipTwo (SS2).
Specifically, Tactair will be providing brake metering valves and hydraulic accumulators.