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 anim = figure.createAnimation(dt) time.sleep(1) sub = figure.create(UnityFigure.OBJECT_3D_SUBMARINE, 0, -0.4, 0, dimX=5, dimY=5, dimZ=5) anim.addObject(sub) sensor = figure.create(UnityFigure.OBJECT_3D_CUBE, 0, -0.5, 0, dimX=0.2, dimY=0.2, dimZ=1, color=UnityFigure.COLOR_BLACK) anim.addObject(sensor) # Track the submarine with the camera # sub1.track()
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 # z: the new z coordinate boat.updatePosition(1, 1, 1) time.sleep(1) # Update of an object's rotation # Parameters: # x: the new x euler angle # y: the new y euler angle # z: the new z euler angle
#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, color=UnityFigure.COLOR_YELLOW)) anim.addObject(AUVs[ind_auv]) time.sleep(0.1) for coord in [[0, 0], [50, 0], [25, 25]]: boue = figure.create(UnityFigure.OBJECT_3D_CUBE, -coord[1], -2, coord[0], dimX=0.2, dimY=5, dimZ=0.2, color=UnityFigure.COLOR_RED)
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): for ind_auv, auv in enumerate(AUVs): if t%10.0 < 0.1: sys.stdout.write("- t = {}s\r".format(t)) x, y, z, rx, ry, rz = parts[ind_auv, int(t/dt)] anim.appendFrame(auv, x=x, y=y, z=z, rx=rx, ry=ry, rz=rz) time.sleep(1) ### Display ###
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()