In [3]:
# This needs python-control: http://sourceforge.net/projects/python-control/
# Basically a matlab compability layer for python
import numpy as np
import control
g = 9.81 # gravity constant
z = 0.86 # hight of COM
T = 0.05 # lag of ZMP
A = np.matrix([[0, 1, 0], [g/z, 0, -g/z], [0, 0, -1/T]])
B = np.matrix([[0], [0], [1/T]])
C = np.matrix([[0, 0, 1]])
D = np.matrix([[0,]])
ss = control.StateSpace(A, B, C, D)
# Pole placement as suggested in the paper
control.pole(ss)
gains = control.place(A, B, [-13, 3, -np.sqrt(g/z)])
print(gains)