├── .gitignore
├── README.md
├── ZMP_preview_control.py
└── pic
    ├── Figure_1-16411423577941.png
    ├── Figure_3.png
    ├── qrcode.png
    └── 博士的沙漏-二维码-小尺寸.jpg


/.gitignore:
--------------------------------------------------------------------------------
1 | *pyc
2 | __pycache__
3 | *.ipynb_checkpoints
4 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # ZMP Preview Control
 2 | 
 3 | This is the Python implementation of ZMP Preview Control approach for biped robot control.
 4 | 
 5 | Required Python 3.6+, and use the following commands to install reqired packages:
 6 | 
 7 | ```python
 8 | pip install numpy, control, matplotlib
 9 | ```
10 | 
11 | 
12 | 
13 | 代码配备了中文博客，详见：[双足机器人ZMP预观控制算法介绍及代码实现](https://zhuanlan.zhihu.com/p/452704228?)
14 | 
15 | 
16 | 
17 | ## Basic ZMP preview control approach
18 | 
19 | Reference paper:
20 | 
21 | 1. [Kajita, Shuuji, et al. "Biped walking pattern generation by using preview control of zero-moment point" in 2003 IEEE International Conference on Robotics and Automation (Cat. No. 03CH37422). Vol. 2. IEEE, 2003.](https://ieeexplore.ieee.org/iel5/8794/27834/01241826.pdf)
22 | 2. Book：”Introduction to Humanoid Robotics” by Kajita, Shuuji. 中文译名为《仿人机器人》，可关注公众号后，在后台回复【HR】获取书籍的下载链接。
23 | 
24 | <img src="pic/qrcode.png" style="zoom: 80%;" />
25 | 
26 | 
27 | 
28 | ## Improved ZMP preview control approach
29 | 
30 | Reference paper:
31 | 
32 | 1. [Jonghoon Park, Youngil Youm, “General ZMP Preview Control for Bipedal Walking” in Proceedings 2007 IEEE International Conference on Robotics and Automation.](https://ieeexplore.ieee.org/document/4209488/authors#authors)
33 | 
34 | 
35 | 
36 | ## Experiments
37 | 
38 | ### Basic ZMP preview control
39 | 
40 | ![](pic/Figure_1-16411423577941.png)
41 | 
42 | 
43 | 
44 | ### Improved ZMP preview control
45 | 
46 | ![](pic/Figure_3.png)
47 | 


--------------------------------------------------------------------------------
/ZMP_preview_control.py:
--------------------------------------------------------------------------------
  1 | # This code is implemented by Chauby (chaubyZou@163.com), feel free to use it.
  2 | # the repository of this code is: https://github.com/chauby/ZMP_preview_control.git
  3 | 
  4 | 
  5 | #%% Initialize global variations
  6 | import numpy as np
  7 | from control.matlab import dare # for solving the discrete algebraic Riccati equation
  8 | 
  9 | # ------------------------------- Preview Control, reference: 2003-Biped Walking Pattern Generation by Using Preview Control of Zero-moment Point
 10 | def calculatePreviewControlParams(A, B, C, Q, R, N):
 11 |     [P, _, _] = dare(A, B, C.T*Q*C, R)
 12 |     K = (R + B.T*P*B).I*(B.T*P*A)
 13 | 
 14 |     f = np.zeros((1, N))
 15 |     for i in range(N):
 16 |         f[0,i] = (R+B.T*P*B).I*B.T*(((A-B*K).T)**i)*C.T*Q
 17 | 
 18 |     return K, f
 19 | 
 20 | # ------------------------------- Improved Preview Control, reference: 2007-General ZMP Preview Control for Bipedal Walking
 21 | def calculatePreviewControlParams2(A, B, C, Q, R, N):
 22 |     C_dot_A = C*A
 23 |     C_dot_B = C*B
 24 | 
 25 |     A_tilde = np.matrix([[1, C_dot_A[0,0], C_dot_A[0,1], C_dot_A[0,2]],
 26 |                             [0, A[0,0], A[0,1], A[0,2]],
 27 |                             [0, A[1,0], A[1,1], A[1,2]],
 28 |                             [0, A[2,0], A[2,1], A[2,2]]])
 29 |     B_tilde = np.matrix([[C_dot_B[0,0]],
 30 |                             [B[0,0]],
 31 |                             [B[1,0]],
 32 |                             [B[2,0]]])
 33 |     C_tilde = np.matrix([[1, 0, 0, 0]])
 34 | 
 35 |     [P_tilde, _, _] = dare(A_tilde, B_tilde, C_tilde.T*Q*C_tilde, R)
 36 |     K_tilde = (R + B_tilde.T*P_tilde*B_tilde).I*(B_tilde.T*P_tilde*A_tilde)
 37 | 
 38 |     Ks = K_tilde[0, 0]
 39 |     Kx = K_tilde[0, 1:]
 40 | 
 41 |     Ac_tilde = A_tilde - B_tilde*K_tilde
 42 | 
 43 |     G = np.zeros((1, N))
 44 | 
 45 |     G[0] = -Ks
 46 |     I_tilde = np.matrix([[1],[0],[0],[0]])
 47 |     X_tilde = -Ac_tilde.T*P_tilde*I_tilde
 48 | 
 49 |     for i in range(N):
 50 |         G[0,i] = (R + B_tilde.T*P_tilde*B_tilde).I*(B_tilde.T)*X_tilde
 51 |         X_tilde = Ac_tilde.T*X_tilde
 52 | 
 53 |     return Ks, Kx, G
 54 | 
 55 | 
 56 | 
 57 | # %% demo
 58 | if __name__ == '__main__':
 59 |     step_pos = np.array([[0, 0.0],
 60 |                         [0.2, 0.06],
 61 |                         [0.4, -0.06],
 62 |                         [0.6, 0.09],
 63 |                         [0.8, -0.03],
 64 |                         [1.3, 0.09],
 65 |                         [1.7, -0.03],
 66 |                         [1.9, 0.09],
 67 |                         [2.0, -0.03]])
 68 |     z_c = 0.5
 69 |     g = 9.81
 70 |     dt = 0.01
 71 |     t_step = 0.7 # timing for one step
 72 |     t_preview = 1.0 # timing for preview
 73 |     n_step = len(step_pos)
 74 |     t_simulation = n_step*t_step - t_preview - dt # timing for simulation
 75 | 
 76 |     N_preview = int(t_preview/dt) # preview length
 77 |     N_simulation = int(t_simulation/dt)
 78 | 
 79 |     # Generate ZMP trajectory
 80 |     ZMP_x_ref = []
 81 |     ZMP_y_ref = []
 82 |     i = 0
 83 |     for t in  np.arange(0, n_step*t_step, dt):
 84 |         ZMP_x_ref.append(step_pos[i,0])
 85 |         ZMP_y_ref.append(step_pos[i,1])
 86 | 
 87 |         if (t != 0) and (t%t_step < 1e-6):
 88 |             i += 1
 89 | 
 90 | 
 91 |     # Define basic matrix
 92 |     A = np.mat(([1, dt, dt**2/2],
 93 |                 [0, 1, dt],
 94 |                 [0, 0, 1]))
 95 |     B = np.mat((dt**3/6, dt**2/2, dt)).T
 96 |     C = np.mat((1, 0, -z_c/g))
 97 | 
 98 |     Q = 1
 99 |     R = 1e-6
100 | 
101 |     # Calculate Preview control parameters
102 |     K, f = calculatePreviewControlParams(A, B, C, Q, R, N_preview)
103 | 
104 |     # Calculate Improved Preview control parameters
105 |     Ks, Kx, G = calculatePreviewControlParams2(A, B, C, Q, R, N_preview)
106 | 
107 | 
108 |     # ------------------------------- for Preview Control
109 |     ux_1 = np.asmatrix(np.zeros((N_simulation, 1)))
110 |     uy_1 = np.asmatrix(np.zeros((N_simulation, 1)))
111 |     COM_x_1 = np.asmatrix(np.zeros((3, N_simulation+1)))
112 |     COM_y_1 = np.asmatrix(np.zeros((3, N_simulation+1)))
113 | 
114 |     # record data for plot
115 |     COM_x_record_1 = []
116 |     COM_y_record_1 = []
117 |     ZMP_x_record_1 = []
118 |     ZMP_y_record_1 = []
119 | 
120 |     # ------------------------------- for Improved Preview Control 1
121 |     ux_2 = np.asmatrix(np.zeros((N_simulation, 1)))
122 |     uy_2 = np.asmatrix(np.zeros((N_simulation, 1)))
123 |     COM_x_2 = np.asmatrix(np.zeros((3, N_simulation+1)))
124 |     COM_y_2 = np.asmatrix(np.zeros((3, N_simulation+1)))
125 | 
126 |     # record data for plot
127 |     COM_x_record_2 = []
128 |     COM_y_record_2 = []
129 |     ZMP_x_record_2 = []
130 |     ZMP_y_record_2 = []
131 | 
132 |     e_x_2 = np.zeros((N_simulation, 1))
133 |     e_y_2 = np.zeros((N_simulation, 1))
134 | 
135 | 
136 |     # ------------------------------- for Improved Preview Control 2
137 |     ux_3 = np.asmatrix(np.zeros((N_simulation, 1)))
138 |     uy_3 = np.asmatrix(np.zeros((N_simulation, 1)))
139 |     COM_x_3 = np.asmatrix(np.zeros((3, N_simulation+1)))
140 |     COM_y_3 = np.asmatrix(np.zeros((3, N_simulation+1)))
141 | 
142 |     # record data for plot
143 |     COM_x_record_3 = []
144 |     COM_y_record_3 = []
145 |     ZMP_x_record_3 = []
146 |     ZMP_y_record_3 = []
147 | 
148 |     e_x_3 = np.zeros((N_simulation, 1))
149 |     e_y_3 = np.zeros((N_simulation, 1))
150 | 
151 |     sum_e_x = 0
152 |     sum_e_y = 0
153 | 
154 | 
155 |     # main loop
156 |     for k in range(N_simulation):
157 |         ZMP_x_preview = np.asmatrix(ZMP_x_ref[k:k+N_preview]).T
158 |         ZMP_y_preview = np.asmatrix(ZMP_y_ref[k:k+N_preview]).T
159 | 
160 |         # ------------------------------- 1: Preview Control
161 |         # update ZMP
162 |         ZMP_x = C*COM_x_1[:,k]
163 |         ZMP_y = C*COM_y_1[:,k]
164 |         ZMP_x_record_1.append(ZMP_x[0,0])
165 |         ZMP_y_record_1.append(ZMP_y[0,0])
166 | 
167 |         # update u
168 |         ux_1[k] = -K*COM_x_1[:, k] + f*ZMP_x_preview
169 |         uy_1[k] = -K*COM_y_1[:, k] + f*ZMP_y_preview
170 | 
171 |         # update COM state
172 |         COM_x_1[:,k+1] = A*COM_x_1[:, k] + B*ux_1[k]
173 |         COM_y_1[:,k+1] = A*COM_y_1[:, k] + B*uy_1[k]
174 |         COM_x_record_1.append(COM_x_1[0,k])
175 |         COM_y_record_1.append(COM_y_1[0,k])
176 | 
177 | 
178 |         # -------------------------------- 2: Improved Preview Control with only current error
179 |         # update ZMP
180 |         ZMP_x = C*COM_x_2[:,k]
181 |         ZMP_y = C*COM_y_2[:,k]
182 |         ZMP_x_record_2.append(ZMP_x[0,0])
183 |         ZMP_y_record_2.append(ZMP_y[0,0])
184 | 
185 |         # calculate errors
186 |         e_x_2[k] = ZMP_x_ref[k] - ZMP_x
187 |         e_y_2[k] = ZMP_y_ref[k] - ZMP_y
188 | 
189 |         # update u
190 |         ux_2[k] = -Ks*e_x_2[k] - Kx*COM_x_2[:, k] - G*ZMP_x_preview
191 |         uy_2[k] = -Ks*e_y_2[k] - Kx*COM_y_2[:, k] - G*ZMP_y_preview
192 | 
193 |         # update COM state
194 |         COM_x_2[:,k+1] = A*COM_x_2[:, k] + B*ux_2[k]
195 |         COM_y_2[:,k+1] = A*COM_y_2[:, k] + B*uy_2[k]
196 |         COM_x_record_2.append(COM_x_2[0,k])
197 |         COM_y_record_2.append(COM_y_2[0,k])
198 | 
199 |         # --------------------------------- 3: Improved Preview Control with the summary of history errors
200 |         # update ZMP
201 |         ZMP_x = C*COM_x_3[:,k]
202 |         ZMP_y = C*COM_y_3[:,k]
203 |         ZMP_x_record_3.append(ZMP_x[0,0])
204 |         ZMP_y_record_3.append(ZMP_y[0,0])
205 | 
206 |         # calculate errors
207 |         e_x_3[k] = ZMP_x - ZMP_x_ref[k] 
208 |         e_y_3[k] = ZMP_y - ZMP_y_ref[k]
209 |         sum_e_x += e_x_3[k]
210 |         sum_e_y += e_y_3[k]
211 | 
212 |         # update u
213 |         ux_3[k] = -Ks*sum_e_x - Kx*COM_x_3[:, k] - G*ZMP_x_preview
214 |         uy_3[k] = -Ks*sum_e_y - Kx*COM_y_3[:, k] - G*ZMP_y_preview
215 | 
216 |         # update COM state
217 |         COM_x_3[:,k+1] = A*COM_x_3[:, k] + B*ux_3[k]
218 |         COM_y_3[:,k+1] = A*COM_y_3[:, k] + B*uy_3[k]
219 |         COM_x_record_3.append(COM_x_3[0,k])
220 |         COM_y_record_3.append(COM_y_3[0,k])
221 | 
222 | 
223 |     # plot
224 |     import matplotlib.pyplot as plt
225 |     plt.figure()
226 |     plt.title("Preview Control")
227 |     plt.subplot(3,1,1)
228 |     plt.plot(ZMP_x_ref, 'g--', label='ZMP_x_ref')
229 |     plt.plot(ZMP_x_record_1, 'b', label='ZMP_x')
230 |     plt.plot(COM_x_record_1, 'r--', label='COM_x')
231 |     plt.legend()
232 |     plt.subplot(3,1,2)
233 |     plt.plot(ZMP_y_ref, 'g--', label='ZMP_y_ref')
234 |     plt.plot(ZMP_y_record_1, 'b', label='ZMP_y')
235 |     plt.plot(COM_y_record_1, 'r--', label='COM_y')
236 |     plt.legend()
237 |     plt.subplot(3,1,3)
238 |     plt.plot(ZMP_x_ref, ZMP_y_ref, 'g--', label='ZMP_ref')
239 |     plt.plot(ZMP_x_record_1, ZMP_y_record_1, 'b', label='ZMP')
240 |     plt.plot(COM_x_record_1, COM_y_record_1, 'r--', label='COM')
241 |     plt.legend()
242 | 
243 |     plt.figure()
244 |     plt.title("Preview Control 2")
245 |     plt.subplot(3,1,1)
246 |     plt.plot(ZMP_x_ref, 'g--', label='ZMP_x_ref')
247 |     plt.plot(ZMP_x_record_2, 'b', label='ZMP_x')
248 |     plt.plot(COM_x_record_2, 'r--', label='COM_x')
249 |     plt.legend()
250 |     plt.subplot(3,1,2)
251 |     plt.plot(ZMP_y_ref, 'g--', label='ZMP_y_ref')
252 |     plt.plot(ZMP_y_record_2, 'b', label='ZMP_y')
253 |     plt.plot(COM_y_record_2, 'r--', label='COM_y')
254 |     plt.legend()
255 |     plt.subplot(3,1,3)
256 |     plt.plot(ZMP_x_ref, ZMP_y_ref, 'g--', label='ZMP_ref')
257 |     plt.plot(ZMP_x_record_2, ZMP_y_record_2, 'b', label='ZMP')
258 |     plt.plot(COM_x_record_2, COM_y_record_2, 'r--', label='COM')
259 |     plt.legend()
260 | 
261 |     plt.figure()
262 |     plt.title("Preview Control 3")
263 |     plt.subplot(3,1,1)
264 |     plt.plot(ZMP_x_ref, 'g--', label='ZMP_x_ref')
265 |     plt.plot(ZMP_x_record_3, 'b', label='ZMP_x')
266 |     plt.plot(COM_x_record_3, 'r--', label='COM_x')
267 |     plt.legend()
268 |     plt.subplot(3,1,2)
269 |     plt.plot(ZMP_y_ref, 'g--', label='ZMP_y_ref')
270 |     plt.plot(ZMP_y_record_3, 'b', label='ZMP_y')
271 |     plt.plot(COM_y_record_3, 'r--', label='COM_y')
272 |     plt.legend()
273 |     plt.subplot(3,1,3)
274 |     plt.plot(ZMP_x_ref, ZMP_y_ref, 'g--', label='ZMP_ref')
275 |     plt.plot(ZMP_x_record_3, ZMP_y_record_3, 'b', label='ZMP')
276 |     plt.plot(COM_x_record_3, COM_y_record_3, 'r--', label='COM')
277 |     plt.legend()
278 | 
279 |     plt.show()
280 | 
281 | # %%
282 | 


--------------------------------------------------------------------------------
/pic/Figure_1-16411423577941.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/chauby/ZMP_preview_control/a8d99e7f8fac4da3a3301b4e0094ac41f3107b3d/pic/Figure_1-16411423577941.png


--------------------------------------------------------------------------------
/pic/Figure_3.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/chauby/ZMP_preview_control/a8d99e7f8fac4da3a3301b4e0094ac41f3107b3d/pic/Figure_3.png


--------------------------------------------------------------------------------
/pic/qrcode.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/chauby/ZMP_preview_control/a8d99e7f8fac4da3a3301b4e0094ac41f3107b3d/pic/qrcode.png


--------------------------------------------------------------------------------
/pic/博士的沙漏-二维码-小尺寸.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/chauby/ZMP_preview_control/a8d99e7f8fac4da3a3301b4e0094ac41f3107b3d/pic/博士的沙漏-二维码-小尺寸.jpg


--------------------------------------------------------------------------------