#print("U: ", U[0:2])
q1.set_a_2D_alt_lya(U[0:2], -alt_d)
q2.set_a_2D_alt_lya(U[2:4], -alt_d)
q3.set_a_2D_alt_lya(U[4:6], -alt_d)
q1.step(dt)
q2.step(dt)
q3.step(dt)
# Animation
if it%frames == 0:
pl.figure(0)
axis3d.cla()
ani.draw3d(axis3d, q1.xyz, q1.Rot_bn(), quadcolor[0])
ani.draw3d(axis3d, q2.xyz, q2.Rot_bn(), quadcolor[1])
ani.draw3d(axis3d, q3.xyz, q3.Rot_bn(), quadcolor[2])
axis3d.set_xlim(-5, 5)
axis3d.set_ylim(-5, 5)
axis3d.set_zlim(0, 10)
axis3d.set_xlabel('South [m]')
axis3d.set_ylabel('East [m]')
axis3d.set_zlabel('Up [m]')
axis3d.set_title("Time %.3f s" %t)
pl.pause(0.001)
pl.draw()
#namepic = '%i'%it
#digits = len(str(it))
#for j in range(0, 5-digits):
# namepic = '0' + namepic
v_2D_d = np.array([2, -0.5])
alt_d = -8
q1.yaw_d = np.pi / 3
for t in time:
# Simulation
q1.set_v_2D_alt_lya(v_2D_d, alt_d)
q1.step(dt)
# Animation
if it % frames == 0:
pl.figure(0)
axis3d.cla()
ani.draw3d(axis3d, q1.xyz, q1.Rot_bn(), quadcolor[0])
axis3d.set_xlim(-10, 10)
axis3d.set_ylim(-10, 10)
axis3d.set_zlim(0, 15)
axis3d.set_xlabel('South [m]')
axis3d.set_ylabel('East [m]')
axis3d.set_zlabel('Up [m]')
axis3d.set_title("Time %.3f s" % t)
pl.pause(0.001)
pl.draw()
# Log
q1_log.xyz_h[it, :] = q1.xyz
q1_log.att_h[it, :] = q1.att
q1_log.w_h[it, :] = q1.w
q1_log.v_ned_h[it, :] = q1.v_ned
####################################################TEST SEQUENCE###########################################################
#for d in quad_list:
#d.group.syncronize(d,quad_list,radius)
#rules.flocking(d)
#if t > 120:
# for d in quad_list:
# d.group.syncronize(d,quad_list,radius)
# rules.spread(d)
for d in quad_list:
d.step(dt)
if it % frames == 0:
axis3d.cla()
ani.draw3d(axis3d, q1.xyz, q1.Rot_bn(), quadcolor[0])
ani.draw3d(axis3d, q2.xyz, q2.Rot_bn(), quadcolor[0])
ani.draw3d(axis3d, q3.xyz, q3.Rot_bn(), quadcolor[0])
ani.draw3d(axis3d, q4.xyz, q4.Rot_bn(), quadcolor[0])
ani.draw3d(axis3d, q5.xyz, q5.Rot_bn(), quadcolor[0])
ani.draw3d(axis3d, q6.xyz, q6.Rot_bn(), quadcolor[0])
axis3d.set_xlim(-15, 15)
axis3d.set_ylim(-15, 15)
axis3d.set_zlim(0, 15)
axis3d.set_xlabel('South [m]')
axis3d.set_ylabel('East [m]')
axis3d.set_zlabel('Up [m]')
axis3d.set_title("Time %.3f s" % t)
v_2D_d = np.array([2, -0.5])
alt_d = -8
q1.yaw_d = np.pi/3
for t in time:
# Simulation
q1.set_v_2D_alt_lya(v_2D_d, alt_d)
q1.step(dt)
# Animation
if it%frames == 0:
pl.figure(0)
axis3d.cla()
ani.draw3d(axis3d, q1.xyz, q1.Rot_bn(), quadcolor[0])
axis3d.set_xlim(-10, 10)
axis3d.set_ylim(-10, 10)
axis3d.set_zlim(0, 15)
axis3d.set_xlabel('South [m]')
axis3d.set_ylabel('East [m]')
axis3d.set_zlabel('Up [m]')
axis3d.set_title("Time %.3f s" %t)
pl.pause(0.001)
pl.draw()
# Log
q1_log.xyz_h[it, :] = q1.xyz
q1_log.att_h[it, :] = q1.att
q1_log.w_h[it, :] = q1.w
def run_simulation(self):
radius = 2
angle = 360 / self.number_of_drones
angle_list = np.ones(self.number_of_drones)*angle
des_dist_list = np.zeros(self.number_of_drones)
for idx in range(0,len(angle_list)): # This only works for equal angles for the whole formation.
des_dist_list[idx] = self.angle_to_chord(radius, angle_list[idx])
act_dist_list = np.zeros(self.number_of_drones)
err_dist_list = np.zeros(self.number_of_drones)
unit_vector_list = np.zeros((self.number_of_drones, 3))
inter_angle_list = np.zeros(self.number_of_drones)
ke = 0.00285*2 *0.7 # Gain for going towards circle
kt = 0.01*7 # Gain for orbiting
kf = 0.08 *0.5 # Gain for formation
drone_added = True
#drone_added = True
do_animation = False
for self.t in self.time:
# If we want to add a new drone
if drone_added == False and int(self.t % 100) == 0 and self.t > 1.0:
self.add_new_drone()
drone_added = True
self.number_of_drones = len(self.qc_list) # Recalculate number of drones for all for-loops in the code.
if self.number_of_drones > len(act_dist_list):
print("[" + str(self.t) + "] - Adding new drone and reinitializing vectors")
act_dist_list = np.zeros((self.number_of_drones))
err_dist_list = np.zeros(self.number_of_drones)
unit_vector_list = np.zeros((self.number_of_drones, 3))
inter_angle_list = np.zeros(self.number_of_drones)
angle = 360 / self.number_of_drones
angle_list = np.ones(self.number_of_drones)*angle
des_dist_list = np.zeros(self.number_of_drones)
for idx in range(0,len(angle_list)): # This only works for equal angles for the whole formation.
des_dist_list[idx] = self.angle_to_chord(radius, angle_list[idx])
self.q_log_list = np.append(self.q_log_list, quadlog.quadlog(self.time))
drone_id_list = []
for idx in range(0, self.number_of_drones):
drone_id_list.append(self.qc_list[idx].identification)
for i in drone_id_list:
copter = self.qc_list[i]
copter.broadcast_rel_xyz(self.qt)
for idx in drone_id_list:
act_dist_list[idx] = self.to_euclid(self.qc_list[idx].calc_neighbour_vector(self.qc_list[(idx + 1) % self.number_of_drones]))
for idx in drone_id_list:
err_dist_list[idx] = act_dist_list[idx] - des_dist_list[idx]
#print(unit_vector_list.shape)
#print(len(qc_list))
#print(act_dist_list)
for idx in drone_id_list:
for i in range (0, 3):
unit_vector_list[idx][i] = self.qc_list[idx].calc_neighbour_vector(self.qc_list[(idx + 1) % self.number_of_drones])[i] / act_dist_list[idx]
# Formation adjustment by rearranging drones in the list depending on the angles between them
for idx in drone_id_list:
i = idx - 1 # A delayed counter that wraps in the beginning.
o = idx + 1 # A hastened counter that wraps in the end.
if idx == 0:
i = self.number_of_drones - 1
if idx == self.number_of_drones - 1:
o = 0
inter_angle_list[idx] = self.angle_between_unit_vectors(self.qc_list[i].xyz - self.qc_list[idx].xyz, self.qc_list[o].xyz - self.qc_list[idx].xyz)
for idx in range (0, self.number_of_drones):
if inter_angle_list[idx] < 30:
if self.qc_list[idx].cooldown < 0 and self.qc_list[i].cooldown < 0 and self.qc_list[o].cooldown < 0:
i = idx - 1 # A delayed counter that wraps in the beginning.
o = idx + 1 # A hastened counter that wraps in the end.
if idx == 0:
i = self.number_of_drones - 1
if idx == self.number_of_drones - 1:
o = 0
print("[" + str(self.t) + "] - Swapping drone q" + str(o+1) + " and q" + str(idx+1))
# Swap the drones
get = self.qc_list[o], self.qc_list[idx]
self.qc_list[idx], self.qc_list[o] = get
# Reset the cooldown on these drones
self.qc_list[idx].cooldown = self.qc_list[o].cooldown = self.qc_list[i].cooldown = 150*20
self.number_of_swaps += 1
# Swap the colors as well, so it won't look weird in the graphs
#get_color = quadcolor[o+1], quadcolor[idx+1]
#quadcolor[idx+1], quadcolor[o+1] = get_color
#print("Desired: ", des_dist_list)
#print("Actual: ", act_dist_list)
#print("Errors: ", err_dist_list)
#print("unit_vectors: " , unit_vector_list)
# Simulation
X = V = []
for idx in drone_id_list:
X = np.append(X, self.qc_list[idx].xyz[0:2])
V = np.append(V, self.qc_list[idx].v_ned[0:2])
grad_list = np.empty((0,2))
for i in drone_id_list:
grad_list = np.append(grad_list, np.array([[(X[i*2] - self.qt.xyz[0])*2, (X[i*2 + 1] - self.qt.xyz[1])*2]]), axis=0)
tangent_list = np.empty((0,2))
for idx in drone_id_list:
tangent_list = np.append(tangent_list, np.array([self.tangentHelp.dot((grad_list[idx]/la.norm(grad_list[idx])))]), axis=0)
e_list = np.array([])
for i in drone_id_list:
e_list = np.append(e_list, self.impPath(X[i*2], X[i*2 + 1], radius))
# This small u is our circle following control input (velocity)
# The form is:
# ControlInput = FollowCircleError + RotateOnCircle + StayInFormationDistance
formation_list = np.empty((0,2))
for idx in drone_id_list:
i = idx - 1
if idx == 0:
i = self.number_of_drones - 1 # A delayed counter that wraps in the beginning.
formation_list = np.append(formation_list, np.array([[(err_dist_list[idx]*unit_vector_list[idx][0] + err_dist_list[i]*unit_vector_list[idx][0])*kf, (err_dist_list[idx]*unit_vector_list[idx][1] + err_dist_list[idx]*unit_vector_list[i][1])*kf]]), axis=0)
#print(err_dist_list)
u_list = np.empty((0,2))
for idx in drone_id_list:
u_list = np.append(u_list, np.array([ke*(-e_list[idx])*grad_list[idx] + kt*tangent_list[idx] + formation_list[idx]]), axis=0)
for idx in drone_id_list:
u_list[idx] = self.sigmoid(u_list[idx])
#zero = np.array([0, 0])
ut = np.array([0.25, 0.25]) # move the middle drone qt a little bit in x and y
#ut = np.array([0, 0])
#ut = impPath(qt.xyz[0], qt.xyz[1], 2)
self.qt.set_v_2D_alt_lya(ut, -self.alt_d)
for idx in drone_id_list:
self.qc_list[idx].set_v_2D_alt_lya(u_list[idx] + ut, -self.alt_d)
self.qt.step(self.dt)
for idx in drone_id_list:
self.qc_list[idx].step(self.dt)
# Animation
if self.it % self.frames == 0 and do_animation == True:
pl.figure(0)
self.axis3d.cla()
ani.draw3d(self.axis3d, self.qt.xyz, self.qt.Rot_bn(), self.quadcolor[0])
for idx in drone_id_list:
ani.draw3d(self.axis3d, self.qc_list[idx].xyz, self.qc_list[idx].Rot_bn(), self.quadcolor[idx+1])
#ani.draw3d(axis3d, q1.xyz, q1.Rot_bn(), quadcolor[1])
#ani.draw3d(axis3d, q2.xyz, q2.Rot_bn(), quadcolor[2])
#ani.draw3d(axis3d, q3.xyz, q3.Rot_bn(), quadcolor[3])
self.axis3d.set_xlim(-10, 10)
self.axis3d.set_ylim(-10, 10)
self.axis3d.set_zlim(0, 10)
self.axis3d.set_xlabel('South [m]')
self.axis3d.set_ylabel('East [m]')
self.axis3d.set_zlabel('Up [m]')
self.axis3d.set_title("Time %.3f s" % self.t)
pl.pause(0.001)
pl.draw()
# namepic = '%i'%it
# digits = len(str(it))
# for j in range(0, 5-digits):
# namepic = '0' + namepic
# pl.savefig("./images/%s.png"%namepic)
#cid = fig.canvas.mpl_connect('key_press_event', on_key)
# Log
p = 0
for idx in drone_id_list:
self.q_log_list[p].xyz_h[self.it, :] = self.qc_list[idx].xyz
self.q_log_list[p].att_h[self.it, :] = self.qc_list[idx].att
self.q_log_list[p].w_h[self.it, :] = self.qc_list[idx].w
self.q_log_list[p].v_ned_h[self.it, :] = self.qc_list[idx].v_ned
self.q_log_list[p].formation_error[self.it] = err_dist_list[idx]
self.q_log_list[p].circle_error[self.it] = e_list[idx]
p += 1
self.qt_log.xyz_h[self.it, :] = self.qt.xyz
self.qt_log.att_h[self.it, :] = self.qt.att
self.qt_log.w_h[self.it, :] = self.qt.w
self.qt_log.v_ned_h[self.it, :] = self.qt.v_ned
self.it += 1
# Stop if crash
for q in self.qc_list:
if q.crashed==1:
break
return self.q_log_list, self.qt_log, drone_id_list # and also return qt_log...
def run(self, method, run_animation=False):
if not (method == 'NF' or method == 'KF' or method == 'PF' or method == 'PKF' or \
method == 'NF4' or method == 'KF4' or method == 'PF4' or method == 'PKF4' or method == 'PF2' or method == 'PKF2'):
print ("Wrong Input, your in put was: ", method)
return -1
use = 4
use4 = False
if len(method) > 2:
if method == 'PKF4' or method == 'PKF' or method == 'PKF2':
if method == 'PKF4':
method = method[0:3]
use4 = True
elif method == 'PKF2':
method = method[0:3]
use4 = True
use = 2
else:
use4 = True
method = method[0:2]
dt = self.dt
it = self.it
kalmanStarted = False
PFstarted = False
PKFstarted = False
quadcolor = ['r', 'g', 'b']
pl.close("all")
pl.ion()
fig = pl.figure(0)
axis3d = fig.add_subplot(111, projection='3d')
frames = self.frames
for t in self.time:
acc_err = np.random.normal(0, 0.012, 1)[0]
#HANDLE RANGE MEASUREMENTS:
if it % 50 == 0 or it == 0: # or method == 'NF':
#print(t)
self.UAV_agent.handle_range_msg(self.RA0.id, self.get_dist(self.UAV.xyz, self.uwb0.xyz))
self.UAV_agent.handle_range_msg(self.RA1.id, self.get_dist(self.UAV.xyz, self.uwb1.xyz))
self.UAV_agent.handle_range_msg(self.RA2.id, self.get_dist(self.UAV.xyz, self.uwb2.xyz))
self.UAV_agent.handle_range_msg(self.RA3.id, self.get_dist(self.UAV.xyz, self.uwb3.xyz))
self.UAV_agent.handle_range_msg(self.RA4.id, self.get_dist(self.UAV.xyz, self.uwb4.xyz))
self.UAV_agent.handle_range_msg(self.RA5.id, self.get_dist(self.UAV.xyz, self.uwb5.xyz))
self.UAV_agent.handle_range_msg(self.RA6.id, self.get_dist(self.UAV.xyz, self.uwb6.xyz))
if PFstarted and method == 'PF':
self.UAV_agent.PFupdate(use4=use4, use=use)
if PKFstarted and method == 'PKF':
self.UAV_agent.updatePKF(use4=use4, use=use)
if kalmanStarted and method == 'KF':
alg_pos = self.UAV_agent.calc_pos_alg(use4=use4)
#HANDLE POS NO FILTER:
if method == 'NF':
alg_pos = self.UAV_agent.calc_pos_alg(use4=use4)
#HANDLE KALMAN FILTER:
if method == 'KF':
if self.UAV.xyz[2] < -3 and not kalmanStarted:
self.R, self.Q = self.UAV_agent.startKF(self.UAV.xyz, v_ned=self.UAV.v_ned, dt=dt)
kalmanStarted = True
if kalmanStarted:
self.UAV_agent.KFpredict( self.UAV.acc + acc_err )
alg_pos = self.UAV_agent.get_kf_state()
else:
alg_pos = self.UAV_agent.calc_pos_alg(use4=use4)
#HANDLE PARTICLE FILTER
if method == 'PF':
if self.UAV.xyz[2] < -3 and not PFstarted:
self.n_of_particles, self.std_add = self.UAV_agent.startPF(start_vel=self.UAV.v_ned, dt=dt)
PFstarted = True
if PFstarted:
alg_pos = self.UAV_agent.getPFpos()
#print (PF_pos)
self.UAV_agent.PFpredict(self.UAV.acc + acc_err)
else:
alg_pos = self.UAV.xyz
#HANDLE PARTICLE KALMAN FILTER
if method == 'PKF':
if self.UAV.xyz[2] < -3 and not PKFstarted:
self.R, self.Q, self.n_of_particles, self.std_add = self.UAV_agent.startPKF(self.UAV.acc + acc_err, dt=dt, xyz=self.UAV.xyz, v_ned=self.UAV.v_ned)
PKFstarted = True
if PKFstarted:
alg_pos = self.UAV_agent.get_PKFstate()
self.UAV_agent.predictPKF(self.UAV.acc + acc_err)
else:
alg_pos = self.UAV.xyz
#print("Estimated pos:",alg_pos)
#print("True pos: ",self.UAV.xyz)
#HANDLE UAV MOVEMENT:
x_err = abs(self.wp[self.state][0] - self.UAV.xyz[0])
y_err = abs(self.wp[self.state][1] - self.UAV.xyz[1])
if self.state == 0:
self.UAV.set_v_2D_alt_lya(np.array([x_err*0.03, y_err*0.03]), -3)
if self.get_dist_clean(self.UAV.xyz, self.wp[self.state]) < 0.4:
self.state = 2
elif self.state == 1:
self.UAV.set_v_2D_alt_lya(np.array([x_err*0.03, -y_err*0.03]),-3)
if self.get_dist_clean(self.UAV.xyz, self.wp[self.state]) < 0.4:
self.state = 0
elif self.state == 2:
self.UAV.set_v_2D_alt_lya(np.array([-x_err*0.03, -y_err*0.03]), -3)
if self.get_dist_clean(self.UAV.xyz, self.wp[self.state]) < 0.4:
self.state = 3
elif self.state == 3:
self.UAV.set_v_2D_alt_lya(np.array([-x_err*0.03, y_err*0.03]), -3)
if self.get_dist_clean(self.UAV.xyz, self.wp[self.state]) < 0.4:
self.state = 1
self.UAV.step(dt)
#LOGS:
#self.est_pos[it] = alg_pos
#self.gt_pos[it] = self.UAV.xyz
if kalmanStarted or PFstarted or PKFstarted or method == 'NF':
self.Ed_log[it, :] = np.array([ self.get_dist_clean(alg_pos, self.UAV.xyz) ])
self.Ed2d_log[it, :] = np.array([ self.get_dist_clean(alg_pos[0:2], self.UAV.xyz[0:2]) ])
self.Edalt_log[it, :] = np.array([ self.get_dist_clean(alg_pos[2], self.UAV.xyz[2]) ])
self.Ed_vel_log[it, :] = np.array([ self.get_dist_clean(alg_pos, self.UAV.v_ned) ])
self.alg_log.xyz_h[it, :] = alg_pos
self.UAV_log.xyz_h[it, :] = self.UAV.xyz
self.UAV_log.att_h[it, :] = self.UAV.att
self.UAV_log.w_h[it, :] = self.UAV.w
self.UAV_log.v_ned_h[it, :] = self.UAV.v_ned
it+=1
# Stop if crash
if (self.UAV.crashed == 1):
break
if run_animation:
if it%frames == 0:
pl.figure(0)
axis3d.cla()
ani.draw3d(axis3d, self.uwb0.xyz, self.uwb0.Rot_bn(), quadcolor[0])
ani.draw3d(axis3d, self.uwb1.xyz, self.uwb1.Rot_bn(), quadcolor[2])
ani.draw3d(axis3d, self.uwb2.xyz, self.uwb2.Rot_bn(), quadcolor[2])
ani.draw3d(axis3d, self.uwb3.xyz, self.uwb3.Rot_bn(), quadcolor[2])
ani.draw3d(axis3d, self.uwb4.xyz, self.uwb4.Rot_bn(), quadcolor[2])
ani.draw3d(axis3d, self.uwb5.xyz, self.uwb5.Rot_bn(), quadcolor[2])
ani.draw3d(axis3d, self.uwb6.xyz, self.uwb6.Rot_bn(), quadcolor[2])
ani.draw3d(axis3d, self.UAV.xyz, self.UAV.Rot_bn(), quadcolor[1])
axis3d.set_xlim(-6, 6)
axis3d.set_ylim(-6, 6)
axis3d.set_zlim(0, 10)
axis3d.set_xlabel('South [m]')
axis3d.set_ylabel('East [m]')
axis3d.set_zlabel('Up [m]')
axis3d.set_title("Time %.3f s" %t)
pl.pause(0.001)
pl.draw()
alg_log = np.array([self.alg_log.xyz_h[:,0], self.alg_log.xyz_h[:,1], self.alg_log.xyz_h[:,2]])
uav_log = np.array([self.UAV_log.xyz_h[:,0], self.UAV_log.xyz_h[:,1], self.UAV_log.xyz_h[:,2]])
#print(self.Ed_log)
return [uav_log, alg_log, self.Ed_log, self.Ed2d_log, self.Edalt_log]
U = fc.u_acc(X, V)
q1.set_a_2D_alt_lya(U[0:2], -alt_d)
q2.set_a_2D_alt_lya(U[2:4], -alt_d)
q3.set_a_2D_alt_lya(U[4:6], -alt_d)
q1.step(dt)
q2.step(dt)
q3.step(dt)
# Animation
if it%frames == 0:
pl.figure(0)
axis3d.cla()
ani.draw3d(axis3d, q1.xyz, q1.Rot_bn(), quadcolor[0])
ani.draw3d(axis3d, q2.xyz, q2.Rot_bn(), quadcolor[1])
ani.draw3d(axis3d, q3.xyz, q3.Rot_bn(), quadcolor[2])
axis3d.set_xlim(-5, 5)
axis3d.set_ylim(-5, 5)
axis3d.set_zlim(0, 10)
axis3d.set_xlabel('South [m]')
axis3d.set_ylabel('East [m]')
axis3d.set_zlabel('Up [m]')
axis3d.set_title("Time %.3f s" %t)
pl.pause(0.001)
pl.draw()
#namepic = '%i'%it
#digits = len(str(it))
#for j in range(0, 5-digits):
# namepic = '0' + namepic
