def formation(self):
'''
formation control algorithm
'''
ready = True
for rc in self.rotorcrafts:
if not rc.initialized:
if self.verbose:
print("Waiting for state of rotorcraft ", rc.id)
sys.stdout.flush()
ready = False
if rc.timeout > 0.5:
if self.verbose:
print("The state msg of rotorcraft ", rc.id, " stopped")
sys.stdout.flush()
ready = False
if rc.initialized and 'geo_fence' in list(self.config.keys()):
geo_fence = self.config['geo_fence']
if not self.ignore_geo_fence:
if (rc.X[0] < geo_fence['x_min']
or rc.X[0] > geo_fence['x_max']
or rc.X[1] < geo_fence['y_min']
or rc.X[1] > geo_fence['y_max']
or rc.X[2] < geo_fence['z_min']
or rc.X[2] > geo_fence['z_max']):
if self.verbose:
print("The rotorcraft", rc.id,
" is out of the fence (", rc.X, ", ",
geo_fence, ")")
sys.stdout.flush()
ready = False
if not ready:
return
dim = 2 * len(self.rotorcrafts)
X = np.zeros(dim)
V = np.zeros(dim)
U = np.zeros(dim)
i = 0
for rc in self.rotorcrafts:
X[i] = rc.X[0]
X[i + 1] = rc.X[1]
V[i] = rc.V[0]
V[i + 1] = rc.V[1]
i = i + 2
Bb = la.kron(self.B, np.eye(2))
Z = Bb.T.dot(X)
Dz = rf.make_Dz(Z, 2)
Dzt = rf.make_Dzt(Z, 2, 1)
Dztstar = rf.make_Dztstar(self.d * self.scale, 2, 1)
Zh = rf.make_Zh(Z, 2)
E = rf.make_E(Z, self.d * self.scale, 2, 1)
# Shape and motion control
jmu_t1 = self.joystick.trans * self.t1[0, :]
jtilde_mu_t1 = self.joystick.trans * self.t1[1, :]
jmu_r1 = self.joystick.rot * self.r1[0, :]
jtilde_mu_r1 = self.joystick.rot * self.r1[1, :]
jmu_t2 = self.joystick.trans2 * self.t2[0, :]
jtilde_mu_t2 = self.joystick.trans2 * self.t2[1, :]
jmu_r2 = self.joystick.rot2 * self.r2[0, :]
jtilde_mu_r2 = self.joystick.rot2 * self.r2[1, :]
Avt1 = rf.make_Av(self.B, jmu_t1, jtilde_mu_t1)
Avt1b = la.kron(Avt1, np.eye(2))
Avr1 = rf.make_Av(self.B, jmu_r1, jtilde_mu_r1)
Avr1b = la.kron(Avr1, np.eye(2))
Avt2 = rf.make_Av(self.B, jmu_t2, jtilde_mu_t2)
Avt2b = la.kron(Avt2, np.eye(2))
Avr2 = rf.make_Av(self.B, jmu_r2, jtilde_mu_r2)
Avr2b = la.kron(Avr2, np.eye(2))
Avb = Avt1b + Avr1b + Avt2b + Avr2b
Avr = Avr1 + Avr2
if self.B.size == 2:
Zhr = np.array([-Zh[1], Zh[0]])
U = -self.k[1] * V - self.k[0] * Bb.dot(Dz).dot(Dzt).dot(
E) + self.k[1] * (Avt1b.dot(Zh) + Avt2b.dot(Zhr)) + np.sign(
jmu_r1[0]) * la.kron(
Avr1.dot(Dztstar).dot(self.B.T).dot(Avr1),
np.eye(2)).dot(Zhr)
else:
U = -self.k[1] * V - self.k[0] * Bb.dot(Dz).dot(Dzt).dot(
E) + self.k[1] * Avb.dot(Zh) + la.kron(
Avr.dot(Dztstar).dot(self.B.T).dot(Avr), np.eye(2)).dot(Zh)
if self.verbose:
#print "Positions: " + str(X).replace('[','').replace(']','')
#print "Velocities: " + str(V).replace('[','').replace(']','')
#print "Acceleration command: " + str(U).replace('[','').replace(']','')
print("Error distances: " +
str(E).replace('[', '').replace(']', ''))
sys.stdout.flush()
i = 0
for ac in self.rotorcrafts:
msg = PprzMessage("datalink", "DESIRED_SETPOINT")
msg['ac_id'] = ac.id
msg['flag'] = 0 # full 3D not supported yet
msg['ux'] = U[i]
msg['uy'] = U[i + 1]
msg['uz'] = self.altitude
self._interface.send(msg)
i = i + 2
def formation(self):
'''
formation control algorithm
'''
ready = True
for rc in self.rotorcrafts:
if not rc.initialized:
if self.verbose:
print("Waiting for state of rotorcraft ", rc.id)
sys.stdout.flush()
ready = False
if rc.timeout > 0.5:
if self.verbose:
print("The state msg of rotorcraft ", rc.id, " stopped")
sys.stdout.flush()
ready = False
if rc.initialized and 'geo_fence' in self.config.keys():
geo_fence = self.config['geo_fence']
if not self.ignore_geo_fence:
if (rc.X[0] < geo_fence['x_min'] or rc.X[0] > geo_fence['x_max']
or rc.X[1] < geo_fence['y_min'] or rc.X[1] > geo_fence['y_max']
or rc.X[2] < geo_fence['z_min'] or rc.X[2] > geo_fence['z_max']):
if self.verbose:
print("The rotorcraft", rc.id, " is out of the fence (", rc.X, ", ", geo_fence, ")")
sys.stdout.flush()
ready = False
if not ready:
return
dim = 2 * len(self.rotorcrafts)
X = np.zeros(dim)
V = np.zeros(dim)
U = np.zeros(dim)
i = 0
for rc in self.rotorcrafts:
X[i] = rc.X[0]
X[i+1] = rc.X[1]
V[i] = rc.V[0]
V[i+1] = rc.V[1]
i = i + 2
Bb = la.kron(self.B, np.eye(2))
Z = Bb.T.dot(X)
Dz = rf.make_Dz(Z, 2)
Dzt = rf.make_Dzt(Z, 2, 1)
Dztstar = rf.make_Dztstar(self.d * self.scale, 2, 1)
Zh = rf.make_Zh(Z, 2)
E = rf.make_E(Z, self.d * self.scale, 2, 1)
# Shape and motion control
jmu_t1 = self.joystick.trans * self.t1[0,:]
jtilde_mu_t1 = self.joystick.trans * self.t1[1,:]
jmu_r1 = self.joystick.rot * self.r1[0,:]
jtilde_mu_r1 = self.joystick.rot * self.r1[1,:]
jmu_t2 = self.joystick.trans2 * self.t2[0,:]
jtilde_mu_t2 = self.joystick.trans2 * self.t2[1,:]
jmu_r2 = self.joystick.rot2 * self.r2[0,:]
jtilde_mu_r2 = self.joystick.rot2 * self.r2[1,:]
Avt1 = rf.make_Av(self.B, jmu_t1, jtilde_mu_t1)
Avt1b = la.kron(Avt1, np.eye(2))
Avr1 = rf.make_Av(self.B, jmu_r1, jtilde_mu_r1)
Avr1b = la.kron(Avr1, np.eye(2))
Avt2 = rf.make_Av(self.B, jmu_t2, jtilde_mu_t2)
Avt2b = la.kron(Avt2, np.eye(2))
Avr2 = rf.make_Av(self.B, jmu_r2, jtilde_mu_r2)
Avr2b = la.kron(Avr2, np.eye(2))
Avb = Avt1b + Avr1b + Avt2b + Avr2b
Avr = Avr1 + Avr2
if self.B.size == 2:
Zhr = np.array([-Zh[1],Zh[0]])
U = -self.k[1]*V - self.k[0]*Bb.dot(Dz).dot(Dzt).dot(E) + self.k[1]*(Avt1b.dot(Zh) + Avt2b.dot(Zhr)) + np.sign(jmu_r1[0])*la.kron(Avr1.dot(Dztstar).dot(self.B.T).dot(Avr1), np.eye(2)).dot(Zhr)
else:
U = -self.k[1]*V - self.k[0]*Bb.dot(Dz).dot(Dzt).dot(E) + self.k[1]*Avb.dot(Zh) + la.kron(Avr.dot(Dztstar).dot(self.B.T).dot(Avr), np.eye(2)).dot(Zh)
if self.verbose:
#print "Positions: " + str(X).replace('[','').replace(']','')
#print "Velocities: " + str(V).replace('[','').replace(']','')
#print "Acceleration command: " + str(U).replace('[','').replace(']','')
print "Error distances: " + str(E).replace('[','').replace(']','')
sys.stdout.flush()
i = 0
for ac in self.rotorcrafts:
msg = PprzMessage("datalink", "DESIRED_SETPOINT")
msg['ac_id'] = ac.id
msg['flag'] = 0 # full 3D not supported yet
msg['ux'] = U[i]
msg['uy'] = U[i+1]
msg['uz'] = self.altitude
self._interface.send(msg)
i = i+2