ex_4_walking.py
Go to the documentation of this file.
1 import time
2 
3 import ex_4_conf as conf
4 import matplotlib.pyplot as plt
5 import numpy as np
6 import plot_utils as plut
7 import tsid
8 from numpy import nan
9 from numpy.linalg import norm as norm
10 from tsid_biped import TsidBiped
11 
12 print("".center(conf.LINE_WIDTH, "#"))
13 print(" Test Walking ".center(conf.LINE_WIDTH, "#"))
14 print("".center(conf.LINE_WIDTH, "#"), "\n")
15 
16 
17 USE_EIQUADPROG = 1
18 USE_PROXQP = 0
19 USE_OSQP = 0
20 VERBOSE = 0
21 
22 PLOT_COM = 1
23 PLOT_COP = 1
24 PLOT_FOOT_TRAJ = 0
25 PLOT_TORQUES = 0
26 PLOT_JOINT_VEL = 0
27 
28 try:
29  data = np.load(conf.DATA_FILE_TSID)
30 except FileNotFoundError as e:
31  print(
32  "To run this walking example, you need to first execute ex_4_plan_LIPM_romeo.py and ex_4_LIPM_to_TSID."
33  )
34  raise e
35 
36 tsid_biped = TsidBiped(conf, conf.viewer)
37 
38 # overwrite the default solver
39 if USE_EIQUADPROG:
40  print("Using eiquadprog")
41  tsid_biped.solver = tsid.SolverHQuadProgFast("qp solver")
42 
43 elif USE_PROXQP:
44  print("Using proxqp")
45  tsid_biped.solver = tsid.SolverProxQP("qp solver")
46  tsid_biped.solver.set_epsilon_absolute(1e-5)
47  tsid_biped.solver.set_maximum_iterations(int(1e6))
48  tsid_biped.solver.set_verbose(VERBOSE)
49 
50 elif USE_OSQP:
51  print("Using osqp")
52  tsid_biped.solver = tsid.SolverOSQP("qp solver")
53  tsid_biped.solver.set_epsilon_absolute(1e-8)
54  tsid_biped.solver.set_maximum_iterations(int(1e6))
55  tsid_biped.solver.set_verbose(VERBOSE)
56 
57 else:
58  print("Please specify which solver to use.")
59  exit()
60 
61 tsid_biped.solver.resize(
62  tsid_biped.formulation.nVar, tsid_biped.formulation.nEq, tsid_biped.formulation.nIn
63 )
64 
65 N = data["com"].shape[1]
66 N_pre = int(conf.T_pre / conf.dt)
67 N_post = int(conf.T_post / conf.dt)
68 
69 com_pos = np.empty((3, N + N_post)) * nan
70 com_vel = np.empty((3, N + N_post)) * nan
71 com_acc = np.empty((3, N + N_post)) * nan
72 x_LF = np.empty((3, N + N_post)) * nan
73 dx_LF = np.empty((3, N + N_post)) * nan
74 ddx_LF = np.empty((3, N + N_post)) * nan
75 ddx_LF_des = np.empty((3, N + N_post)) * nan
76 x_RF = np.empty((3, N + N_post)) * nan
77 dx_RF = np.empty((3, N + N_post)) * nan
78 ddx_RF = np.empty((3, N + N_post)) * nan
79 ddx_RF_des = np.empty((3, N + N_post)) * nan
80 f_RF = np.zeros((6, N + N_post))
81 f_LF = np.zeros((6, N + N_post))
82 cop_RF = np.zeros((2, N + N_post))
83 cop_LF = np.zeros((2, N + N_post))
84 tau = np.zeros((tsid_biped.robot.na, N + N_post))
85 q_log = np.zeros((tsid_biped.robot.nq, N + N_post))
86 v_log = np.zeros((tsid_biped.robot.nv, N + N_post))
87 
88 contact_phase = data["contact_phase"]
89 com_pos_ref = np.asarray(data["com"])
90 com_vel_ref = np.asarray(data["dcom"])
91 com_acc_ref = np.asarray(data["ddcom"])
92 x_RF_ref = np.asarray(data["x_RF"])
93 dx_RF_ref = np.asarray(data["dx_RF"])
94 ddx_RF_ref = np.asarray(data["ddx_RF"])
95 x_LF_ref = np.asarray(data["x_LF"])
96 dx_LF_ref = np.asarray(data["dx_LF"])
97 ddx_LF_ref = np.asarray(data["ddx_LF"])
98 cop_ref = np.asarray(data["cop"])
99 com_acc_des = (
100  np.empty((3, N + N_post)) * nan
101 ) # acc_des = acc_ref - Kp*pos_err - Kd*vel_err
102 
103 x_rf = tsid_biped.get_placement_RF().translation
104 offset = x_rf - x_RF_ref[:, 0]
105 for i in range(N):
106  com_pos_ref[:, i] += offset + np.array([0.0, 0.0, 0.0])
107  x_RF_ref[:, i] += offset
108  x_LF_ref[:, i] += offset
109 
110 t = -conf.T_pre
111 q, v = tsid_biped.q, tsid_biped.v
112 
113 qp_data_list = []
114 c = 0
115 q_list = []
116 
117 input("Press enter to start")
118 for i in range(-N_pre, N + N_post):
119  time_start = time.time()
120 
121  if i == 0:
122  print("Starting to walk (remove contact left foot)")
123  tsid_biped.remove_contact_LF()
124  elif i > 0 and i < N - 1:
125  if contact_phase[i] != contact_phase[i - 1]:
126  print(
127  "Time %.3f Changing contact phase from %s to %s"
128  % (t, contact_phase[i - 1], contact_phase[i])
129  )
130  if contact_phase[i] == "left":
131  tsid_biped.add_contact_LF()
132  tsid_biped.remove_contact_RF()
133  else:
134  tsid_biped.add_contact_RF()
135  tsid_biped.remove_contact_LF()
136 
137  if i < 0:
138  tsid_biped.set_com_ref(
139  com_pos_ref[:, 0], 0 * com_vel_ref[:, 0], 0 * com_acc_ref[:, 0]
140  )
141  elif i < N:
142  tsid_biped.set_com_ref(com_pos_ref[:, i], com_vel_ref[:, i], com_acc_ref[:, i])
143  tsid_biped.set_LF_3d_ref(x_LF_ref[:, i], dx_LF_ref[:, i], ddx_LF_ref[:, i])
144  tsid_biped.set_RF_3d_ref(x_RF_ref[:, i], dx_RF_ref[:, i], ddx_RF_ref[:, i])
145 
146  HQPData = tsid_biped.formulation.computeProblemData(t, q, v)
147  sol = tsid_biped.solver.solve(HQPData)
148 
149  if sol.status != 0:
150  print("QP problem could not be solved! Error code:", sol.status)
151  break
152  if norm(v, 2) > 40.0:
153  print("Time %.3f Velocities are too high, stop everything!" % (t), norm(v))
154  break
155 
156  if i > 0:
157  q_log[:, i] = q
158  v_log[:, i] = v
159  tau[:, i] = tsid_biped.formulation.getActuatorForces(sol)
160  dv = tsid_biped.formulation.getAccelerations(sol)
161 
162  if i >= 0:
163  com_pos[:, i] = tsid_biped.robot.com(tsid_biped.formulation.data())
164  com_vel[:, i] = tsid_biped.robot.com_vel(tsid_biped.formulation.data())
165  com_acc[:, i] = tsid_biped.comTask.getAcceleration(dv)
166  com_acc_des[:, i] = tsid_biped.comTask.getDesiredAcceleration
167  x_LF[:, i], dx_LF[:, i], ddx_LF[:, i] = tsid_biped.get_LF_3d_pos_vel_acc(dv)
168  if not tsid_biped.contact_LF_active:
169  ddx_LF_des[:, i] = tsid_biped.leftFootTask.getDesiredAcceleration[:3]
170  x_RF[:, i], dx_RF[:, i], ddx_RF[:, i] = tsid_biped.get_RF_3d_pos_vel_acc(dv)
171  if not tsid_biped.contact_RF_active:
172  ddx_RF_des[:, i] = tsid_biped.rightFootTask.getDesiredAcceleration[:3]
173 
174  if tsid_biped.formulation.checkContact(tsid_biped.contactRF.name, sol):
175  T_RF = tsid_biped.contactRF.getForceGeneratorMatrix
176  f_RF[:, i] = T_RF.dot(
177  tsid_biped.formulation.getContactForce(tsid_biped.contactRF.name, sol)
178  )
179  if f_RF[2, i] > 1e-3:
180  cop_RF[0, i] = f_RF[4, i] / f_RF[2, i]
181  cop_RF[1, i] = -f_RF[3, i] / f_RF[2, i]
182  if tsid_biped.formulation.checkContact(tsid_biped.contactLF.name, sol):
183  T_LF = tsid_biped.contactLF.getForceGeneratorMatrix
184  f_LF[:, i] = T_LF.dot(
185  tsid_biped.formulation.getContactForce(tsid_biped.contactLF.name, sol)
186  )
187  if f_LF[2, i] > 1e-3:
188  cop_LF[0, i] = f_LF[4, i] / f_LF[2, i]
189  cop_LF[1, i] = -f_LF[3, i] / f_LF[2, i]
190 
191  if i % conf.PRINT_N == 0:
192  print("Time %.3f" % (t))
193  if (
194  tsid_biped.formulation.checkContact(tsid_biped.contactRF.name, sol)
195  and i >= 0
196  ):
197  print(
198  "\tnormal force %s: %.1f"
199  % (tsid_biped.contactRF.name.ljust(20, "."), f_RF[2, i])
200  )
201 
202  if (
203  tsid_biped.formulation.checkContact(tsid_biped.contactLF.name, sol)
204  and i >= 0
205  ):
206  print(
207  "\tnormal force %s: %.1f"
208  % (tsid_biped.contactLF.name.ljust(20, "."), f_LF[2, i])
209  )
210 
211  print(
212  "\ttracking err %s: %.3f"
213  % (
214  tsid_biped.comTask.name.ljust(20, "."),
215  norm(tsid_biped.comTask.position_error, 2),
216  )
217  )
218  print("\t||v||: %.3f\t ||dv||: %.3f" % (norm(v, 2), norm(dv)))
219 
220  q, v = tsid_biped.integrate_dv(q, v, dv, conf.dt)
221  t += conf.dt
222 
223  q_list.append(q)
224 
225  if i % conf.DISPLAY_N == 0:
226  tsid_biped.display(q)
227 
228  time_spent = time.time() - time_start
229  if time_spent < conf.dt:
230  time.sleep(conf.dt - time_spent)
231 
232 while True:
233  replay = input("Play video again? [Y]/n: ") or "Y"
234  if replay.lower() == "y":
235  for q in q_list:
236  tsid_biped.display(q)
237  else:
238  break
239 # PLOT STUFF
240 time = np.arange(0.0, (N + N_post) * conf.dt, conf.dt)
241 
242 if PLOT_COM:
243  (f, ax) = plut.create_empty_figure(3, 1)
244  for i in range(3):
245  ax[i].plot(time, com_pos[i, :], label="CoM " + str(i))
246  ax[i].plot(time[:N], com_pos_ref[i, :], "r:", label="CoM Ref " + str(i))
247  ax[i].set_xlabel("Time [s]")
248  ax[i].set_ylabel("CoM [m]")
249  leg = ax[i].legend()
250  leg.get_frame().set_alpha(0.5)
251 
252  (f, ax) = plut.create_empty_figure(3, 1)
253  for i in range(3):
254  ax[i].plot(time, com_vel[i, :], label="CoM Vel " + str(i))
255  ax[i].plot(time[:N], com_vel_ref[i, :], "r:", label="CoM Vel Ref " + str(i))
256  ax[i].set_xlabel("Time [s]")
257  ax[i].set_ylabel("CoM Vel [m/s]")
258  leg = ax[i].legend()
259  leg.get_frame().set_alpha(0.5)
260 
261  (f, ax) = plut.create_empty_figure(3, 1)
262  for i in range(3):
263  ax[i].plot(time, com_acc[i, :], label="CoM Acc " + str(i))
264  ax[i].plot(time[:N], com_acc_ref[i, :], "r:", label="CoM Acc Ref " + str(i))
265  ax[i].plot(time, com_acc_des[i, :], "g--", label="CoM Acc Des " + str(i))
266  ax[i].set_xlabel("Time [s]")
267  ax[i].set_ylabel("CoM Acc [m/s^2]")
268  leg = ax[i].legend()
269  leg.get_frame().set_alpha(0.5)
270 
271 if PLOT_COP:
272  (f, ax) = plut.create_empty_figure(2, 1)
273  for i in range(2):
274  ax[i].plot(time, cop_LF[i, :], label="CoP LF " + str(i))
275  ax[i].plot(time, cop_RF[i, :], label="CoP RF " + str(i))
276  # ax[i].plot(time[:N], cop_ref[i,:], label='CoP ref '+str(i))
277  if i == 0:
278  ax[i].plot(
279  [time[0], time[-1]],
280  [-conf.lxn, -conf.lxn],
281  ":",
282  label="CoP Lim " + str(i),
283  )
284  ax[i].plot(
285  [time[0], time[-1]],
286  [conf.lxp, conf.lxp],
287  ":",
288  label="CoP Lim " + str(i),
289  )
290  elif i == 1:
291  ax[i].plot(
292  [time[0], time[-1]],
293  [-conf.lyn, -conf.lyn],
294  ":",
295  label="CoP Lim " + str(i),
296  )
297  ax[i].plot(
298  [time[0], time[-1]],
299  [conf.lyp, conf.lyp],
300  ":",
301  label="CoP Lim " + str(i),
302  )
303  ax[i].set_xlabel("Time [s]")
304  ax[i].set_ylabel("CoP [m]")
305  leg = ax[i].legend()
306  leg.get_frame().set_alpha(0.5)
307 
308 
309 # (f, ax) = plut.create_empty_figure(3,2)
310 # ax = ax.reshape((6))
311 # for i in range(6):
312 # ax[i].plot(time, f_LF[i,:], label='Force LF '+str(i))
313 # ax[i].plot(time, f_RF[i,:], label='Force RF '+str(i))
314 # ax[i].set_xlabel('Time [s]')
315 # ax[i].set_ylabel('Force [N/Nm]')
316 # leg = ax[i].legend()
317 # leg.get_frame().set_alpha(0.5)
318 
319 if PLOT_FOOT_TRAJ:
320  for i in range(3):
321  plt.figure()
322  plt.plot(time, x_RF[i, :], label="x RF " + str(i))
323  plt.plot(time[:N], x_RF_ref[i, :], ":", label="x RF ref " + str(i))
324  plt.plot(time, x_LF[i, :], label="x LF " + str(i))
325  plt.plot(time[:N], x_LF_ref[i, :], ":", label="x LF ref " + str(i))
326  plt.legend()
327 
328  # for i in range(3):
329  # plt.figure()
330  # plt.plot(time, dx_RF[i,:], label='dx RF '+str(i))
331  # plt.plot(time[:N], dx_RF_ref[i,:], ':', label='dx RF ref '+str(i))
332  # plt.plot(time, dx_LF[i,:], label='dx LF '+str(i))
333  # plt.plot(time[:N], dx_LF_ref[i,:], ':', label='dx LF ref '+str(i))
334  # plt.legend()
335  #
336  # for i in range(3):
337  # plt.figure()
338  # plt.plot(time, ddx_RF[i,:], label='ddx RF '+str(i))
339  # plt.plot(time[:N], ddx_RF_ref[i,:], ':', label='ddx RF ref '+str(i))
340  # plt.plot(time, ddx_RF_des[i,:], '--', label='ddx RF des '+str(i))
341  # plt.plot(time, ddx_LF[i,:], label='ddx LF '+str(i))
342  # plt.plot(time[:N], ddx_LF_ref[i,:], ':', label='ddx LF ref '+str(i))
343  # plt.plot(time, ddx_LF_des[i,:], '--', label='ddx LF des '+str(i))
344  # plt.legend()
345 
346 if PLOT_TORQUES:
347  plt.figure()
348  for i in range(tsid_biped.robot.na):
349  tau_normalized = (
350  2
351  * (tau[i, :] - tsid_biped.tau_min[i])
352  / (tsid_biped.tau_max[i] - tsid_biped.tau_min[i])
353  - 1
354  )
355  # plot torques only for joints that reached 50% of max torque
356  if np.max(np.abs(tau_normalized)) > 0.5:
357  plt.plot(
358  time, tau_normalized, alpha=0.5, label=tsid_biped.model.names[i + 2]
359  )
360  plt.plot([time[0], time[-1]], 2 * [-1.0], ":")
361  plt.plot([time[0], time[-1]], 2 * [1.0], ":")
362  plt.gca().set_xlabel("Time [s]")
363  plt.gca().set_ylabel("Normalized Torque")
364  leg = plt.legend()
365  leg.get_frame().set_alpha(0.5)
366 
367 if PLOT_JOINT_VEL:
368  plt.figure()
369  for i in range(tsid_biped.robot.na):
370  v_normalized = (
371  2
372  * (v_log[6 + i, :] - tsid_biped.v_min[i])
373  / (tsid_biped.v_max[i] - tsid_biped.v_min[i])
374  - 1
375  )
376  # plot v only for joints that reached 50% of max v
377  if np.max(np.abs(v_normalized)) > 0.5:
378  plt.plot(time, v_normalized, alpha=0.5, label=tsid_biped.model.names[i + 2])
379  plt.plot([time[0], time[-1]], 2 * [-1.0], ":")
380  plt.plot([time[0], time[-1]], 2 * [1.0], ":")
381  plt.gca().set_xlabel("Time [s]")
382  plt.gca().set_ylabel("Normalized Joint Vel")
383  leg = plt.legend()
384 # leg.get_frame().set_alpha(0.5)
385 
386 plt.show()
tsid_biped.TsidBiped
Definition: tsid_biped.py:10
center
Vec3f center

tsid
Author(s): Andrea Del Prete, Justin Carpentier
autogenerated on Sun Jul 2 2023 02:21:51