%###########################################################################
%
%   Controllers
%
%###########################################################################
\chapter{Controllers} \label{chap:controllers}
In this chapter we briefly present the \enquote{Nonlinear Model Predictive Control Framework} which is developed by \cite{Meduri} for whole body motion planning and implemented for Solo12.
%
\section{MPC Controller} \label{sec:MPC}
The rigid body dynamics of any robot can be described as follows
%
\begin{align}
	\Mb(\qb) \dot{\vb} + \Nb(\qb, \vb) = \Sb^T\boldsymbol{\tau} + \Sum_{j=1}^N\Jb_j^T\boldsymbol{\lambda_j}
\end{align}
%
where $\qb$ represents the generalized joint configuration of the robot, and $\vb$ is the generalized velocity vector. $\Mb(\qb)$ is the mass matrix for the given robot configuration, $\Nb(\qb, \vb)$ is coriolis matrix which consist of the nonlinear effects of the dynamics, $\tau$ is the vector of joint torques, $\Sb$ is the selection matrix that defines the under actuation of the robot, $\Jb_j$ are the end effector jacobians and $\boldsymbol{\lambda} = [\fb, \boldsymbol{\tau}]$ is the vector of forces and torques applied at each end effector.\par
%
In the case of a floating base robot, the dynamics can be split into two parts, the actuated and unactuated part
%
\begin{align}
	\Mb_u(\qb) \dot{\vb} + \Nb_u(\qb, \vb) &= \Sum_{j=1}^N\Jb_{u,j}^T\boldsymbol{\lambda_j}\\
	\Mb(\qb)_a \dot{\vb} + \Nb_a(\qb, \vb) &= \boldsymbol{\tau} + \Sum_{j=1}^N\Jb_{a,j}^T\boldsymbol{\lambda_j}
\end{align}
%
where the subscript \enquote{a}, and \enquote{u} correspond to \enquote{actuated} and \enquote{unactuated} dynamics respectively. The unactuated dynamics is equivalent to the Newton-Euler equations of the center of mass (CoM) for the robot's base
%
\begin{align}
	\begin{bmatrix}
		\dot{\lb}\\
		\dot{\kb}
	\end{bmatrix} = 
	\begin{bmatrix}
		m\gb + \Sum_{j=1}^Nn_j\fb_j\\
		\Sum_{j=1}^Nn_j\lef(\rb_j-\cb)\times \fb_j + \boldsymbol{\tau}_j\rig
	\end{bmatrix}
\end{align}
%
where $\lb$, $\kb$ are the linear and angular momentum, $m$ is the robot mass, $\gb$ is the gravity vector, $\cb$ represents the center of mass location, $n_j$ is a binary integer that describes whether the end effector $j$ is in contact, $\fb_j$, $\boldsymbol{\tau}_j$, $\rb_j$ are the end effector force, torque and location respectively.\par
%
Splitting the dynamics enables multi-contact motion generation by only considering the unactuated dynamics or centroidal dynamics of the robot. Subsequently, a feasible whole-body trajectory can then be determined based on the centroidal plan and desired whole body tasks, provided there is sufficient torque authority. This is an attractive approach since it breaks the original highly nonlinear optimization problem into two simpler sub-problems. A desired motion plan using the centroidal dynamics can be generated by solving the following discrete optimal control problem (OCP)
%
\begin{align}
	\underset{\cb,\dot{\cb},\kb,\fb,\boldsymbol{\tau}}{\boldsymbol{min}}.& \;\Sum_{t=0}^{T-1}\phi_r^t(\cb_t,\dot{\cb}_t,\kb_t,\fb_t,\boldsymbol{\tau}_t) + \phi^T(\cb_T,\dot{\cb}_T,\kb_T,\fb_T,\boldsymbol{\tau}_T) \nonumber \\
	\boldsymbol{s.t}.& \; \cb_{t+1} = \cb_t + \dot{\cb}_t\Delta t \nonumber\\
	&\: \dot{\cb}_{t+1} = \dot{\cb}_t + \sum_{j=1}^N\lef (n_t^j\frac{\fb_t^j}{m} + \gb) \Delta t \rig\nonumber \\
	&\: \kb_{t+1} = \kb_t + \sum_{j=1}^N\lef n_t^j ((\rb_t^j - \cb_t) \times \fb_t^j + \boldsymbol{\tau}_t^j) \Delta t \rig \nonumber \\
	&\: \forall_{t,j} \: \sqrt{(\fb_{t,x}^j)^2 + (\fb_{t,y}^j)^2} \leq \mu \fb_{t,z}^j \nonumber \\
	&\: \forall_{t,j} \: \fb_{t,z}^j \geq 0 \nonumber \\
	&\: \forall_{t,j} \: \rb_t^j \in \boldsymbol{\Psi} \nonumber \\
	&\: \forall \cb_t \in \boldsymbol{\Omega} \nonumber \\
	&\: \cb_0 , \dot{\cb}_0 = \cb_{init}, \dot{\cb}_{init}
