# 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 --------')