class mission:
def __init__(self, num):
#Unity Display
self.figure = UnityFigure(UnityFigure.FIGURE_3D, UnityFigure.SCENE_EMPTY)
time.sleep(1)
self.listParticules = [Particule(np.array([[0],[0],[0]]), np.array([[1],[0]]), np.diag((10**-9,10**-9,10**-9)), self.figure ) for i in range(num)]
self.t = 0
self.dt = 0.1
self.tfinal = 130
self.num = num
self.anim = self.figure.createAnimation(self.dt)
time.sleep(1)
print("Ajout des objets à l'animation")
for part in self.listParticules:
self.anim.addObject(part.auv)
time.sleep(1)
def __repr__(self):
return "Programme de la mission: \n Aller en ligne droite, retour a 60s\n Nombre de particules {}".format(self.num)
def display(self):
print("Affichage de la mission sur PyUnityVibes")
self.figure.animate(self.anim)
def run(self):
while self.t < self.tfinal :
sys.stdout.write("t = %f \r" % self.t)
for part in self.listParticules:
part.step(self.t, self.dt)
part.appendFrame(self.anim)
self.t += self.dt
print("\n Done ! ")
time.sleep(1)
self.display()
class mission:
def __init__(self, num):
#Unity Display
self.figure = UnityFigure(UnityFigure.FIGURE_3D,
UnityFigure.SCENE_EMPTY)
time.sleep(1)
self.listParticules = [
Particule(np.array([[0.0], [0.0], [0.0]]), np.array([[1.0],
[0.0]]),
np.diag((10**-9, 10**-9, 10**-9)), self.figure)
for i in range(num)
]
self.t = 0
self.dt = 0.1
self.tfinal = 60
self.num = num
self.anim = self.figure.createAnimation(self.dt)
time.sleep(1)
print("Ajout des objets à l'animation")
for part in self.listParticules:
self.anim.addObject(part.auv)
time.sleep(1)
def __repr__(self):
return "Programme de la mission: \n Aller en ligne droite, retour a 60s\n Nombre de particules {}".format(
self.num)
def display(self):
print("Affichage de la mission sur PyUnityVibes")
self.figure.animate(self.anim)
def afficher_ellipse_all(self, fig, col=[0.9, 0, 0]):
global max_x, max_y, min_y, min_x #pour regler la fenetre de l'affichage
all_Xchap = [p.Xchap[0, 0] for p in self.listParticules]
all_Ychap = [p.Xchap[1, 0] for p in self.listParticules]
if min(all_Xchap) < min_x or min_x == None:
min_x = min(all_Xchap)
if min(all_Ychap) < min_y or min_y == None:
min_y = min(all_Ychap)
if max(all_Xchap) > max_x or max_x == None:
max_x = max(all_Xchap)
if max(all_Ychap) > max_y or max_y == None:
max_y = max(all_Ychap)
ax = fig.add_subplot(111, aspect='equal')
ax.set_xlim(min_x - 30, max_x + 30)
ax.set_ylim(min_y - 30, max_y + 30)
for p in self.listParticules:
p.afficher_ellipse(ax, col)
def recalage(self):
for part in self.listParticules:
x_gps = part.X[0, 0] + part.noise(0.48)
y_gps = part.X[1, 0] + part.noise(0.48)
part.Xchap[0, 0] = x_gps
part.Xchap[1, 0] = y_gps
part.cov = np.diag([0.48**2, 0.48**2, part.cov[2, 2]])
L = part.Xchap[0, 0]
h = part.Xchap[1, 0]
part.theta = -(np.arctan2(h, L) + np.pi)
part.U[1, 0] = part.theta
def aller_retour(self):
global max_x, max_y, min_y, min_x #pour regler la fenetre de l'affichage
max_x, max_y, min_y, min_x = self.listParticules[0].Xchap[
0, 0], self.listParticules[0].Xchap[1, 0], self.listParticules[
0].Xchap[1, 0], self.listParticules[0].Xchap[0, 0]
for part in self.listParticules:
part.theta = np.arctan2(0.0001, 50)
while self.t < self.tfinal:
print("[{:.2f},{:.2f},{:.2f}]".format(
self.listParticules[0].X[0, 0], self.listParticules[0].X[1, 0],
self.listParticules[0].X[2, 0]))
sys.stdout.write("Aller : t = %f \r" % self.t)
for part in self.listParticules:
part.step(self.t, self.dt)
part.appendFrame(self.anim)
self.t += self.dt
""" Affichage """
fig_ellipse = plt.figure()
self.afficher_ellipse_all(fig_ellipse, [0.9, 0, 0])
self.recalage()
while self.t < 2 * self.tfinal:
sys.stdout.write("Retour : t = %f \r" % self.t)
for part in self.listParticules:
part.step(self.t, self.dt)
part.appendFrame(self.anim)
self.t += self.dt
print("\n Done ! ")
time.sleep(1)
self.display()
""" Affichage """
self.afficher_ellipse_all(fig_ellipse, [0, 0, 0.9])
plt.xlabel("coordonnee x en metres")
plt.ylabel("coordonnee y en metres")
plt.title(
"Ellipses d'incertitude en position pour les differents\n auv apres trajet aller puis apres trajet retour"
)
plt.show()
def run(self):
while self.t < self.tfinal:
sys.stdout.write("t = %f \r" % self.t)
for part in self.listParticules:
part.step(self.t, self.dt)
part.appendFrame(self.anim)
self.t += self.dt
print("\n Done ! ")
time.sleep(1)
self.display()
# Evolution Equations
xa = xa + dt * xdot(xa, ua)
xb = xb + dt * xdot(xb, ub)
# Append the new frame with calculated position to the submarine and sensor
# Calculation of the rotation angle to maintain the direction of the objects
angle1 = math.atan2(dxa - xa[0][0], dza - xa[1][0]) - math.pi
angle2 = math.atan2(dxb - xb[0][0], dzb - xb[1][0]) - math.pi
anim.appendFrame(sub,
x=xa[0][0],
y=-0.4,
z=xa[1][0],
rx=0,
ry=math.degrees(angle1),
rz=0)
anim.appendFrame(sensor,
x=xb[0][0],
y=-0.4,
z=xb[1][0],
rx=0,
ry=math.degrees(angle2),
rz=0)
# Updating the last position for the direction angle calculation
dxa, dxb = xa[0][0], xb[0][0]
dza, dzb = xa[1][0], xb[1][0]
time.sleep(1)
# Start the animation
figure.animate(anim)
class mission:
def __init__(self, num):
self.figure = UnityFigure(UnityFigure.FIGURE_3D,
UnityFigure.SCENE_EMPTY)
time.sleep(1)
self.tfinal = 60
self.listParticules = [
Particule(np.array([[0.0], [0.0], [0.0]]), np.array([[1.0],
[0.0]]),
np.diag((10**-9, 10**-9, 10**-9)), self.figure)
for i in range(num)
]
time.sleep(1)
self.t = 0
self.dt = 0.1
self.num = num
self.traj_part_X = []
self.traj_part_Y = []
for _ in range(N):
self.traj_part_X.append([])
self.traj_part_Y.append([])
self.anim = self.figure.createAnimation(self.dt)
self.listboue = [[50, 0], [25, 25], [0, 0]]
time.sleep(1)
print("Ajout des objets à l'animation")
for part in self.listParticules:
self.anim.addObject(part.auv)
time.sleep(1.5)
for coord in self.listboue:
boue = self.figure.create(UnityFigure.OBJECT_3D_CUBE,
coord[1],
0,
coord[0],
dimX=0.2,
dimY=5,
dimZ=0.2,
color=UnityFigure.COLOR_BLACK)
time.sleep(1.5)
self.anim.addObject(boue)
time.sleep(1.5)
def __repr__(self):
return "Programme de la mission: \n Aller en ligne droite, retour a 60s\n Nombre de particules {}".format(
self.num)
def display(self):
print("Affichage de la mission sur PyUnityVibes")
self.figure.animate(self.anim)
def afficher_ellipse_all(self, ax, col=[0.9, 0, 0]):
for p in self.listParticules:
p.afficher_ellipse(ax, col)
def recalage(self, amer):
for part in self.listParticules:
x_gps = part.X[0, 0] + part.noise(0.48**2)
y_gps = part.X[1, 0] + part.noise(0.48**2)
L = 0 - part.Xchap[0, 0]
h = 0 - part.Xchap[1, 0]
part.theta = np.arctan2(h, L) #-(np.arctan2(h,L) + np.pi)
part.U[1, 0] = part.theta
def aller_retour(self, amer):
for part in self.listParticules:
part.theta = np.arctan2(0.0001, 50)
# Figure
################
fig = plt.figure()
ax = fig.add_subplot(111, aspect='equal')
################
#self.afficher_ellipse_all(ax, "red")
#Aller
############################
while self.t < self.tfinal:
sys.stdout.write("Aller : t = %f \r" % self.t)
for ind, part in enumerate(self.listParticules):
part.step_aller_retour(self.t, self.dt, amer)
part.appendFrame(self.anim)
self.traj_part_X[ind].append(part.X[0, 0])
self.traj_part_Y[ind].append(part.X[1, 0])
#self.afficher_ellipse_all(ax, "green")
self.t += self.dt
############################
#self.afficher_ellipse_all(ax, "red")
#Retour
############################
while self.t < 2 * self.tfinal + 20:
sys.stdout.write("Retour : t = %f \r" % self.t)
for ind, part in enumerate(self.listParticules):
part.step_aller_retour(self.t, self.dt, amer)
part.appendFrame(self.anim)
self.traj_part_X[ind].append(part.X[0, 0])
self.traj_part_Y[ind].append(part.X[1, 0])
#self.afficher_ellipse_all(ax, "green")
self.t += self.dt
#self.afficher_ellipse_all(ax, "green")
print("\n")
# Display the trajectories
#########################
ind_inter = int(self.tfinal / self.dt)
for k in range(N):
plt.plot(self.traj_part_X[k][:ind_inter],
self.traj_part_Y[k][:ind_inter], 'b')
plt.plot(self.traj_part_X[k][:ind_inter],
self.traj_part_Y[k][:ind_inter], 'b')
for k in range(N):
plt.plot(self.traj_part_X[k][ind_inter:],
self.traj_part_Y[k][ind_inter:], 'r')
plt.plot(self.traj_part_X[k][ind_inter:],
self.traj_part_Y[k][ind_inter:], 'r')
#########################
plt.xlim([-30, 150])
plt.ylim([-60, 60])
plt.xlabel("x (m)")
plt.ylabel("y (m)")
plt.show()
print("\n Done ! ")
time.sleep(1)
self.display()
def run(self):
while self.t < self.tfinal:
#sys.stdout.write("t = %f \r" % self.t)
for part in self.listParticules:
part.step_aller_retour(self.t, self.dt)
part.appendFrame(self.anim)
self.t += self.dt
print("\n Done ! ")
time.sleep(1)
self.display()
def mission_triangle(self, liste_coord_amer):
T = 0
retard = 0
for amer in liste_coord_amer:
T += self.listParticules[0].distance_amer(amer)
print("T:", T)
print("t:", self.t)
presence_amer = True
#self.recalage(amer)
while self.t < T:
presence_amer = False
print(
amer, "[{:.2f},{:.2f},{:.2f},{:.2f}]".format(
self.listParticules[0].X[0, 0],
self.listParticules[0].X[1, 0],
self.listParticules[0].X[2, 0],
self.listParticules[0].theta)
) #print("[{:.2f},{:.2f},{:.2f}]".format(self.listParticules[0].X[0,0], self.listParticules[0].X[1,0], self.listParticules[0].X[2,0]))
for part in self.listParticules:
#part.U[1,0]= np.arctan2(amer[1] - part.Xchap[1,0],amer[0] - part.Xchap[0,0])
part.step_mission(self.t, self.dt, presence_amer, amer)
part.appendFrame(self.anim)
if self.listParticules[0].X[2, 0] < 0.5:
T = T + 0.5
self.t += self.dt
T += 0
#correspond au retard du au virage
print("\n Done ! ")
time.sleep(1)
self.display()