updated scheduler works but is slow, need to run wider tests (#70)

src/decmcts.py

@@ -72,6 +72,7 @@ class CombinedSchedule:
         self.end_times = [[0] for i in range(self.n)]
         self.t_mul = joint2time_mul
         self.safe_time = SAFE_TIME # safety buffer for Collisions
+        self.start_next_contour = [0]*self.n
 
 
     def planLenOrder(self):
@@ -98,7 +99,7 @@ class CombinedSchedule:
         return max([self.end_times[r][-1] for r in range(self.n)])
 
 
-    def append(self, ct, robot):
+    def append(self, ct, robot, force=False):
         """
         add a contour trajectory to the CombinedSchedule
         """
@@ -108,11 +109,11 @@ class CombinedSchedule:
             self.trajs[robot].append(ct)
             self.start_times[robot].append(self.end_times[robot][-1])
             self.end_times[robot].append(self.start_times[robot][-1] + ct.time[-1])
-            return 0
+            return True
 
         # travel time based on joint_diff
         trav_time = self.t_mul * joint_diff(ct.positions[-1], self.lastJointPos(robot))
-        t_0 = self.end_times[robot][-1] + trav_time
+        t_0 = self.start_next_contour[robot] + trav_time
 
         # for every other robot, get collision free time offsets
         allowed_offsets = []
@@ -147,11 +148,22 @@ class CombinedSchedule:
 
         # easy for n = 2
         if self.n == 2:
-            t_wait = max(trav_time,-min(0,allowed_offsets[0][-1][1]))
+            t_wait = max(trav_time, -min(0, allowed_offsets[0][-1][1]))
+
+            #if this contour starts after other arm is finished
+            # and it isnt the first contour of the plan
+            # and it isn't being added regardless
+            if t_0 + t_wait > 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
             self.trajs[robot].append(ct)
-            self.start_times[robot].append(self.end_times[robot][-1] + t_wait)
+            self.start_times[robot].append(t_0 + t_wait)
             self.end_times[robot].append(self.start_times[robot][-1] + ct.time[-1])
-            return t_wait
+            self.start_next_contour[robot] = self.end_times[robot][-1]
+            return True
 
         # for n > 2
         t_wait_last = -1
@@ -211,27 +223,54 @@ def createCombinedSchedule(data, states):
     plan = CombinedSchedule(data, len(states))
     cts_left = [copy.copy(s) for s in states.values()]
 
+    if len(cts_left)>2:
+        raise Exception("New CombinedScheduler does not support >2 arms")
+
     done_contours = []
     t_used = 0
 
+    robot = 0
+    force=False
+    assert(len(cts_left[0])>0)
     while sum([len(r) for r in cts_left])>0:
-
-        rs = plan.planLenOrder()
-        for r in rs:
-            if len(cts_left[r]) != 0:
-                robot = r
-                break
-
         # get next cts whose contour hasn't been done
-        while len(cts_left[robot]) > 0:
-            ct = cts_left[robot].popleft()
-
-            if ct.contour not in done_contours:
-                # place contour in plan and sum wasted time
-                done_contours.append(ct.contour)
-                plan.append(ct, robot)
-                t_used += ct.time[-1]
-                break
+        i = 0
+        while i < len(cts_left[robot]):
+            ct = cts_left[robot][i]
+
+            if ct.contour in done_contours:
+                del cts_left[robot][i]
+                if len(cts_left[robot]) == 0:
+                    robot = 1-robot
+                    break
+                continue
+            else:
+                # try adding
+                if plan.append(ct, robot, force=force):
+
+                    # log done contours and time
+                    done_contours.append(ct.contour)
+                    t_used += ct.time[-1]
+                    force = False
+
+                    # get next arm to add to
+                    rs = plan.planLenOrder()
+                    for r in rs:
+                        if len(cts_left[r]) != 0:
+                            robot = r
+                            break
+                    break
+
+                # if tried everything, switch robots
+                elif i == len(cts_left[robot])-1:
+                    # TODO work with more than two robots
+                    robot = 1-robot
+                    force=True
+                    break
+                else:
+                    # try next contour in list
+                    i+=1
+
     return plan, t_used
 
 
@@ -241,7 +280,7 @@ def reward(data, states):
     """
     plan, t_used = createCombinedSchedule(data, states)
     # return t_used/(plan.len()*len(states)), plan # FOR TESTING
-    return t_used/(plan.len()*len(states))
+    return t_used/plan.len()
 
 
 def decmcts_layer(
@@ -332,7 +371,7 @@ def decmcts_layer(
         # grow the trees
         for tree in trees:
             tree.grow(nsims=1, depth=1e6)
-        if i%100==0 and i!=0:
+        if i%1==0 and i!=0:
             e = time.time()
             print("Completed {} nodes in {:.2g}s".format(i, e-s))
 
@@ -382,31 +421,31 @@ def decmcts_layer(
             joint_diff(home, traj.positions[0])
 
         # if there's enough time to get home in the wait period, do that
-        athome = list(plan.getLastJoint(arm)) == home
-        additional_time_spent = 0
-        if time_last2home + time_home2next <= decmcts_wait_time:
-
-            # try going to home pos
-            while not athome:
-                gohome = plan.plan_to(env.home, arm)
-
-                # if no travel found, stay at last pos
-                if gohome is not None and plan.appendTrajectory([gohome], arm)==0:
-                    print(arm, "returning to home to wait")
-                    athome = True
-                else:
-                    print("There's enough time to get home but couldn't find a path - Unfortunate")
-                    print("Staying put for {}s".format(default_wait))
-                    last_js = plan.trajs[arm][-1].positions[-1]
-                    stayput = tu.JTrajectory([last_js, last_js],[0,default_wait], arm)
-                    if plan.appendTrajectory([stayput], arm)!=0:
-                        print("cannot go home or stay put Logically possible but unlikely - TODO")
-                        raise Exception("cannot go home or stay put Logically possible but unlikely - TODO")
-                    additional_time_spent += default_wait
-
-                    if time_last2home + additional_time_spent + default_wait + time_home2next > decmcts_wait_time:
-                        # can't go home anymore
-                        break
+        #athome = list(plan.getLastJoint(arm)) == home
+        #additional_time_spent = 0
+        #if time_last2home + time_home2next <= decmcts_wait_time:
+
+        #    # try going to home pos
+        #    while not athome:
+        #        gohome = plan.plan_to(env.home, arm)
+
+        #        # if no travel found, stay at last pos
+        #        if gohome is not None and plan.appendTrajectory([gohome], arm)==0:
+        #            print(arm, "returning to home to wait")
+        #            athome = True
+        #        else:
+        #            print("There's enough time to get home but couldn't find a path - Unfortunate")
+        #            print("Staying put for {}s".format(default_wait))
+        #            last_js = plan.trajs[arm][-1].positions[-1]
+        #            stayput = tu.JTrajectory([last_js, last_js],[0,default_wait], arm)
+        #            if plan.appendTrajectory([stayput], arm)!=0:
+        #                print("cannot go home or stay put Logically possible but unlikely - TODO")
+        #                raise Exception("cannot go home or stay put Logically possible but unlikely - TODO")
+        #            additional_time_spent += default_wait
+
+        #            if time_last2home + additional_time_spent + default_wait + time_home2next > decmcts_wait_time:
+        #                # can't go home anymore
+        #                break
 
         # add to plan
         traj_added = False

src/tests/test_decmcts.py

@@ -43,32 +43,17 @@ def test_reward(contour_tracker, combined_schedule, real_plan, arm0=0, arm1=1):
 if __name__ == "__main__":
 
     # Read gcode file
-    model = "flexirex_big"
+    model = "Kitchen_sponge_holder_small-ascii"
     tracker = tu.get_tracker(model)
 
-    # 3arms simulation
-    env_desc3 = tu.PlanningEnv([[0,0,1,0],[0,0,1,0],[0,0,0,1]],
-                       [[0.9, -0.35, 0],[-0.8, -0.35, 0.001],[0, 0.5, 0]],
-                       [[0,0,0,1]]*3,
-                       [[0.15,0,0]]*3) # WARNING - also edit urdf
-
     # 2arms real calibration
     env_desc2 = tu.read_env('calibrations/r1_tforms.yaml',
                             'calibrations/r2_tforms.yaml')
-    env = tu.SimEnv(env_desc=env_desc3, gui=False, home=home)
+    env = tu.SimEnv(env_desc=env_desc2, gui=True, home=home)
 
     # decmcts plan first n contours
     c_traj_file = "gcode/" + model + ".c_traj"
-    cs, plan = decmcts(tracker,
-                   env,
-                   n_layers=1,
-                   mcts_nodes=1000
-                   )
+    cs, plan = decmcts(env, tracker, nth_layer=50, nodes_per_layer=1000)
 
     # run test test_reward
-    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)