five-link-walker

Stable locomotion using IO linearization, CLFs, and TSC. This was initially made to get a better understanding of bipedal robotic locomotion and get familiar with some of the 'standard' basic control approaches to 5-link walkers. These 5-link walkers are close to being the easiest, morphologically sound walker. For a detailed description of the biped in question, please refer to [1] which is well documented

The 5-link walker code ('main.m') allows for setting a down-step height to analyze some heuristic version of the robustness of the control strategies, affecting by controller gains and other decision variables. Nothing too formal though but fun to play around with!

Usage:

Just run 'main.m'. You can change the controller type before the main loop by changing the string 'controller' on line 100. If you wish to make changes to the underlying biped model, make the changes in 'generate.m' and run it again (this will take a long time with symbolic equations and simplyfing them!).

For the CLF and TSC controller, you will need CVX.

System of equations

We consider a standard system of equations.

$\dot{x} = f(x) + g(x)u$

where

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

Input-output linearization

According to [2]

$h$ is defined as the difference between the actual outputs (a linear mapping of the state, identity in this instance) and the desired outputs, obtained from the offline trajectory optimization using HZD.

Lie derivative of the outputs.

$L_fh = (H_0 - \frac{dh}{dt}c)\dot{q}$

Second Lie derivative with respect to the controlled PDE

$L_gL_fh = (H_0 - \frac{dh}{dt}c)D^{-1}B$

Second Lie derivative with respect to the autonomous PDE

$L_fL_fh = -\frac{d^2h}{dt^2}(c q)^2 + (H_0 - \frac{dh}{dt}c)D^{-1}F_{vec}$

Auxilliary output

$v = -(1/\epsilon)K_D L_fh - (1/\epsilon^2)K_Ph$

Mapping to the actual output

$u = L_gL_fh^{-1}(v - L_fL_fh)$

RES-CLFs

According to [3]. Defined as a Quadratic Program (QP). Requires the CVX MATLAB optimization package for solving! However, creating something with MATLAB's 'quadprog' should be straightforward enough, given the following equations. 'icare' is the solution of the algebraic continuous time Ricatti equations.

$\eta = \begin{bmatrix} h \\ L_fh \end{bmatrix}\\ F = \begin{bmatrix} 0 & I \\ 0 & 0\end{bmatrix}\\ G = \begin{bmatrix} 0 \\ I \end{bmatrix}\\ I_{\epsilon} = diag(\frac{1}{\epsilon}[M\times1],1[M\times1])\\ Q = I[2M]\\ P = icare(F,G,Q)\\ P_{\epsilon} = I_{\epsilon}PI_{\epsilon}\\ V = \eta^TP_{\epsilon}\eta\\ L_{FV} = \eta^T (F^T P_{\epsilon} + P_{\epsilon}F)\eta \\ L_{GV} = 2\eta^TP_{\epsilon}G\\\\ minimize (L_gL_fhu + L_fL_fh)^T(L_gL_fhu + L_fL_fh) + \delta^2\\ s.t. (L_{FV} + L{GV}(L_gL_fhu + L_fL_fh)) \leq -\frac{1}{\epsilon} \frac{min(eig(Q))}{max(eig(P))}V + \delta$

TSC

According to [4]. Defined as a Quadratic Program (QP). Requires the CVX MATLAB optimization package for solving! However, creating something with MATLAB's 'quadprog' should be straightforward enough, given the following equations:

$Y = h_{actual} - h_d \\ \dot{Y} = J_y\dot{q} - \frac{dh}{dt} \\ \ddot{Y} = J_y(A_qu + b_q) + \dot{J}_y\dot{q} - \frac{d^2h}{dt^2} \\ \ddot{Y}^* = -K_P Y - K_D \dot{Y} \\\\ minimize (\ddot{Y} - \ddot{Y}^*)^T(\ddot{Y} - \ddot{Y}^*)$

Sources

1: @book{westervelt2018feedback, title={Feedback control of dynamic bipedal robot locomotion}, author={Westervelt, Eric R and Grizzle, Jessy W and Chevallereau, Christine and Choi, Jun Ho and Morris, Benjamin}, year={2018}, publisher={CRC press} }

2: @article{westervelt2003hybrid, title={Hybrid zero dynamics of planar biped walkers}, author={Westervelt, Eric R and Grizzle, Jessy W and Koditschek, Daniel E}, journal={IEEE transactions on automatic control}, volume={48}, number={1}, pages={42--56}, year={2003}, publisher={IEEE} }

3: @article{ames2014rapidly, title={Rapidly exponentially stabilizing control lyapunov functions and hybrid zero dynamics}, author={Ames, Aaron D and Galloway, Kevin and Sreenath, Koushil and Grizzle, Jessy W}, journal={IEEE Transactions on Automatic Control}, volume={59}, number={4}, pages={876--891}, year={2014}, publisher={IEEE} }

4: @inproceedings{wensing2013generation, title={Generation of dynamic humanoid behaviors through task-space control with conic optimization}, author={Wensing, Patrick M and Orin, David E}, booktitle={2013 IEEE International Conference on Robotics and Automation}, pages={3103--3109}, year={2013}, organization={IEEE} }