Legged Robot Control

Footstep Planner

To achieve high-frequency footstep planning, we can use the Linear Inverted Pendulum Model (LIP), i.e., a point mass, approximating the robot's center of mass (CoM), attached to a massless prismatic actuator. The acceleration of the point mass is given in this paper:

$$ \ddot{p}_{com}=\frac{g}{h_{com}}(p_{com}-p_{base}) $$

where $g$ is the gravitational constant, $h_{com}$ is the constant height of the CoM and $p_{com}$ and $p_{base}$ are the $x$, $y$ CoM and base (foot) positions, respectively. The equations of motion of the LIP are given by

$$ \begin{bmatrix} x_{com}\\ x_{com} \\ v_{x, com} \\ v_{y, com} \\ \end{bmatrix}_{k+1} = \begin{bmatrix}\frac{1}{2e^{\frac{\Delta t}{\tau}}} + \frac{e^{\frac{\Delta t}{\tau}}}{2} & 0 & \frac{-\tau}{2e^{\frac{\Delta t}{\tau}}} + \frac{\tau e^{\frac{\Delta t}{\tau}}}{2} & 0 \\ 0 & \frac{1}{2e^{\frac{\Delta t}{\tau}}} + \frac{e^{\frac{\Delta t}{\tau}}}{2} & 0 & \frac{-\tau}{2e^{\frac{\Delta t}{\tau}}} + \frac{\tau e^{\frac{\Delta t}{\tau}}}{2}\\ \frac{e^{\frac{\Delta t}{\tau}}}{2\tau} - \frac{1}{2\tau e^{\frac{\Delta t}{\tau}}} & 0 & \frac{e^{\frac{\Delta t}{\tau}}}{2} + \frac{1}{2e^{\frac{\Delta t}{\tau}}} & 0 \\ 0 & \frac{e^{\frac{\Delta t}{\tau}}}{2\tau} - \frac{1}{2\tau e^{\frac{\Delta t}{\tau}}} & 0 & \frac{e^{\frac{\Delta t}{\tau}}}{2} + \frac{1}{2e^{\frac{\Delta t}{\tau}}} \end{bmatrix} \begin{bmatrix} x_{com}\\ x_{com} \\ v_{x, com} \\ v_{y, com} \\ \end{bmatrix}_{k} $$

$$ +\begin{bmatrix} \frac{-1}{2e^{\frac{\Delta t}{\tau}}} - \frac{e^{\frac{\Delta t}{\tau}}}{2} & 0 \\ 0 & \frac{-1}{2e^{\frac{\Delta t}{\tau}}} - \frac{e^{\frac{\Delta t}{\tau}}}{2} \\ \frac{1}{2\tau e^{\frac{\Delta t}{\tau}}} - \frac{e^{\frac{\Delta t}{\tau}}}{2\tau} & 0 \\ 0 & \frac{1}{2\tau e^{\frac{\Delta t}{\tau}}} - \frac{e^{\frac{\Delta t}{\tau}}}{2\tau} \end{bmatrix} \begin{bmatrix} x_{base} \\ y_{base} \end{bmatrix}_k $$

where $\tau = \sqrt{\frac{h_{com}}{g}}$ and $\Delta t$ is a chosen discretization time-step. We can represent this discrete linear system as

$$ q_{k+1} = Aq_k + Bu_k $$

where the input $u_k$ is the foot position. In order to build the cost, we plan a reference plan as done in this paper. Using this reference path, our final cost function for the MPC looks as followers:

$$ \begin{align*} \min_{q, u} & \quad \sum_{k=1}^N (p_{com, k+1} + p_{com, k} - 2p_{m, k})^\top Q_q (p_{com, k+1} + p_{com, k} - 2p_{m, k})\\ &+ \sum_{k=1}^N \left((p_{com, k+1} - p_{com, k}) - v_{des, k}\Delta t \right)^\top Q_{dq} \left((p_{com, k+1} - p_{com, k}) - v_{des, k}\Delta t \right)\\ &+ \sum_{k=1}^N (u_k - u_{des, k})^\top Q_u (u_k - u_{des, k})^\ \\ &+ \sum_{k=1}^N \left((u_{k+1} - u_k) - (u_{des, k+1} - u_{des, k}) \right)^\top Q_{du} \left((u_{k+1} - u_k) - (u_{des, k+1} - u_{des, k}) \right)\\ \textrm{s.t.} & \quad q_{k+1} = Aq_k + Bu_k \end{align*} $$

