Notebook

This notebook describes the details of forming the coefficient matrix and ordinate vector from walking data for a linear least squares solution to the optimal gains from a simple full state feedback controller employed by the human while walking.

In [1]:

!pip freeze

Cython==0.19.1
-e git+git@github.com:moorepants/DynamicistToolKit.git@83822a2634756ad8a0a1701a0cb3cca3c07877d4#egg=DynamicistToolKit-dev
Jinja2==2.7.1
MarkupSafe==0.18
argparse==1.2.1
coverage==3.6
ipython==1.0.0
matplotlib==1.3.0
nose==1.3.0
numpy==1.7.1
pandas==0.12.0
pyparsing==2.0.1
python-dateutil==2.1
pytz==2013b
pyzmq==13.1.0
scipy==0.12.0
six==1.3.0
sympy==0.7.3
tornado==3.1
wsgiref==0.1.2

In [2]:

from sympy import init_printing
init_printing()

In [3]:

from sympy import symbols, Matrix, Symbol, zeros, eye

In [4]:

n = 3 # number of time samples in each cycle (step)
m = 2 # number of cycles (steps)
p = 2 # number of sensors
q = 2 # number of controls

First form the equation that generates the controls given the error in the sensors at a single time step during a single foot step.

$m_m(t) = m(t)_{lc} + K(t) s_e(t) = m(t)_{lc} + K(t) (s_{lc}(t) - s_m(t))$

Now rearrange the equations such that we have one linear in both the gains, $K(t)$ , and in $m^*(t)$ :

$m_m(t) = m(t)_{lc} + K(t) s_{lc}(t) - K(t) s_m(t) = m^*(t) - K(t) s_m(t)$

The "measured" controls, $m_m(t)$ :

In [5]:

mm = Matrix(q, 1, lambda i, j: Symbol('m_m_q={}'.format(i)))
mm

Out[5]:

$\left[\begin{smallmatrix}{}m_{m q=0}\\m_{m q=1}\end{smallmatrix}\right]$

The unknown gains, $K$ :

In [6]:

K = Matrix(q, p, lambda i, j: Symbol('k_q={},p={}'.format(i, j)))
K

Out[6]:

$\left[\begin{smallmatrix}{}k_{q=0,p=0} & k_{q=0,p=1}\\k_{q=1,p=0} & k_{q=1,p=1}\end{smallmatrix}\right]$

The unknown "limit cycle" controls, $m^*(t)$ :

In [7]:

ms = Matrix(q, 1, lambda i, j: Symbol('m^*_q={}'.format(i)))
ms

Out[7]:

$\left[\begin{smallmatrix}{}m^{*}_{q=0}\\m^{*}_{q=1}\end{smallmatrix}\right]$

The measured sensors, $s_m(t)$ :

In [8]:

sm = Matrix(p, 1, lambda i, j: Symbol('s_m_p={}'.format(i)))
sm

Out[8]:

$\left[\begin{smallmatrix}{}s_{m p=0}\\s_{m p=1}\end{smallmatrix}\right]$

And the equation:

$0 = m(t) - m^*(t) + K(t) s(t)$

In [9]:

zero = mm - ms + K * sm
zero

Out[9]:

$\left[\begin{smallmatrix}{}k_{q=0,p=0} s_{m p=0} + k_{q=0,p=1} s_{m p=1} - m^{*}_{q=0} + m_{m q=0}\\k_{q=1,p=0} s_{m p=0} + k_{q=1,p=1} s_{m p=1} - m^{*}_{q=1} + m_{m q=1}\end{smallmatrix}\right]$

Now for a simple linear least squares the equations can be massage into this form:

$Ax=b$

which gives and over determined system of linear equations.

We define $x$ as:

In [10]:

x = K.reshape(q * p, 1).col_join(ms)
x

Out[10]:

$\left[\begin{smallmatrix}{}k_{q=0,p=0}\\k_{q=0,p=1}\\k_{q=1,p=0}\\k_{q=1,p=1}\\m^{*}_{q=0}\\m^{*}_{q=1}\end{smallmatrix}\right]$

To form the A matrix for a single time step in a single foot step the equation can be rearranged:

In [11]:

A = zeros(q, q * p)
for row in range(q):
    A[row, row * p:(row + 1) * p] =  sm.T
A = A.row_join(-eye(q))
A

Out[11]:

$\left[\begin{smallmatrix}{}s_{m p=0} & s_{m p=1} & 0 & 0 & -1 & 0\\0 & 0 & s_{m p=0} & s_{m p=1} & 0 & -1\end{smallmatrix}\right]$

In [12]:

b = -mm
b

Out[12]:

$\left[\begin{smallmatrix}{}- m_{m q=0}\\- m_{m q=1}\end{smallmatrix}\right]$

