class Tutorial:
x_off = 1.0
y_off = 1.0
count = 0
x_f = 0
y_f = 0
x = 0
y = 0
# Setting quads
m = 0.65 # Kg
l = 0.23 # m
Jxx = 7.5e-3 # Kg/m^2
Jyy = Jxx
Jzz = 1.3e-2
Jxy = 0
Jxz = 0
Jyz = 0
J = np.array([[Jxx, Jxy, Jxz], \
[Jxy, Jyy, Jyz], \
[Jxz, Jyz, Jzz]])
CDl = 9e-3
CDr = 9e-4
kt = 3.13e-5 # Ns^2
km = 7.5e-7 # Ns^2
kw = 1 / 0.18 # rad/s
# Initial conditions
att_0 = np.array([0.0, 0.0, 0.0])
pqr_0 = np.array([0.0, 0.0, 0.0])
xyz1_0 = np.array([1.0, 1.2, 0.0])
v_ned_0 = np.array([0.0, 0.0, 0.0])
w_0 = np.array([0.0, 0.0, 0.0, 0.0])
q1 = quad.quadrotor(1, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz1_0, v_ned_0, w_0)
count_back = 0
vres = numpy.array([0.0, 0.0, 0.0])
blockListDict = {
'block_a': Block('iris_1', ""),
'block_b': Block('iris_2', ""),
'block_c': Block('iris_3', "")
}
def swarm(self, uav_name, blockListDict):
#separation=rules.separation("iris_1",self.blockListDict)
#cohesion=rules.cohesion("iris_1",self.blockListDict)
self.vres[0] = rules.flocking(uav_name,
blockListDict)[0] #+ self.x_off
self.vres[1] = 0.4 * rules.flocking(uav_name,
blockListDict)[1] #+ self.y_off
self.vres[2] = self.q1.set_v_2D_alt_lya(self.vres[0:1], -1)[2]
return self.vres
[Jxz, Jyz, Jzz]])
CDl = 9e-3
CDr = 9e-4
kt = 3.13e-5 # Ns^2
km = 7.5e-7 # Ns^2
kw = 1 / 0.18 # rad/s
# Initial conditions
att_0 = np.array([0.0, 0.0, 0.0])
pqr_0 = np.array([0.0, 0.0, 0.0])
xyz_0 = np.array([0.0, 0.0, 0.0])
v_ned_0 = np.array([0.0, 0.0, 0.0])
w_0 = np.array([0.0, 0.0, 0.0, 0.0])
# Setting quads
q1 = quad.quadrotor(1, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz_0, v_ned_0, w_0)
# Simulation parameters
tf = 250
dt = 1e-2
time = np.linspace(0, tf, tf / dt)
it = 0
frames = 100
# Data log
q1_log = quadlog.quadlog(time)
# Plots
quadcolor = ['k']
pl.close("all")
pl.ion()
CDr = 9e-4
kt = 3.13e-5 # Ns^2
km = 7.5e-7 # Ns^2
kw = 1/0.18 # rad/s
# Initial conditions
att_0 = np.array([0.0, 0.0, 0.0])
pqr_0 = np.array([0.0, 0.0, 0.0])
xyz1_0 = np.array([1.0, 1.2, 0.0])
xyz2_0 = np.array([1.2, 2.0, 0.0])
xyz3_0 = np.array([-1.1, 2.6, 0.0])
v_ned_0 = np.array([0.0, 0.0, 0.0])
w_0 = np.array([0.0, 0.0, 0.0, 0.0])
# Setting quads
q1 = quad.quadrotor(1, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz1_0, v_ned_0, w_0)
q2 = quad.quadrotor(2, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz2_0, v_ned_0, w_0)
q3 = quad.quadrotor(3, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz3_0, v_ned_0, w_0)
# Formation Control
# Shape
side = 8
Btriang = np.array([[1, 0, -1],[-1, 1, 0],[0, -1, 1]])
dtriang = np.array([side, side, side])
# Motion
mu = 0e-2*np.array([1, 1, 1])
# Initial conditions
att_0 = np.array([0.0, 0.0, 0.0])
pqr_0 = np.array([0.0, 0.0, 0.0])
xyz1_0 = np.array([1.0, 1.2, 0.0])
xyz2_0 = np.array([2.2, 2.0, 0.0])
xyz3_0 = np.array([-3.1, 20.6, 0.0])
xyz4_0 = np.array([12.0, 8.2, 0.0])
xyz5_0 = np.array([10.2, 9.0, 0.0])
xyz6_0 = np.array([-11.2, 10.6, 0.0])
v_ned_0 = np.array([0.0, 0.0, 0.0])
w_0 = np.array([0.0, 0.0, 0.0, 0.0])
# Setting quads
q1 = quad.quadrotor(1, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz1_0, v_ned_0, w_0)
q2 = quad.quadrotor(2, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz2_0, v_ned_0, w_0)
q3 = quad.quadrotor(3, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz3_0, v_ned_0, w_0)
q4 = quad.quadrotor(4, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz4_0, v_ned_0, w_0)
q5 = quad.quadrotor(5, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz5_0, v_ned_0, w_0)
q6 = quad.quadrotor(6, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz6_0, v_ned_0, w_0)
[Jxz, Jyz, Jzz]])
CDl = 9e-3
CDr = 9e-4
kt = 3.13e-5 # Ns^2
km = 7.5e-7 # Ns^2
kw = 1/0.18 # rad/s
# Initial conditions
att_0 = np.array([0.0, 0.0, 0.0])
pqr_0 = np.array([0.0, 0.0, 0.0])
xyz_0 = np.array([0.0, 0.0, 0.0])
v_ned_0 = np.array([0.0, 0.0, 0.0])
w_0 = np.array([0.0, 0.0, 0.0, 0.0])
# Setting quads
q1 = quad.quadrotor(1, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz_0, v_ned_0, w_0)
# Simulation parameters
tf = 250
dt = 1e-2
time = np.linspace(0, tf, tf/dt)
it = 0
frames = 100
# Data log
q1_log = quadlog.quadlog(time)
# Plots
quadcolor = ['k']
pl.close("all")
pl.ion()
def add_new_drone(self):
self.qc_list = np.append(self.qc_list, quad.quadrotor(0, self.m, self.l, self.J, self.CDl, self.CDr, self.kt, self.km, self.kw, self.att_0, self.pqr_0, self.xyzt_0 + np.append([np.random.rand(1,2)], 0.0), self.v_ned_0, self.w_0, self.number_of_drones))
self.qc_list[self.number_of_drones - 1].yaw_d = 0
def __init__(self, tf, dt, time, it, frames, number_of_drones):
self.number_of_drones = number_of_drones
self.frames = frames
self.it = it
self.time = time
self.dt = dt
self.tf = tf
# Quadrotor
self.m = 0.65 # Kg
self.l = 0.23 # m
Jxx = 7.5e-3 # Kg/m^2
Jyy = Jxx
Jzz = 1.3e-2
Jxy = 0
Jxz = 0
Jyz = 0
self.J = np.array([[Jxx, Jxy, Jxz],
[Jxy, Jyy, Jyz],
[Jxz, Jyz, Jzz]])
self.CDl = 9e-3
self.CDr = 9e-4
self.kt = 3.13e-5 # Ns^2
self.km = 7.5e-7 # Ns^2
self.kw = 1.0/0.18 # rad/s
# Initial conditions
self.att_0 = np.array([0.0, 0.0, 0.0])
self.pqr_0 = np.array([0.0, 0.0, 0.0])
self.xyzt_0 = np.array([0.0, 0.0, -0.0])
self.v_ned_0 = np.array([0.0, 0.0, 0.0])
self.w_0 = np.array([0.0, 0.0, 0.0, 0.0])
# Setting quads
self.qt = quad.quadrotor(0, self.m, self.l, self.J, self.CDl, self.CDr, self.kt, self.km, self.kw, self.att_0, self.pqr_0, self.xyzt_0, self.v_ned_0, self.w_0, 't')
self.alt_d = 2
self.qc_list = np.empty((0, 1))
for idx in range(number_of_drones):
self.qc_list = np.append(self.qc_list, quad.quadrotor(0, self.m, self.l, self.J, self.CDl, self.CDr, self.kt, self.km, self.kw, self.att_0, self.pqr_0, self.xyzt_0 + np.append([np.random.rand(1,2)], 0.0), self.v_ned_0, self.w_0, idx))
self.qc_list[idx].yaw_d = 0
# Data log
self.qt_log = quadlog.quadlog(self.time)
self.q_log_list = np.empty((0,1))
for idx in range(number_of_drones):
self.q_log_list = np.append(self.q_log_list, quadlog.quadlog(self.time))
#q1_log = quadlog.quadlog(time)
#q2_log = quadlog.quadlog(time)
#q3_log = quadlog.quadlog(time)
# Plots
self.quadcolor = ['m', 'r', 'g', 'b', 'c', 'y', 'k', 'w', 'r', 'g', 'b', 'c', 'y', 'k', 'w', 'r', 'g', 'b', 'c', 'y', 'k', 'w']
pl.close("all")
pl.ion()
fig = pl.figure(0)
self.axis3d = fig.add_subplot(111, projection='3d')
self.init_area = 5
self.s = 2
self.number_of_swaps = 0
# Constants
self.tangentHelp = np.array([[0, -1], [1, 0]])
w_0 = np.array([0.0, 0.0, 0.0, 0.0])
# Formation information
B = np.array([[-1, 0], [0, 1], [1, -1]])
D_e = np.array([[2], [2]])
D_n = np.array([[3.464], [-3.464]])
# Simulation parameters
tf = 400
dt = 1e-2
time = np.linspace(0, tf, tf / dt)
it = 0
frames = 1000
# Setting quads
q1 = quad.quadrotor(1, dt, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz1_0, v_ned_0, w_0, B, D_e, D_n)
q2 = quad.quadrotor(2, dt, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz2_0, v_ned_0, w_0, B, D_e, D_n)
q3 = quad.quadrotor(3, dt, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz3_0, v_ned_0, w_0, B, D_e, D_n)
# Data log
q1_log = quadlog.quadlog(time)
q2_log = quadlog.quadlog(time)
q3_log = quadlog.quadlog(time)
# Plots
quadcolor = ['r', 'g', 'b']
pl.close("all")
I = np.diag([5e-7, 5e-7, 10e-7])
k=3e-5
m=0.5
kd=0.25
#inicialization of variables
N=500
z, z_vel, z_acel= [0]*N, [0]*N, [0]*N
phi_acel, theta_acel, psi_acel= [0]*N, [0]*N, [0]*N
phi_vel, theta_vel, psi_vel= [0]*N, [0]*N, [0]*N
phi = theta = psi = [0]*N
Torque=[0]*4
Thrust=10
Height_objective=-10
dt=0.1
for i in range(N):
#DRONE height position, velocity and acceleration
phi[i], theta[i], psi[i], phi_vel[i], theta_vel[i], psi_vel[i], Thrust, z_acel[i],error= quadrotor(phi[i-1], theta[i-1],psi[i-1],phi_vel[i-1], theta_vel[i-1], psi_vel[i-1], Height_objective, Thrust,z_vel[i-1],z[i-1],z_vel[i-2])
z_vel[i]= z_vel[i-1] + z_acel[i]/2*dt
z[i]= z[i-1] + z_vel[i]/2*dt
plt.plot(i,z[i],'or') #plot of the drone height vs time
plt.pause(0.1)
plt.show()
def __init__(self, tf=500, dt=0.02):
m = 0.65 # Kg
l = 0.23 # m
Jxx = 7.5e-3 # Kg/m^2
Jyy = Jxx
Jzz = 1.3e-2
Jxy = 0
Jxz = 0
Jyz = 0
J = np.array([[Jxx, Jxy, Jxz], \
[Jxy, Jyy, Jyz], \
[Jxz, Jyz, Jzz]])
CDl = 9e-3
CDr = 9e-4
kt = 3.13e-5 # Ns^2
km = 7.5e-7 # Ns^2
kw = 1/0.18 # rad/s
# Initial conditions
att_0 = np.array([0.0, 0.0, 0.0])
pqr_0 = np.array([0.0, 0.0, 0.0])
d = 4.0
dy = d * (np.sqrt(3)/2)
xyz0_0 = np.array([0.0, 0.0, 0.1]) #4
xyz1_0 = np.array([d, 0.0, 0.05]) #1.5
xyz2_0 = np.array([d/2, dy, 0.0])
xyz3_0 = np.array([d/2, -dy, 0.0])
xyz4_0 = np.array([-(d/2), dy, 0.0])
xyz5_0 = np.array([-d, 0.0, 0.05]) #1.5
xyz6_0 = np.array([-(d/2), -dy, 0.0])
xyz_uav_0 = np.array([1.0, 1.5, 0.0])
self.state = 0
#wp = np.array([ [ 2, 2, -5 ], [ 2, -2, -5], [ -2, -2, -5 ], [-2, 2, -5] ])
#wp = np.array([ [ d, dy+1, -3 ], [ d, -(dy+1), -3 ], [ -1, -(dy+1), -3 ], [-1, dy+1, -3 ] ])
self.wp = np.array([ [ d, dy+1, -3 ], [ d, -dy+1, -3 ], [ -3, -dy-1, -3 ], [-4, dy+1, -3 ] ])
v_ned_0 = np.array([0.0, 0.0, 0.0])
w_0 = np.array([0.0, 0.0, 0.0, 0.0])
# Setting quads
self.uwb0 = quad.quadrotor(0, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz0_0, v_ned_0, w_0)
self.uwb1 = quad.quadrotor(1, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz1_0, v_ned_0, w_0)
self.uwb2 = quad.quadrotor(2, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz2_0, v_ned_0, w_0)
self.uwb3 = quad.quadrotor(3, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz3_0, v_ned_0, w_0)
self.uwb4 = quad.quadrotor(4, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz4_0, v_ned_0, w_0)
self.uwb5 = quad.quadrotor(5, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz5_0, v_ned_0, w_0)
self.uwb6 = quad.quadrotor(6, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz6_0, v_ned_0, w_0)
self.UAV = quad.quadrotor(10, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz_uav_0, v_ned_0, w_0)
# Simulation parameters
self.tf = tf
self.dt = dt
self.time = np.linspace(0, tf, int(tf/dt))
self.it = 0
self.frames = 50
# Data log
self.alg_log = quadlog.quadlog(self.time)
self.UAV_log = quadlog.quadlog(self.time)
#self.est_pos = np.zeros((self.time.size, 3))
#self.gt_pos = np.zeros((self.time.size, 3))
self.Ed_log = np.zeros((self.time.size, 1))
self.Ed2d_log = np.zeros((self.time.size, 1))
self.Edalt_log = np.zeros((self.time.size, 1))
self.Ed_vel_log = np.zeros((self.time.size, 1))
self.eig_log = np.zeros((self.time.size, 3))
self.RA0 = range_agent.uwb_agent( ID=0 )
self.RA1 = range_agent.uwb_agent( ID=1 )
self.RA2 = range_agent.uwb_agent( ID=2 )
self.RA3 = range_agent.uwb_agent( ID=3 )
self.RA4 = range_agent.uwb_agent( ID=4 )
self.RA5 = range_agent.uwb_agent( ID=5 )
self.RA6 = range_agent.uwb_agent( ID=6 )
self.UAV_agent = range_agent.uwb_agent( ID=10, d=d)
self.R = self.Q = self.n_of_particles = self.std_add = 0.0
CDr = 9e-4
kt = 3.13e-5 # Ns^2
km = 7.5e-7 # Ns^2
kw = 1/0.18 # rad/s
# Initial conditions
att_0 = np.array([0.0, 0.0, 0.0])
pqr_0 = np.array([0.0, 0.0, 0.0])
xyz1_0 = np.array([1.0, 1.2, 0.0])
xyz2_0 = np.array([1.2, 2.0, 0.0])
xyz3_0 = np.array([-1.1, 2.6, 0.0])
v_ned_0 = np.array([0.0, 0.0, 0.0])
w_0 = np.array([0.0, 0.0, 0.0, 0.0])
# Setting quads
q1 = quad.quadrotor(1, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz1_0, v_ned_0, w_0)
q2 = quad.quadrotor(2, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz2_0, v_ned_0, w_0)
q3 = quad.quadrotor(3, m, l, J, CDl, CDr, kt, km, kw, \
att_0, pqr_0, xyz3_0, v_ned_0, w_0)
# Formation Control
# Shape
side = 8
Btriang = np.array([[1, 0, -1],[-1, 1, 0],[0, -1, 1]])
dtriang = np.array([side, side, side])
# Motion
mu = 0e-2*np.array([1, 1, 1])
