contact_phase = data['contact_phase']
com_pos_ref = np.asmatrix(data['com'])
com_vel_ref = np.asmatrix(data['dcom'])
com_acc_ref = np.asmatrix(data['ddcom'])
x_RF_ref = np.asmatrix(data['x_RF'])
dx_RF_ref = np.asmatrix(data['dx_RF'])
ddx_RF_ref = np.asmatrix(data['ddx_RF'])
x_LF_ref = np.asmatrix(data['x_LF'])
dx_LF_ref = np.asmatrix(data['dx_LF'])
ddx_LF_ref = np.asmatrix(data['ddx_LF'])
cop_ref = np.asmatrix(data['cop'])
com_acc_des = matlib.empty(
(3, N + N_post)) * nan # acc_des = acc_ref - Kp*pos_err - Kd*vel_err
x_rf = tsid.get_placement_RF().translation
offset = x_rf - x_RF_ref[:, 0]
for i in range(N):
com_pos_ref[:, i] += offset
x_RF_ref[:, i] += offset
x_LF_ref[:, i] += offset
t = -conf.T_pre
q, v = tsid.q, tsid.v
for i in range(-N_pre, N + N_post):
time_start = time.time()
if i == 0:
print("Starting to walk (remove contact left foot)")
tsid.remove_contact_LF()
contact_phase = data['contact_phase']
com_pos_ref = np.asarray(data['com'])
com_vel_ref = np.asarray(data['dcom'])
com_acc_ref = np.asarray(data['ddcom'])
x_RF_ref = np.asarray(data['x_RF'])
dx_RF_ref = np.asarray(data['dx_RF'])
ddx_RF_ref = np.asarray(data['ddx_RF'])
x_LF_ref = np.asarray(data['x_LF'])
dx_LF_ref = np.asarray(data['dx_LF'])
ddx_LF_ref = np.asarray(data['ddx_LF'])
cop_ref = np.asarray(data['cop'])
com_acc_des = np.empty(
(3, N + N_post)) * nan # acc_des = acc_ref - Kp*pos_err - Kd*vel_err
x_rf = tsid.get_placement_RF().translation
offset = x_rf - x_RF_ref[:, 0]
for i in range(N):
com_pos_ref[:, i] += offset + np.array([0., 0., 0.0])
cop_ref[:, i] += offset[:2]
x_RF_ref[:, i] += offset
x_LF_ref[:, i] += offset
# remove nan from cop_ref
cop_ref[:, -1] = cop_ref[:, -2]
t = -conf.T_pre
q, v = tsid.q, tsid.v
for i in range(-N_pre, N + N_post):
time_start = time.time()