Now we can check to make sure that our new form gives the same answer as the previous form:

In [13]:

zero2 = A * x - b 
zero2

Out[13]:

$\left[\begin{smallmatrix}{}k_{q=0,p=0} s_{m p=0} + k_{q=0,p=1} s_{m p=1} - m^{*}_{q=0} + m_{m q=0}\\k_{q=1,p=0} s_{m p=0} + k_{q=1,p=1} s_{m p=1} - m^{*}_{q=1} + m_{m q=1}\end{smallmatrix}\right]$

In [14]:

assert zero == zero2

Now for n timesteps we want to form the A matrix such that there are different gains for each time step.

In [15]:

gain_matrices = []
control_vectors = []
control_star_vectors = []
sensor_vectors = []
for time_step in range(n):
    gain_matrices.append(Matrix(q, p, lambda i, j: Symbol('k^{}_q={},p={}'.format(time_step, i, j))))
    control_vectors.append(Matrix(q, 1, lambda i, j: Symbol('m^{}_m_q={}'.format(time_step, i))))
    control_star_vectors.append(Matrix(q, 1, lambda i, j: Symbol('m^*{}_q={}'.format(time_step, i))))
    sensor_vectors.append(Matrix(p, 1, lambda i, j: Symbol('s^{}_m_p={}'.format(time_step, i))))

In [16]:

gain_matrices

Out[16]:

$\begin{bmatrix}\left[\begin{smallmatrix}{}k^{0}_{q=0,p=0} & k^{0}_{q=0,p=1}\\k^{0}_{q=1,p=0} & k^{0}_{q=1,p=1}\end{smallmatrix}\right], & \left[\begin{smallmatrix}{}k^{1}_{q=0,p=0} & k^{1}_{q=0,p=1}\\k^{1}_{q=1,p=0} & k^{1}_{q=1,p=1}\end{smallmatrix}\right], & \left[\begin{smallmatrix}{}k^{2}_{q=0,p=0} & k^{2}_{q=0,p=1}\\k^{2}_{q=1,p=0} & k^{2}_{q=1,p=1}\end{smallmatrix}\right]\end{bmatrix}$

In [17]:

control_vectors

Out[17]:

$\begin{bmatrix}\left[\begin{smallmatrix}{}m^{0}_{m q=0}\\m^{0}_{m q=1}\end{smallmatrix}\right], & \left[\begin{smallmatrix}{}m^{1}_{m q=0}\\m^{1}_{m q=1}\end{smallmatrix}\right], & \left[\begin{smallmatrix}{}m^{2}_{m q=0}\\m^{2}_{m q=1}\end{smallmatrix}\right]\end{bmatrix}$

In [18]:

control_star_vectors

Out[18]:

$\begin{bmatrix}\left[\begin{smallmatrix}{}m^{*0}_{q=0}\\m^{*0}_{q=1}\end{smallmatrix}\right], & \left[\begin{smallmatrix}{}m^{*1}_{q=0}\\m^{*1}_{q=1}\end{smallmatrix}\right], & \left[\begin{smallmatrix}{}m^{*2}_{q=0}\\m^{*2}_{q=1}\end{smallmatrix}\right]\end{bmatrix}$

In [19]:

sensor_vectors

Out[19]:

$\begin{bmatrix}\left[\begin{smallmatrix}{}s^{0}_{m p=0}\\s^{0}_{m p=1}\end{smallmatrix}\right], & \left[\begin{smallmatrix}{}s^{1}_{m p=0}\\s^{1}_{m p=1}\end{smallmatrix}\right], & \left[\begin{smallmatrix}{}s^{2}_{m p=0}\\s^{2}_{m p=1}\end{smallmatrix}\right]\end{bmatrix}$

Picking off a trio we should get our familiar equation back:

In [20]:

zero = control_vectors[0] - control_star_vectors[0] + gain_matrices[0] * sensor_vectors[0]
zero

Out[20]:

The larger A matrix for all of the time steps can be formed by stacking the A matrices from a single time step diagonally:

In [21]:

Am = zeros(n * q, n * (q + q * p))
for time_step in range(n):
    An = zeros(q, q * p)
    for row in range(q):
        An[row, row * p:(row + 1) * p] =  sensor_vectors[time_step].T
    An = An.row_join(-eye(q))
    num_rows, num_cols = An.shape
    Am[time_step * num_rows:(time_step + 1) * num_rows, time_step * num_cols:(time_step + 1) * num_cols] = An

In [22]:

Am

Out[22]:

