fix logic error in decmcts logic (#70)

src/bulk_test.py

@@ -60,6 +60,7 @@ def batch_run(
                 try:
                     s_time = time.time()
                     plan = decmcts.decmcts(
+                            model,
                             env,
                             tracker,
                             nth_layer=nth_layer,
@@ -213,11 +214,11 @@ if __name__ == '__main__':
 
         for i in range(len(models)):
             models[i] = models[i][:-1]
-   # batch_run(
-   #     models,
-   #     'bulk.log',
-   #     'bulk.stats',
-   #     'plans',
-   #     50,
-   #     2000)
-    plot_comparison("bulk.stats")
+    batch_run(
+        models,
+        'bulk.log',
+        'bulk.stats',
+        'plans',
+        50,
+        2000)
+   # plot_comparison("bulk.stats")

src/decmcts.py

@@ -149,14 +149,14 @@ class CombinedSchedule:
         # easy for n = 2
         if self.n == 2:
             # if collision forces this contour to start after other arm is finished their last contour, reject
-            if t_0 + allowed_offsets[0][-1][1] > self.end_times[1-robot][-1] \
+            if t_0 - allowed_offsets[0][-1][1] > self.end_times[1-robot][-1] \
                     and not (self.end_times[robot][-1] == 0 and self.end_times[1-robot][-1] == 0) \
                     and not force:
                 # reject contour if it cannot be parallelised with current plan
                 self.start_next_contour[robot] = self.end_times[1-robot][-1] + self.safe_time
                 return False
 
-            t_wait = max(trav_time, -min(0, allowed_offsets[0][-1][1]))
+            t_wait = -min(0, allowed_offsets[0][-1][1])
             self.trajs[robot].append(ct)
             self.start_times[robot].append(t_0 + t_wait)
             self.end_times[robot].append(self.start_times[robot][-1] + ct.time[-1])
@@ -320,7 +320,7 @@ def decmcts_layer(
             with open(save_collisions, 'rb') as f:
                 data['collisions'] = pickle.load(f)
         except:
-            print("no save file found, will generate new one")
+            print("save {}  not file found, will generate new one".format(save_collisions))
 
     # Update Data for trees to include latest contours
     s_layer = time.time()
@@ -374,10 +374,9 @@ def decmcts_layer(
         for tree in trees:
             tree.grow(nsims=1, depth=1e6)
 
-        if save_collisions:
+        if i%100==0:
             with open(save_collisions, 'wb') as f:
                 pickle.dump(data['collisions'], f)
-        if i%1==0:
             e = time.time()
             print("Completed {} nodes in {:.1f}s".format(i+1, e-s))
 
@@ -573,7 +572,7 @@ def decmcts(
                 sim_env,
                 plan=plan,
                 finish_home=fin_home,
-                save_collisions=model + str(i) + ".collisions",
+                save_collisions= "col_save_files/" + model + str(i) + ".collisions",
                 mcts_nodes=nodes_per_layer
                 )
 
@@ -586,7 +585,7 @@ def decmcts(
 if __name__ == "__main__":
 
     # Read gcode file
-    model = "ir_sensor_main-ascii"
+    model = "orb_half_2-ascii"
     tracker = tu.get_tracker(model)
 
     # 3arms simulation
@@ -601,4 +600,4 @@ if __name__ == "__main__":
     sim_env = tu.SimEnv(env_desc=env_desc2, gui=True, home=home)
 
     # decmcts plan first n contours
-    plan = decmcts(model, sim_env, tracker, nth_layer=50, interactive=True)
+    plan = decmcts(model, sim_env, tracker, nth_layer=50)