decmcts creating sensible plan for 3 arms (#43)

src/decmcts.py

@@ -153,23 +153,24 @@ class CombinedSchedule:
             self.end_times[robot].append(self.start_times[robot][-1] + ct.time[-1])
             return t_wait
 
-        raise Exception("function not implemented for more than two robots yet")
         # for n > 2
-        t_wait_last = 1
+        t_wait_last = -1
         while True:
-            t_wait = min([0] + [o[-1][1] for o in allowed_offsets])
+            t_wait = max(trav_time,-min([0] + [o[-1][1] for o in allowed_offsets]))
             if t_wait_last == t_wait:
                 Exception("Logic error, this should never happen")
             for ro in allowed_offsets:
-                while ro[-1][0] > t_wait:
+                while ro[-1][0] > -t_wait:
                     ro.pop()
-            valid = [ro[-1][0] <= t_wait <= ro[-1][1] for ro in allowed_offsets]
+            valid = [ro[-1][0] <= -t_wait <= ro[-1][1] for ro in allowed_offsets]
             if all(valid):
                 break
             t_wait_last = t_wait
 
-        # t_wait is a negative number
-        return trav_time - t_wait
+        self.trajs[robot].append(ct)
+        self.start_times[robot].append(self.end_times[robot][-1] + t_wait)
+        self.end_times[robot].append(self.start_times[robot][-1] + ct.time[-1])
+        return t_wait
 
 
 def visualiseCombinedSchedule(combined_schedule):

src/test_decmcts.py

@@ -4,7 +4,7 @@ Test Functions in decmcts.py
 from decmcts import *
 
 
-def test_reward(contour_tracker, combined_schedule, real_plan):
+def test_reward(contour_tracker, combined_schedule, real_plan, arm0=0, arm1=1):
     """
     test _reward function
     """
@@ -13,23 +13,23 @@ def test_reward(contour_tracker, combined_schedule, real_plan):
     print("orange horizonatl line means looking for start time above" + \
           ", black line means looking for a start time below\n")
     i,j = 1, 1
-    robot = 1
-    while i<len(p.start_times[0]) and j<len(p.start_times[1]):
+    robot = arm1
+    while i<len(p.start_times[arm0]) and j<len(p.start_times[arm1]):
         # run for a pair of trajectories
-        ct_r0, ct_r1 = p.trajs[0][i-1], p.trajs[1][j-1]
-        t_wait = p.start_times[1][j] - p.start_times[0][i]
+        ct_r0, ct_r1 = p.trajs[arm0][i-1], p.trajs[arm1][j-1]
+        t_wait = p.start_times[arm1][j] - p.start_times[arm0][i]
 
         print("({},{})".format(i,j))
 
         tu.vis_col_region(ct_r0, ct_r1, env, res=1, chosen_time=t_wait, safe_time = SAFE_TIME)
 
         # increment ct we're up to
-        if p.end_times[0][i] <= p.end_times[1][j]:
+        if p.end_times[arm0][i] <= p.end_times[arm1][j]:
             i +=1
-            robot = 0
+            robot = arm0
         else:
             j +=1
-            robot = 1
+            robot = arm1
 
         user_input = input('enter for next, "p" for show plan and next, or "q" for quit: ')
         if user_input =='q':
@@ -55,7 +55,7 @@ if __name__ == "__main__":
     # 2arms real calibration
     env_desc2 = tu.read_env('calibrations/r1_tforms.yaml',
                             'calibrations/r2_tforms.yaml')
-    env = tu.SimEnv(env_desc=env_desc2, gui=False, home=home)
+    env = tu.SimEnv(env_desc=env_desc3, gui=False, home=home)
 
     # decmcts plan first n contours
     c_traj_file = "gcode/" + model + ".c_traj"
@@ -66,4 +66,9 @@ if __name__ == "__main__":
                    )
 
     # run test test_reward
-    test_reward(tracker, cs, plan)
+    print("Compare arms 0 and 1")
+    test_reward(tracker, cs, plan, arm0=0, arm1=1)
+    print("Compare arms 1 and 2")
+    test_reward(tracker, cs, plan, arm0=1, arm1=2)
+    print("Compare arms 0 and 2")
+    test_reward(tracker, cs, plan, arm0=0, arm1=2)