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  "Time %.3f Changing contact phase from %s to %s"
100  % (t, contact_phase[i - 1], contact_phase[i])
101  )
102  if contact_phase[i] == "left":
103  tsid.add_contact_LF()
104  tsid.remove_contact_RF()
105  else:
106  tsid.add_contact_RF()
107  tsid.remove_contact_LF()
108  elif i == N:
109  # switch to double support at the end
110  if contact_phase[i - 1] == "left":
111  tsid.add_contact_RF()
112  else:
113  tsid.add_contact_LF()
114 
115  if i < 0:
116  tsid.set_com_ref(
117  com_pos_ref[:, 0], 0 * com_vel_ref[:, 0], 0 * com_acc_ref[:, 0]
118  )
119  elif i < N:
120  tsid.set_com_ref(com_pos_ref[:, i], com_vel_ref[:, i], com_acc_ref[:, i])
121  tsid.set_LF_3d_ref(x_LF_ref[:, i], dx_LF_ref[:, i], ddx_LF_ref[:, i])
122  tsid.set_RF_3d_ref(x_RF_ref[:, i], dx_RF_ref[:, i], ddx_RF_ref[:, i])
123  tsid.copTask.setReference(np.concatenate([cop_ref[:, i], np.zeros(1)]))
124 
125  HQPData = tsid.formulation.computeProblemData(t, q, v)
126 
127  sol = tsid.solver.solve(HQPData)
128  if sol.status != 0:
129  print("QP problem could not be solved! Error code:", sol.status)
130  break
131  if norm(v, 2) > 40.0:
132  print("Time %.3f Velocities are too high, stop everything!" % (t), norm(v))
133  break
134 
135  if i > 0:
136  q_log[:, i] = q
137  v_log[:, i] = v
138  tau[:, i] = tsid.formulation.getActuatorForces(sol)
139  dv = tsid.formulation.getAccelerations(sol)
140 
141  if i >= 0:
142  com_pos[:, i] = tsid.robot.com(tsid.formulation.data())
143  com_vel[:, i] = tsid.robot.com_vel(tsid.formulation.data())
144  com_acc[:, i] = tsid.comTask.getAcceleration(dv)
145  com_acc_des[:, i] = tsid.comTask.getDesiredAcceleration
146  x_LF[:, i], dx_LF[:, i], ddx_LF[:, i] = tsid.get_LF_3d_pos_vel_acc(dv)
147  if not tsid.contact_LF_active:
148  ddx_LF_des[:, i] = tsid.leftFootTask.getDesiredAcceleration[:3]
149  x_RF[:, i], dx_RF[:, i], ddx_RF[:, i] = tsid.get_RF_3d_pos_vel_acc(dv)
150  if not tsid.contact_RF_active:
151  ddx_RF_des[:, i] = tsid.rightFootTask.getDesiredAcceleration[:3]
152 
153  if tsid.formulation.checkContact(tsid.contactRF.name, sol):
154  T_RF = tsid.contactRF.getForceGeneratorMatrix
155  f_RF[:, i] = T_RF.dot(
156  tsid.formulation.getContactForce(tsid.contactRF.name, sol)
157  )
158  if f_RF[2, i] > 1e-3:
159  cop_RF[0, i] = f_RF[4, i] / f_RF[2, i]
160  cop_RF[1, i] = -f_RF[3, i] / f_RF[2, i]
161  if tsid.formulation.checkContact(tsid.contactLF.name, sol):
162  T_LF = tsid.contactRF.getForceGeneratorMatrix
163  f_LF[:, i] = T_LF.dot(
164  tsid.formulation.getContactForce(tsid.contactLF.name, sol)
165  )
166  if f_LF[2, i] > 1e-3:
167  cop_LF[0, i] = f_LF[4, i] / f_LF[2, i]
168  cop_LF[1, i] = -f_LF[3, i] / f_LF[2, i]
169  cop_LF_world = tsid.get_placement_LF().act(
170  np.array([cop_LF[0, i], cop_LF[1, i], 0])
171  )
172  cop_RF_world = tsid.get_placement_RF().act(
173  np.array([cop_RF[0, i], cop_RF[1, i], 0])
174  )
175  cop[:, i] = (cop_LF_world[:2] * f_LF[2, i] + cop_RF_world[:2] * f_RF[2, i]) / (
176  f_LF[2, i] + f_RF[2, i]
177  )
178 
179  if i % conf.PRINT_N == 0:
180  print("Time %.3f" % (t))
181  if tsid.formulation.checkContact(tsid.contactRF.name, sol) and i >= 0:
182  print(
183  "\tnormal force %s: %.1f"
184  % (tsid.contactRF.name.ljust(20, "."), f_RF[2, i])
185  )
186 
187  if tsid.formulation.checkContact(tsid.contactLF.name, sol) and i >= 0:
188  print(
189  "\tnormal force %s: %.1f"
190  % (tsid.contactLF.name.ljust(20, "."), f_LF[2, i])
191  )
192 
193  print(
194  "\ttracking err %s: %.3f"
195  % (tsid.comTask.name.ljust(20, "."), norm(tsid.comTask.position_error, 2))
196  )
197  print("\t||v||: %.3f\t ||dv||: %.3f" % (norm(v, 2), norm(dv)))
198 
199  q, v = tsid.integrate_dv(q, v, dv, conf.dt)
200  t += conf.dt
201 
202  if i % conf.DISPLAY_N == 0:
203  tsid.display(q)
204 
205  time_spent = time.time() - time_start
206  if time_spent < conf.dt:
207  time.sleep(conf.dt - time_spent)
208 
209 # PLOT STUFF
210 time = np.arange(0.0, (N + N_post) * conf.dt, conf.dt)
211 
212 if PLOT_COM:
213  (f, ax) = plut.create_empty_figure(3, 1)
214  for i in range(3):
215  ax[i].plot(time, com_pos[i, :], label="CoM " + str(i))
216  ax[i].plot(time[:N], com_pos_ref[i, :], "r:", label="CoM Ref " + str(i))
217  ax[i].set_xlabel("Time [s]")
218  ax[i].set_ylabel("CoM [m]")
219  leg = ax[i].legend()
220  leg.get_frame().set_alpha(0.5)
221 
222  (f, ax) = plut.create_empty_figure(3, 1)
223  for i in range(3):
224  ax[i].plot(time, com_vel[i, :], label="CoM Vel " + str(i))
225  ax[i].plot(time[:N], com_vel_ref[i, :], "r:", label="CoM Vel Ref " + str(i))
226  ax[i].set_xlabel("Time [s]")
227  ax[i].set_ylabel("CoM Vel [m/s]")
228  leg = ax[i].legend()
229  leg.get_frame().set_alpha(0.5)
230 
231  (f, ax) = plut.create_empty_figure(3, 1)
232  for i in range(3):
233  ax[i].plot(time, com_acc[i, :], label="CoM Acc " + str(i))
234  ax[i].plot(time[:N], com_acc_ref[i, :], "r:", label="CoM Acc Ref " + str(i))
235  ax[i].plot(time, com_acc_des[i, :], "g--", label="CoM Acc Des " + str(i))
236  ax[i].set_xlabel("Time [s]")
237  ax[i].set_ylabel("CoM Acc [m/s^2]")
238  leg = ax[i].legend()
239  leg.get_frame().set_alpha(0.5)
240 
241 if PLOT_COP:
242  (f, ax) = plut.create_empty_figure(2, 1)
243  for i in range(2):
244  ax[i].plot(time, cop_LF[i, :], label="CoP LF " + str(i))
245  ax[i].plot(time, cop_RF[i, :], label="CoP RF " + str(i))
246  if i == 0:
247  ax[i].plot(
248  [time[0], time[-1]],
249  [-conf.lxn, -conf.lxn],
250  ":",
251  label="CoP Lim " + str(i),
252  )
253  ax[i].plot(
254  [time[0], time[-1]],
255  [conf.lxp, conf.lxp],
256  ":",
257  label="CoP Lim " + str(i),
258  )
259  elif i == 1:
260  ax[i].plot(
261  [time[0], time[-1]],
262  [-conf.lyn, -conf.lyn],
263  ":",
264  label="CoP Lim " + str(i),
265  )
266  ax[i].plot(
267  [time[0], time[-1]],
268  [conf.lyp, conf.lyp],
269  ":",
270  label="CoP Lim " + str(i),
271  )
272  ax[i].set_xlabel("Time [s]")
273  ax[i].set_ylabel("CoP (local) [m]")
274  leg = ax[i].legend()
275  leg.get_frame().set_alpha(0.5)
276 
277  (f, ax) = plut.create_empty_figure(2, 1)
278  for i in range(2):
279  ax[i].plot(time[:N], cop_ref[i, :], label="CoP ref " + str(i))
280  ax[i].plot(time, cop[i, :], label="CoP " + str(i))
281  ax[i].set_xlabel("Time [s]")
282  ax[i].set_ylabel("CoP (world) [m]")
283  leg = ax[i].legend()
284  leg.get_frame().set_alpha(0.5)
285 
286 # (f, ax) = plut.create_empty_figure(3,2)
287 # ax = ax.reshape((6))
288 # for i in range(6):
289 # ax[i].plot(time, f_LF[i,:], label='Force LF '+str(i))
290 # ax[i].plot(time, f_RF[i,:], label='Force RF '+str(i))
291 # ax[i].set_xlabel('Time [s]')
292 # ax[i].set_ylabel('Force [N/Nm]')
293 # leg = ax[i].legend()
294 # leg.get_frame().set_alpha(0.5)
295 
296 if PLOT_FOOT_TRAJ:
297  for i in range(3):
298  plt.figure()
299  plt.plot(time, x_RF[i, :], label="x RF " + str(i))
300  plt.plot(time[:N], x_RF_ref[i, :], ":", label="x RF ref " + str(i))
301  plt.plot(time, x_LF[i, :], label="x LF " + str(i))
302  plt.plot(time[:N], x_LF_ref[i, :], ":", label="x LF ref " + str(i))
303  plt.legend()
304 
305  # for i in range(3):
306  # plt.figure()
307  # plt.plot(time, dx_RF[i,:], label='dx RF '+str(i))
308  # plt.plot(time[:N], dx_RF_ref[i,:], ':', label='dx RF ref '+str(i))
309  # plt.plot(time, dx_LF[i,:], label='dx LF '+str(i))
310  # plt.plot(time[:N], dx_LF_ref[i,:], ':', label='dx LF ref '+str(i))
311  # plt.legend()
312  #
313  # for i in range(3):
314  # plt.figure()
315  # plt.plot(time, ddx_RF[i,:], label='ddx RF '+str(i))
316  # plt.plot(time[:N], ddx_RF_ref[i,:], ':', label='ddx RF ref '+str(i))
317  # plt.plot(time, ddx_RF_des[i,:], '--', label='ddx RF des '+str(i))
318  # plt.plot(time, ddx_LF[i,:], label='ddx LF '+str(i))
319  # plt.plot(time[:N], ddx_LF_ref[i,:], ':', label='ddx LF ref '+str(i))
320  # plt.plot(time, ddx_LF_des[i,:], '--', label='ddx LF des '+str(i))
321  # plt.legend()
322 
323 if PLOT_TORQUES:
324  plt.figure()
325  for i in range(tsid.robot.na):
326  tau_normalized = (
327  2 * (tau[i, :] - tsid.tau_min[i]) / (tsid.tau_max[i] - tsid.tau_min[i]) - 1
328  )
329  # plot torques only for joints that reached 50% of max torque
330  if np.max(np.abs(tau_normalized)) > 0.5:
331  plt.plot(time, tau_normalized, alpha=0.5, label=tsid.model.names[i + 2])
332  plt.plot([time[0], time[-1]], 2 * [-1.0], ":")
333  plt.plot([time[0], time[-1]], 2 * [1.0], ":")
334  plt.gca().set_xlabel("Time [s]")
335  plt.gca().set_ylabel("Normalized Torque")
336  leg = plt.legend()
337  leg.get_frame().set_alpha(0.5)
338 
339 if PLOT_JOINT_VEL:
340  plt.figure()
341  for i in range(tsid.robot.na):
342  v_normalized = (
343  2 * (v_log[6 + i, :] - tsid.v_min[i]) / (tsid.v_max[i] - tsid.v_min[i]) - 1
344  )
345  # plot v only for joints that reached 50% of max v
346  if np.max(np.abs(v_normalized)) > 0.5:
347  plt.plot(time, v_normalized, alpha=0.5, label=tsid.model.names[i + 2])
348  plt.plot([time[0], time[-1]], 2 * [-1.0], ":")
349  plt.plot([time[0], time[-1]], 2 * [1.0], ":")
350  plt.gca().set_xlabel("Time [s]")
351  plt.gca().set_ylabel("Normalized Joint Vel")
352  leg = plt.legend()
353 # leg.get_frame().set_alpha(0.5)
354 
355 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