debugging decMCTS plan to real plan failures (#49)

src/decmcts.py

@@ -353,8 +353,8 @@ def decmcts(contour_tracker,
                 if inds[i] < len(combined_schedule.trajs[i]):
                     arm = i
                     break
-            if arm == None:
-                Exception("something's gone wrong here - all arms complete but total is not")
+
+            assert arm != None, "something's gone wrong here - all arms complete but total is not"
 
             # Trajectory to add and time to wait for it
             traj = combined_schedule.trajs[arm][inds[arm]]
@@ -413,19 +413,30 @@ def decmcts(contour_tracker,
                         # TODO if this occurs, (logically possible) need to catch this case by moving to home
                         raise Exception("cannot go home or stay put - need to move out of way, look at TODO")
 
-            # create plan to next traj
-            travel_traj = plan.plan_to(traj.positions[0], arm)
-
             # add to plan
             traj_added = False
+            i=0
             while not traj_added:
 
+                # create plan to next traj
+                print("---")
+                travel_traj = plan.plan_to(traj.positions[0], arm)
+                print(travel_traj)
+                i+=1
+                if travel_traj is None:
+                    print(traj.positions[0])
+                    print(arm)
+                    print(plan.cumul_time)
+                    if i>10:
+                        raise Exception("Failed to create travel traj")
+
                 # try adding traj + contour to planner
                 if travel_traj is not None and plan.appendTrajectory([travel_traj, traj], arm) == 0:
                     # if success, break, else loop again
                     print("contour", traj.contour, "added to real plan")
                     contour_tracker.do_contour(traj.contour)
                     traj_added = True
+                    break
                 else:
                     print("deviating from decmcts plan")
                     last_js = plan.trajs[arm][-1].positions[-1]
@@ -435,6 +446,27 @@ def decmcts(contour_tracker,
                         # TODO if this occurs, (logically possible) need to catch this case by moving to home
                         raise Exception("cannot stay at last position, need to move out of way, look at TODO")
 
+                    # If the cumul time of current arm is greater than that of other arms, send other arms home
+                    if plan.cumul_time[arm][-1] == plan.len():
+                        print("current arm has the longest plan, meaning it is ahead of the other arms. Take other arms home now")
+                        # send other arms home
+                        for other_arm in plan.trajs:
+                            if other_arm == arm:
+                                continue
+                            # if already at home, stay ar home
+                            if all(plan.getLastJoint(other_arm) == home):
+                                print("Other arm is already home")
+                                raise Exception(" this should not occur")
+
+                            # Otherwise, go home
+                            gohome = plan.plan_to(env.home, other_arm)
+                            if gohome is not None and plan.appendTrajectory([gohome], other_arm)==0:
+                                print("current arm is", arm, ", taking other arm home")
+                                print(arm, "waiting, returning to home")
+                                athome = True
+                            else:
+                                raise Exception("Could not go home, need to address logic")
+
             contours_left -= 1 # to ensure loop exits
             inds[arm] += 1 # move to next contour next time
         break # Only do 1 layer for now

src/utils/plan.py

@@ -161,6 +161,13 @@ class Plan:
         return
 
 
+    def len(self):
+        """
+        Length of a plan is time of the arm with the longest plan
+        """
+        return max([a[-1] for a in self.cumul_time.values()])
+
+
 def load_plan(filename, gui=False):
     """
     load a saved plan