$\left[\begin{smallmatrix}{}s^{0}_{m p=0} & s^{0}_{m p=1} & 0 & 0 & -1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & s^{0}_{m p=0} & s^{0}_{m p=1} & 0 & -1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & s^{1}_{m p=0} & s^{1}_{m p=1} & 0 & 0 & -1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & s^{1}_{m p=0} & s^{1}_{m p=1} & 0 & -1 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & s^{2}_{m p=0} & s^{2}_{m p=1} & 0 & 0 & -1 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & s^{2}_{m p=0} & s^{2}_{m p=1} & 0 & -1\end{smallmatrix}\right]$

The control vector and the gain vector are simple column stacks:

In [23]:

bm = -control_vectors[0]
for control in control_vectors[1:]:
    bm = bm.col_join(-control)
bm

Out[23]:

$\left[\begin{smallmatrix}{}- m^{0}_{m q=0}\\- m^{0}_{m q=1}\\- m^{1}_{m q=0}\\- m^{1}_{m q=1}\\- m^{2}_{m q=0}\\- m^{2}_{m q=1}\end{smallmatrix}\right]$

In [24]:

xm = gain_matrices[0].reshape(q * p, 1).col_join(control_star_vectors[0])
for gain, control_star in zip(gain_matrices[1:], control_star_vectors[1:]):
    xm = xm.col_join(gain.reshape(q * p, 1)).col_join(control_star)
xm

Out[24]:

$\left[\begin{smallmatrix}{}k^{0}_{q=0,p=0}\\k^{0}_{q=0,p=1}\\k^{0}_{q=1,p=0}\\k^{0}_{q=1,p=1}\\m^{*0}_{q=0}\\m^{*0}_{q=1}\\k^{1}_{q=0,p=0}\\k^{1}_{q=0,p=1}\\k^{1}_{q=1,p=0}\\k^{1}_{q=1,p=1}\\m^{*1}_{q=0}\\m^{*1}_{q=1}\\k^{2}_{q=0,p=0}\\k^{2}_{q=0,p=1}\\k^{2}_{q=1,p=0}\\k^{2}_{q=1,p=1}\\m^{*2}_{q=0}\\m^{*2}_{q=1}\end{smallmatrix}\right]$

Now the original equations can be regenerated with:

In [25]:

Am * xm - bm

Out[25]:

$\left[\begin{smallmatrix}{}k^{0}_{q=0,p=0} s^{0}_{m p=0} + k^{0}_{q=0,p=1} s^{0}_{m p=1} - m^{*0}_{q=0} + m^{0}_{m q=0}\\k^{0}_{q=1,p=0} s^{0}_{m p=0} + k^{0}_{q=1,p=1} s^{0}_{m p=1} - m^{*0}_{q=1} + m^{0}_{m q=1}\\k^{1}_{q=0,p=0} s^{1}_{m p=0} + k^{1}_{q=0,p=1} s^{1}_{m p=1} - m^{*1}_{q=0} + m^{1}_{m q=0}\\k^{1}_{q=1,p=0} s^{1}_{m p=0} + k^{1}_{q=1,p=1} s^{1}_{m p=1} - m^{*1}_{q=1} + m^{1}_{m q=1}\\k^{2}_{q=0,p=0} s^{2}_{m p=0} + k^{2}_{q=0,p=1} s^{2}_{m p=1} - m^{*2}_{q=0} + m^{2}_{m q=0}\\k^{2}_{q=1,p=0} s^{2}_{m p=0} + k^{2}_{q=1,p=1} s^{2}_{m p=1} - m^{*2}_{q=1} + m^{2}_{m q=1}\end{smallmatrix}\right]$

We have $n(p q + q)$ unknowns and $nq$ equations so we need at least $1 + p$ cycles to solve for the unknowns.

In [26]:

n * (p*q + q)

Out[26]:

$18$

In [27]:

1 + p

Out[27]:

$3$

In [28]:

Am.shape, bm.shape

Out[28]:

$\begin{pmatrix}\begin{pmatrix}6, & 18\end{pmatrix}, & \begin{pmatrix}6, & 1\end{pmatrix}\end{pmatrix}$

In [29]:

b = bm
A = Am
for step in range(p):
    b = b.col_join(bm)
    A = A.col_join(Am)

In [30]:

A.shape, b.shape

Out[30]:

$\begin{pmatrix}\begin{pmatrix}18, & 18\end{pmatrix}, & \begin{pmatrix}18, & 1\end{pmatrix}\end{pmatrix}$

The following should compute the least squares solution but since I didn't actually create new equations for the steps, then you get NaN as a solution.

In [31]:

x = A.LDLsolve(b)
x

Out[31]:

$\left[\begin{smallmatrix}{}\bot\\\bot\\\bot\\\bot\\\bot\\\bot\\\bot\\\bot\\\bot\\\bot\\\bot\\\bot\\\bot\\\bot\\\bot\\\bot\\\bot\\\bot\end{smallmatrix}\right]$

In [32]:

type(x[0])

Out[32]:

sympy.core.numbers.NaN