\end{align}
%
where $\phi_r^t$ is the running cost, and $\phi^T$ is the terminal cost, $\Delta t$ is the time discretization, $\mu$ is the coefficient friction, $\boldsymbol{\Psi}$ is the set of all allowed stepping locations, $\boldsymbol{\Omega}$ are the kinematic constraints, $\cb_{init}$, $\dot{\cb}_{init}$ are the initial conditions of the robot center of mass (COM).\par
%
The optimal joint trajectory is generated by solving an inverse kinematics (IK) problem which tracks the optimal centroidal momentum obtained from the previous step using the centroidal momentum matrix, along with additional full body tasks. The generated momentum trajectory from the inverse kinematics problem is then used as a soft constraint in the centroidal OCP to obtain a refined centroidal trajectory and forces. This process is iterated until the two sub-problems converge. Finally, inverse dynamics (based on within the Recursive Newton Euler Algorithm (RNEA)) \cite{Featherstone} is used to map state trajectories and contact forces to actuated joint torques
%
\begin{align} \label{eq:rnea}
	\boldsymbol{\tau}_{R\!N\!E\!A} = \Mb_a(\qb)\dot{\vb} + \Nb_a(\qb, \vb) - \sum_{j=1}^{N}\Jb_{a,j}^T\boldsymbol{\lambda}_j.
\end{align}
%
An overview of the entire framework is shown in~\figref{fig:mpc}.
%
\begin{figure}[!htbp]
	\begin{center}
		\includegraphics[scale = 0.4]{Bilder/MPC_Controller.pdf}
		\caption{A bird's eye view of the nonlinear MPC framework \cite{Meduri}.}
		\label{fig:mpc}
	\end{center}
\end{figure}\\
%
Given the current states of the robot $\qb_{init}$, $\vb_{init}$, $\dot{\vb}_{init}$, desired gait, planning horizon and velocity, a contact plan is either generated and adapted using the Raibert controller or pre-defined without contact adaptation for general motions. The BiConMP framework takes the input states and computes the optimal end effector forces, joint positions, joint velocities and joint acceleration trajectories for the entire horizon. Given the desired joint trajectories and contact forces, we use~\eqref{eq:rnea} along with a low joint impedance around the desired states to compute the desired torques at $1\: \unit{kHz}$
%
\begin{align} \label{eq:desired_torqu}
	\boldsymbol{\tau}_i = \boldsymbol{\tau}_{R\!N\!E\!A,i} + \Kb_p (\qb_{d,i} - \qb_{a,i}) + \Kb_d (\vb_{d,i} - \vb_{d,i})
\end{align}
%
where subscripts \enquote{d} and \enquote{a} stand for \enquote{desired} and \enquote{actual} respectively. The desired torques is then sent to the robot which are tracked on board at $10\: \unit{kHz}$. The entire full body planning loop is replanned at $20\:  \unit{Hz}\:(50\: \unit{ms})$ to update optimal trajectories.
%
\section{Observer-Based Controller} \label{sec:observer}
As discussed in section \ref{sec:MPC}, one important assumption in the MPC controller framework is that all the states of the robot, $\qb$ and $\dot{\qb}$, are available for the control task. The joint positions and joint velocities are measured directly by the encoders, and for now the base placement, $\qb_b \in \mathit{SE}(3)$ (position and orientation), and base linear velocity, $\vb_b \in \mathbb{R}^3$, are measured by motion capturing system. As mentioned before, the goal of EKF estimator is to have an accurate estimate of the robot base state completely based on proprioceptive sensors.\par
%
We wish to design an observer-based controller. In this framework, first the states are estimated by EKF, then they are used by the controller in order to generate the trajectories for given states and control input. One important aspect of such a structure, is observability of the states. This problem consists of identifying if a set of available measurements is enough to be able to estimate the state of the system. Since in our estimation problem, both the transition and observation functions in \eqref{eq:nonlinear_system} are nonlinear, we need to perform a \enquote{nonlinear observability analysis}. Analyzing the observability characteristics of the underlying nonlinear system reveals the theoretical limitations of state estimation and can validate the employed approach. A detailed analysis of observability, when the measurement model is the base linear velocity measured by the forward kinematics of the leg, is given in \cite{Bloesch}. We briefly mention two essential points of this analysis. First, with the proposed EKF, the four dimensional manifold composed of robot's base position and yaw angle (rotation around gravity vector $\gb$) is always unobservable. The second point is that as long as one foot is in contact the base linear velocity and robot's base roll and pitch angles are always observable.\par
%
The unobservability of the base position, and base yaw angle in our proposed EKF framework imposes some challenges for the control task. In order to deal with this issue, we would like to distinguish between two types of controllers in legged robot locomotion, \enquote{low-level} and \enquote{high-level} control.\par
%
The low-level controller is responsible for motion planning and trajectory generation of the floating base. This controller is an essential part for the task of locomotion. On the other hand, the high-level controllers are implemented on top in order to tackle the different tasks such as localization and navigation in the environment.\par
% 
For the task of locomotion, we need the robot general configuration (including base position, orientation and joint positions), and robot velocity (including base linear, angular velocity and joint velocities). We already mentioned that the joint positions and velocities are sensed through encoders, and angular velocity is measured by the IMU. For the observer-based controller the only challenge is the unobservability of base position and yaw angle. But for the low-level controller, and for motions on flat terrain we only need the relative position of the base to the ground, and the $x$, $y$ position and yaw angle are needed for high-level tasks. Therefore, we will see later that by some modification in the proposed EKF, we can use the estimated states in a feedback scheme with MPC controller in order to generate the trotting motion on the robot without using any exteroceptive sensing, such as Vicon or mounted cameras on the robot.\par
%
In~\figref{fig:observer}, an overview of the observer-based controller is shown, which is later tested on Solo12 robot with trotting motion.
%
\begin{figure}[!htbp]
	\begin{center}
		\includegraphics[scale = 0.5]{Bilder/Observer_Controller.pdf}
		\caption{An overview of the observer-based controller.}
		\label{fig:observer}
	\end{center}
\end{figure}
%
