Debuged

acsl-inha · Jan 9, 2023 · 1d93930 · 1d93930
1 parent 51a590f
commit 1d93930
Show file tree

Hide file tree

Showing 4 changed files with 32 additions and 31 deletions.
diff --git a/Senario3_simarr_MDCA.gif b/Senario3_simarr_MDCA.gif
diff --git a/mdca_wp_insert.py b/mdca_wp_insert.py
@@ -180,9 +180,14 @@ def split_segment(self,interval):
 
                 ip_num = int(self.UAVs[i].d[n]/interval)
 
+                n_ = n + ip_count
+
                 for j in range(1,ip_num):
 
-                    insert_loc = (1-j/ip_num)*self.UAVs[i].wp[n+ip_count].loc + (j/ip_num)*self.UAVs[i].wp[n+1+ip_count].loc
+                    insert_loc = (1-j/ip_num)*self.UAVs[i].wp[n_].loc + (j/ip_num)*self.UAVs[i].wp[n+1+ip_count].loc
+
+                    print(f"1 : {self.UAVs[i].wp[n+ip_count].loc}")
+                    print(f"2 : {self.UAVs[i].wp[n+ip_count].loc}")
 
                     insert_point = WP(insert_loc, i)
 
@@ -206,18 +211,18 @@ def run(self, avoidance=True, simul_arr=True):
         # self.insert_collision_points(collision_points)        
 
 
-        # for i in range(K):
+        for i in range(K):
 
-        #     wps = np.array([0,0])
+            wps = np.array([0,0])
 
-        #     for n in range(len(self.UAVs[i].wp)):
+            for n in range(len(self.UAVs[i].wp)):
 
-        #         wps = np.vstack((wps,self.UAVs[i].wp[n].loc))
+                wps = np.vstack((wps,self.UAVs[i].wp[n].loc))
 
-        #     plt.scatter(wps[1:,0],wps[1:,1])
-        #     plt.plot(wps[1:,0],wps[1:,1])
+            plt.scatter(wps[1:,0],wps[1:,1])
+            plt.plot(wps[1:,0],wps[1:,1])
 
-        # plt.show()
+        plt.show()
 
 
         t = []                          # t = [t^1, t^2, ... , t^K]
@@ -409,6 +414,9 @@ def run(self, avoidance=True, simul_arr=True):
             ti_1 = ti_opt
             ti_2 = np.append(np.zeros(1) ,ti_opt[:-1] )
 
+            # print(ti_1)
+            # print(ti_2)
+
             vi_opt = d[i] / ( ti_1 - ti_2)
 
             (self.UAVs[i]).v_set = vi_opt
@@ -423,29 +431,26 @@ def run(self, avoidance=True, simul_arr=True):
 
         ''' for printing result data '''
 
-        for uav in self.UAVs: # for i'th drone
+        cost = 0
 
-            ti = cp.Variable( uav.N - 1 )          # t^i = [t^i_1, t^i_2, ... , t^i_{N^i-1}]
-            di = uav.d                             # d^i = [d^i_1, d^i_2, ... , d^i_{N^i-1}]
+        for i in range(K):
 
-            t.append(ti)                           # t = [t^1, t^2, ... , t^K]
-            d.append(di)                           # d = [d^1, d^2, ... , d^K]
-            N.append(uav.N)                        # N = [N^1, N^2, ... , N^K]
+            ti_opt = t[i].value 
 
-            obj +=  ti[-1]                         # cost = Sum of arrival time of UAVs
+            cost += ti_opt[-1]
 
         if simul_arr: # additional cost function : simultaneus arrival cost
 
             for i in range(K):
                 for j in range(K-i-1):
 
-                    t_arr_i = t[i] 
-                    t_arr_j = t[i+j+1]
+                    t_arr_i = t[i].value 
+                    t_arr_j = t[i+j+1].value
 
-                    obj += 100*cp.sum_squares(  (t_arr_i[-1] - t_arr_j[-1])  )   # cost = sum( |t_i - t_j|^2 )
+                    cost += 100*(  (t_arr_i[-1] - t_arr_j[-1])**2  )   # cost = sum( |t_i - t_j|^2 )
 
 
-        print(f"Total cost :")
+        print(f"Total cost : {cost}")
 
 
 
@@ -555,6 +560,6 @@ def run(self, avoidance=True, simul_arr=True):
 
     uavs = [uav1,uav2,uav3]
 
-    mdca = MDCA(uavs,1,10,2)
+    mdca = MDCA(uavs,1,10,2,0.5)
 
     mdca.run()
diff --git a/sim_wp_insert.py b/sim_wp_insert.py
@@ -128,7 +128,7 @@ def update(timestep):
 
 
             sim = FuncAnimation(fig=fig1, func=update, frames=total_timesteps, interval=0.1) 
-            # sim.save('Senario3_simarr_MDCA.gif', fps=30, dpi=50)
+            sim.save('Senario3_simarr_MDCA_nosplited.gif', fps=30, dpi=50)
 
             traj.legend()
             plt.show()
@@ -172,8 +172,8 @@ def update(timestep):
                 vel.set_ylim(self.v_min-2,self.v_max+2)
                 vel.set_xlim(0, total_timesteps*self.delt)
 
-                vel.set_title(r"$\bf UAV^{(%d)}$"%(uav.num), loc="right",fontsize=30)
-                vel.set_xlabel(r"$\bf time\;(s)$",fontsize=15)
+                vel.set_title(r"$\bf UAV^{(%d)}$"%(uav.num), loc="right",fontsize=15)
+                vel.set_xlabel(r"$\bf time\;(s)$",fontsize=10)
                 vel.set_ylabel(r"$\bf Velocity\;(m/s)$",fontsize=15)
 
 
@@ -200,8 +200,8 @@ def update(timestep):
                     t_set = np.append( t_set, np.array([t_set[-1],total_timesteps*self.delt]) )
                     v_set = np.append( v_set, np.array([0,0]) )
 
-                vel.hlines(self.v_max, 0, t_set[-1], color="red", linewidth=6, linestyles='--', label=r"Max velocity")
-                vel.hlines(self.v_min, 0, t_set[-1], color="green", linewidth=6, linestyles='--', label=r"Min velocity")
+                vel.hlines(self.v_max, 0, t_set[-1], color="red", linewidth=2, linestyles='--', label=r"Max velocity")
+                vel.hlines(self.v_min, 0, t_set[-1], color="green", linewidth=2, linestyles='--', label=r"Min velocity")
                 vel.plot(t_set, v_set, color="black", linewidth=4, label=r"Velocity")
                 vel.legend(loc='lower left',prop={'size':10})
                 vel.grid()
@@ -318,8 +318,8 @@ def run(self):
     ### visualize ###
     Traj=True
     Reldist=False
-    Vel=False
+    Vel=True
     #################
 
-    SIM = Simulator(0.01,UAVs,1,10,3,0.1,avoidance=True,simul_arr=True,viz=[Traj,Reldist,Vel])
+    SIM = Simulator(0.01,UAVs,1,10,3,1,avoidance=True,simul_arr=True,viz=[Traj,Reldist,Vel])
     SIM.run()
diff --git a/uav_wp_insert.py b/uav_wp_insert.py
@@ -27,14 +27,10 @@ def calculate_dist(self):
 
         for k in range( len(self.wp) - 1 ):
 
-            print(f"k_{self.num} : {k}")
-
             d_k = np.linalg.norm(self.wp[k+1].loc - self.wp[k].loc)
 
             self.d = np.append( self.d, d_k )            
 
-        print(f"{self.num} total d len : {len(self.d)}")
-
 
     def calculate_trajec(self, delt):