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 __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)
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()
# Function for the command with supervisor - alpha the time step between the follower and followed
def followSupervisor(alpha):
w = np.array([[Lx * math.sin(0.1 * (t - alpha))],
[Ly * math.cos(0.1 * (t - alpha))]])
dw = np.array([[Lx * 0.1 * math.cos(0.1 * (t - alpha))],
[-Ly * 0.1 * math.sin(0.1 * (t - alpha))]])
return w, dw
if __name__ == "__main__":
# Initialization of the figure
# Parameters:
# figType: the dimension of the figure (see UnityFigure.FIGURE_*)
# scene: the scene to be loaded (see UnityFigure.SCENE_*)
figure = UnityFigure(UnityFigure.FIGURE_3D, UnityFigure.SCENE_EMPTY)
time.sleep(1)
# Initialization variables
dt = 0.16
xa = np.array([[10], [0], [1], [1]])
ua = np.array([[0], [0]])
xb = np.array([[0], [0], [1], [2]])
dxa, dxb = 0, 0
dza, dzb = 0, 0
s = (4, int(20 / dt) + 1)
l = 6
Lx = 15
Ly = 7
# Creation of a submarine and a black box which represents the sensor
from PyUnityVibes.UnityFigure import UnityFigure
import time
if __name__ == "__main__":
# Initialization of the figure
# Parameters:
# figType: the dimension of the figure (see UnityFigure.FIGURE_*)
# scene: the scene to be loaded (see UnityFigure.SCENE_*)
figure = UnityFigure(UnityFigure.FIGURE_3D, UnityFigure.SCENE_EMPTY)
time.sleep(1)
# Creation of an object
# Parameters:
# meshType: the mesh of the object (see UnityFigure.OBJECT_*)
# x: the x coordinate
# y: the y coordinate
# z: the z coordinate
# [rotX: the x euler angle]
# [rotY: the y euler angle]
# [rotZ: the z euler angle]
# [dimX: the x scale]
# [dimY: the y scale]
# [dimZ: the z scale]
# [color: the color of the mesh (see UnityFigure.COLOR_*)]
boat = figure.create(UnityFigure.OBJECT_3D_BOAT, 0, 0, 0)
time.sleep(1)
# Update of an object's position
# Parameters:
# x: the new x coordinate
# y: the new y coordinate
N, T, dt = heading.split(";")
N = int(N[2:]) # on lit "N=100"
T = int(T[2:]) # ------ "T=1200"
dt= float(dt[3:]) # ------ "dt=0.1"
parts = np.zeros((N, int(T/dt), 6))
for line in data:
data_line = line.split(";")
ID, t, x, y, z, rx, ry, rz = [float(el) for el in data_line]
ID = int(ID)
parts[ID, int(T/dt)-1] = x, y, z, rx, ry , rz
### Initialisation de Unity ###
figure = UnityFigure(UnityFigure.FIGURE_3D, UnityFigure.SCENE_EMPTY)
time.sleep(1)
anim = figure.createAnimation(dt)
time.sleep(1)
### Creation des AUVs ###
AUVs = []
for ind_auv in range(N):
AUVs.append(figure.create(UnityFigure.OBJECT_3D_SUBMARINE, 0, -0.4, 0, dimX=5, dimY=5, dimZ=5))
anim.addObject(AUVs[ind_auv])
time.sleep(0.1)
time.sleep(1)
### Calcul des frames successives ###
for t in np.arange(0, T, dt):
dt = float(dt[3:]) # ------ "dt=0.1"
parts = np.zeros((N, int(T / dt), 6))
for line in data:
data_line = line.split(";")
ID, t, x, y, z, rx, ry, rz = [float(el) for el in data_line]
#print(ID, t, x, y, z)
ID = int(ID)
#print(int(t/dt)-1)
parts[ID, int(t / dt) - 1] = x, y, z, rx, ry, rz
#print(parts[0])
### Initialisation de Unity ###
figure = UnityFigure(UnityFigure.FIGURE_3D, UnityFigure.SCENE_EMPTY)
time.sleep(1)
anim = figure.createAnimation(dt)
time.sleep(1)
### Creation des AUVs ###
AUVs = []
for ind_auv in range(N):
AUVs.append(
figure.create(UnityFigure.OBJECT_3D_SUBMARINE,
0,
-0.4,
0,
dimX=1,
dimY=1,
dimZ=5,
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()