some bad code to be able to continue a print from the middle

src/main2.py

@@ -23,12 +23,12 @@ def execute_plan(plan,
         r2_con,
         move_speed=1,
         retry_time=0.1,
+        last_completed_trajs=[0,0], # not implemented correctly - dangerous - use carefully
         max_traj_retries=20):
     """
     Executes a plan on MoveIt
 
     Single arm only
-    skips the first n trajectories in the plan
     """
     # calibration correction of all trajectories
     for arm in range(len(plan.trajs)):
@@ -38,9 +38,11 @@ def execute_plan(plan,
             plan.trajs[arm][i] = calibration_correction(speed_multiplier(t, 1/move_speed),"/r{}/robot_description".format(arm+1))
             JTraj2ROS(plan.trajs[arm][i]) # also check for double time stamps and positions tolerances when removing
 
+    i_r = copy.copy(last_completed_trajs) # index of which traj we're up to for each arm
+
     # plan to the start position of the plan
-    r1_con.group.go(plan.trajs[0][0].positions[0])
-    r2_con.group.go(plan.trajs[1][0].positions[0])
+    r1_con.group.go(plan.trajs[0][i_r[0]+1].positions[0])
+    r2_con.group.go(plan.trajs[1][i_r[1]+1].positions[0])
     controllers = [r1_con, r2_con]
 
     # execute trajectories in plan
@@ -48,10 +50,12 @@ def execute_plan(plan,
     print("EXPECTED PLAN LENGTH: {}".format(plan_len))
 
     s = time.time()
+    if i_r[0]>0 or i_r[1]>0:
+        start_delay = min(plan.cumul_time[0][i_r[0]], plan.cumul_time[1][i_r[1]])/move_speed
+        s = s - start_delay
     expected_end_time = s + plan_len
     delays = [0,0] # measure delays of each arm
     relative_delay = 0 # how far arm 0 is behind arm 2
-    i_r = [0,0] # index of which traj we're up to for each arm
     arm_complete= None
     while time.time() < expected_end_time + max(delays):
 
@@ -106,13 +110,14 @@ def execute_plan(plan,
             i_r[arm]))
         relative_delay = delays[0]-delays[1]
         print("RELATIVE DELAY: arm 0 is {}s behind".format(relative_delay))
+        print("Time remaining is {}s".format(expected_end_time - time.time()))
 
     return
 
 
 if __name__ == '__main__':
-    plan2arms = load_plan('CuteOcto_contour')
+    plan2arms = load_plan('y_short')
 
     r1_con = Controller(robot=1, disable_extruder=True)
     r2_con = Controller(robot=2, disable_extruder=True)
-    execute_plan(plan2arms, r1_con, r2_con, move_speed=0.2)
+    execute_plan(plan2arms, r1_con, r2_con, move_speed=0.8, last_completed_trajs=[59,64])