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()