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

tsid
Author(s): Andrea Del Prete, Justin Carpentier
autogenerated on Thu Apr 3 2025 02:47:16