state = dynamics.state x_hist = [] t_hist = [] while (t0 < params.tf): tp = t0 + params.t_plot while t0 < tp: x_hist.append(state.copy()) x_ref = np.array([[ 5 * np.cos(0.2 * (t0 + i * params.dt)), 5 * np.sin(0.2 * (t0 + i * params.dt)), -10 - (t0 + i * params.dt) * 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0 ] for i in range(params.T + 1)]).T u = controller.calculateControl(x_ref, state) state = dynamics.updateState(u + u_eq) # state = dynamics.updateState(u) t0 += params.dt if isinstance(controller, LNMPC): A, B = dynamics.get_SS(state) controller.A = A controller.B = B if t0 > 8.0: # x_ref = np.array([-xr for i in range(params.T+1)]).T debug = 1 t = state[:3] ang = state[6:9] R_b_from_i = Rotation.from_euler('ZYX', [ang[2], ang[1], ang[0]]).as_dcm() # viewer.update(t, R_b_from_i) data_view.update(state, x_ref[cmd_idx, 0], params.t_plot)