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()