# Tf = 1.0 # doesnt work dist_check = 15.0 # distance to waypoint to say waypoint reached throttle_const = 0.59375 # constant mapping u4 to throttle(t): t = tc * u4 max_abs_roll_rate = 50.0 # clamping roll rate max_abs_pitch_rate = 50.0 # clamping pitch rate max_abs_yaw_rate = 50.0 # clamping yaw rate max_iter = 400 # maximum iterations to time out of the loop ue = np.array([0, 0, 0, g]) # nominal control x0 = np.array([0, 0, -20, 0, 0, 0, 0, 0, 0]) # linearize the non-linear quadrotor model # Ac, Bc = linear_quad_model(num_states = 9, g = g) # make the acados system model model = acados_linear_quad_model(9, g=g) # create ocp object to formulate the OCP ocp = AcadosOcp() ocp.model = model nx = model.x.size()[0] nu = model.u.size()[0] ny = nx + nu ny_e = nx N = int(Tf / dt) ocp.dims.N = N # set cost Q = kq * np.eye(n) R = kr * np.eye(m)
def fly_plan(self, trajectory, t_kf): # some parameters g = 9.81 # acc. due to gravity dt = 0.1 # time step n = 9 # state dimension m = 4 # control input dimension # kq = 1 # parameter to tune the Q matrix Q = kq * I # kq2 = 1 # kr = 0.1#1 works for linear case # parameter to tune the R matrix R = kr * I kqxy = 1 # parameter to tune the Q matrix Q = kq * I kqz = 10 kqrp = 0.1 #0.01 kqy = 1 kqxyv = 1 kqzv = 1 krt = 0.001 krrpr = 0.1 kryr = 0.1 # rotate_quad = True # set to true if you want yaw to change between waypoints mode = 'nonlinear' v2 = False # set to true if you want to compute jacobian along the way in linear sys Tf = 0.5 # time horizon for MPC # dist_check = 15.0 # distance to waypoint to say waypoint reached # throttle_const = 0.59375 # constant mapping u4 to throttle(t): t = tc * u4 max_abs_roll_rate = 10.0 #50.0 # clamping roll rate max_abs_pitch_rate = 10.0 #50.0 # clamping pitch rate max_abs_yaw_rate = 10.0 #50.0 # clamping yaw rate # max_iter = 400 # maximum iterations to time out of the loop ue = np.array([0, 0, 0, g]) # nominal control ### # initialize quadrotor model q = Quadrotor(dt=dt) init_pos, _ = q.setStateAtNomTime(trajectory, 0) total_steps = int(np.floor((np.cumsum(t_kf))[-1] / dt)) ### # initialize the controller if mode == 'nonlinear': print('--------------------') print('Using Non-linear model') print('--------------------') model = acados_nonlinear_quad_model(9, g=g) else: if v2 == True: print('--------------------') print('Using linear model V2 - same as NL??') print('--------------------') model = acados_linear_quad_model_moving_eq(9, g=g) else: print('--------------------') print('Using linear model') print('--------------------') model = acados_linear_quad_model(9, g=g) # ocp_solver = create_ocp_solver(model, Tf, dt, kq, kr, n, m, # max_abs_roll_rate, max_abs_pitch_rate, # max_abs_yaw_rate, init_pos[:n], kq2) ocp_solver = create_ocp_solver_separate_gains( model, Tf, dt, n, m, max_abs_roll_rate, max_abs_pitch_rate, max_abs_yaw_rate, init_pos[:n], kqxy, kqz, kqrp, kqy, kqxyv, kqzv, krt, krrpr, kryr) N = int(Tf / dt) self.airsim_client.moveToPositionAsync(init_pos[0], init_pos[1], init_pos[2], 5).join() prev_yaw = 0 jump_yaw = np.pi print('-------- Executing the path --------') for i in range(total_steps - 1): # if breakNow == True: # break # q.setSystemTime(i * dt) time = i * dt for num1 in range(N): t = time + num1 * dt if num1 == 0: full_state, _ = q.getStateAtNomTime(trajectory, t) # all_diff_flat_states[i,:] = full_state[:n].copy() else: full_state, _ = q.getStateAtNomTime(trajectory, t) yref = np.hstack((full_state[:n], ue)) ocp_solver.set(num1, "yref", yref) t = time + N * dt full_state, _ = q.getStateAtNomTime(trajectory, t) yref_e = full_state[:n].copy() ocp_solver.set(N, "yref", yref_e) status = ocp_solver.solve() if status != 0: raise Exception( 'acados returned status {}. Exiting.'.format(status)) u = ocp_solver.get(0, "u") # est_x = ocp_solver.get(1, "x") # all_controls[i,:] = u.copy() # all_est_path[i,:] = est_x.copy() # all_diff_flat_states[i,:] = full_state[:n].copy() rotmat_u = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]) u[0:3] = rotmat_u @ u[0:3] self.airsim_client.moveByAngleRatesThrottleAsync( u[0], u[1], u[2], u[3], dt).join() x = get_drone_state(self.drone_state.kinematics_estimated, n) # print(u[3]) # print(x) # if (np.abs(prev_yaw - x[5]) > jump_yaw): # x[5] -= 2 * np.pi if (prev_yaw - x[5] > jump_yaw): x[5] += 2 * np.pi elif (prev_yaw - x[5] < -jump_yaw): x[5] -= 2 * np.pi prev_yaw = x[5] # all_est_path_asim[i,:] = x.copy() ocp_solver.set(0, "lbx", x) ocp_solver.set(0, "ubx", x) print('-------- Done Executing --------')