where $p_m$ is the midpoint between feet and $u_{des}$ and $v_{des}$ are the desired foot positions and CoM body frame velocities given by the reference path computations.

Whole Body Controller

We represent the dynamics of the robot via

$$ M\dot{v}+Cv+G = Bu + J^\top_h \lambda_h $$

where $M$ is the mass matrix, $Cv$ is the Coriolis term, $G$ is the gravitational force vector, $B$ is the actuation matrix and $J$ is the Jacobian that relates the velocities of the contact points to the velocities of the robot. The state vector is represented by

$$ x = \begin{bmatrix} q \\ v \end{bmatrix} $$

where $q$ represents the quaternion orientation of the pelvis link, the position of the CoM and the joint positions, while $v$ represents the angular velocity of the pelvis, the velocity of the CoM and the joint velocities. In the case of our high-frequency whole-body controller, we apply $\hat{x}$, a constant estimate of our state, meaning $x$ itself is not a decision variable in our later optimization. This means that the dynamics become linear with respect to our decision variables $\dot{v}$, $u$ and $\lambda_h$.

Now, we have some list of tracking objectives for our robot. These include a CoM trajectory, torso angle and foot trajectories. Let's defined $y_i$ as our task space state for our $i\text{th}$ tracking objective. We can relate the derivative of our task state space to our configuration with

$$ \begin{align*} \dot{y}_i &= \frac{\partial_i}{\partial q}\dot{q} = J_iv \\ \ddot{y}_i &= \dot{J}_iv + J\dot{v} \end{align*} $$

For each tracking objective, we apply task space PD control, i.e.,

$$ \ddot{y}_{cmd, i}=\ddot{y}_{des, i} + k_p(y_{des, i} - y_i)+k_d(\dot{y}_{des, i} - \dot{y}_i) $$

where $\ddot{y}_{cmd, i}$ will be used as a reference to our whole-body controller and $y_{des}$ comes from our planned trajectories or objectives, i.e., what we actually want to track.

Before we formulating the optimization problem, we formulate the contact and friction constraints. First, we want to enforce that the acceleration of the stance foot (or stance feet in double support) via

$$ \dot{J}_hv + J_h\dot{v} = 0 $$

Next, we build a two-dimensional friction cone as a linear inequality constraint, i.e., we use a linear representation of the friction cone to preserve convexity

$$ \begin{bmatrix} -1 & 0 & -\mu \\ 1 & 0 & -\mu \\ \end{bmatrix}\lambda_h \leq \begin{bmatrix} 0 \ 0 \end{bmatrix} $$

where $\mu$ is the friction coefficient. Let's call the matrix on the left hand side $A$ and the vector on the right hand side $b$ such that $A\lambda_h \leq b$. Given our task state space and our task space command, we formulate the following cost function

$$ J = \min_{u, \dot{v}, \lambda_h} \quad \sum_{i=0}(\ddot{y}_i-\ddot{y}_{cmd, i})^\top W_i (\ddot{y}_i-\ddot{y}_{cmd, i}) $$

where $W_i$ is the tracking cost of the $i\text{th}$ tracking objective. Due to $\ddot{y}_i$ being an affine function of $\dot{v}$, we can represent this cost as a quadratic function of our decision variable $\dot{v}$:

$$ (\ddot{y}_i-\ddot{y}_{cmd, i})^\top W_i (\ddot{y}_i-\ddot{y}_{cmd, i}) = \frac{1}{2}\dot{v}^\top Q_i \dot{v} + b_i^\top \dot{v} + c_i $$

where

$$ \begin{align*} Q_i &= 2J_i^\top W_iJ_i\\ b_i &= -2J_i^\top W_i (\ddot{y}_{cmd, i} - \dot{J}_iv) \\ c_i &= (\ddot{y}_{cmd, i} - \dot{J}_iv)^\top W_i (\ddot{y}_{cmd, i} - \dot{J}_iv) \end{align*} $$

We can finally formulate our whole-body controller optimization problem as a convex quadratic program as follows:

$$ \begin{align*} \min_{u, \dot{v}, \lambda_h} & \quad \sum_{i=0}\frac{1}{2}\dot{v}^\top Q_i \dot{v} + b_i^\top \dot{v} + c_i \\ \textrm{s.t.} & \quad M\dot{v} + Cv + G = Bu + J_h^\top \lambda_h \\ & \quad \dot{J}_hv + J_h\dot{v} = 0 \\ & \quad A\lambda_h \leq b \\ & \quad u_{min} \leq u \leq u_{max} \end{align*} $$