logic correction for decmcts to real plan (#49)

src/decmcts.py

@@ -11,7 +11,7 @@ import pickle
 import copy
 
 
-home = [-pi/2, -pi/2, pi/4, -pi/4, -pi/2, -pi/4]
+home = [0, -pi/2, pi/4, -pi/4, -pi/2, -pi/4]
 SAFE_TIME = 2
 
 
@@ -49,7 +49,7 @@ class CombinedSchedule:
     combine decmcts rollouts
     """
 
-    def __init__(self, data, n_robots, joint2time_mul=0):
+    def __init__(self, data, n_robots, joint2time_mul=2.5):
         self.data = data
         self.col_table = data["collisions"]
         self.n = n_robots
@@ -293,6 +293,8 @@ def decmcts(contour_tracker,
                             )
                     if len(multiple_trajs)>0:
                         c_traj[cid][arm] = [tu.get_closest_traj(multiple_trajs,home)]
+                    else:
+                        c_traj[cid][arm] = []
                     update_savefile = True
 
         # Save file so don't have to recalculate on future runs
@@ -434,22 +436,14 @@ def decmcts(contour_tracker,
                     break
                 else:
                     print("deviating from decmcts plan")
-                    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:
-                        # Actually this is logically possible, but unlikely
-                        # 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 but failing, take other arms home to clear")
                         # send other arms home
                         for other_arm in plan.trajs:
                             if other_arm == arm:
                                 continue
-                            # if already at home, stay ar home
+                            # if already at home, stay at home
                             if all(plan.getLastJoint(other_arm) == home):
                                 print("Other arm is already home")
                                 raise Exception(" this should not occur")
@@ -463,6 +457,17 @@ def decmcts(contour_tracker,
                             else:
                                 raise Exception("Could not go home, need to address logic")
 
+                    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 stay at last position, need to move out of way, going home")
+                        gohome = plan.plan_to(env.home, arm)
+                        if gohome is not None and plan.appendTrajectory([gohome], arm)==0:
+                            print("taking arm {} home since it cannot stay put".format(arm))
+                        else:
+                            raise Exception("Cannot stay put or go home!")
+
+
             contours_left -= 1 # to ensure loop exits
             inds[arm] += 1 # move to next contour next time
         break # Only do 1 layer for now
@@ -472,7 +477,7 @@ def decmcts(contour_tracker,
 if __name__ == "__main__":
 
     # Read gcode file
-    model = "flexirex"
+    model = "flexirex_big"
     tracker = tu.get_tracker(model)
 
     # 2arms real calibration
@@ -485,8 +490,8 @@ if __name__ == "__main__":
                    env_desc2,
                    n_contours=10,
                    c_traj_save_file=c_traj_file,
-                   gui=False,
-                   mcts_nodes=10
+                   gui=True,
+                   mcts_nodes=50
                    )
     visualiseCombinedSchedule(cs)
     plan.visualise()