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

tsid
Author(s): Andrea Del Prete, Justin Carpentier
autogenerated on Sat May 3 2025 02:48:16