Commit 42fa6bb8 by Jayant Khatkar

### fix logic error for when only one arm has trajs left, cleanup (#5)

parent e56bc51e
 ... ... @@ -45,24 +45,32 @@ def execute_plan(plan, 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): print("----------") # Find which arm and which trajectory is next to be executed if arm_complete is not None: arm = 1 - arm_complete # if one arm complete, only need to do other else: arm = np.argmin([plan.cumul_time[0][i_r[0]], plan.cumul_time[1][i_r[1]]]) next_time = min(plan.cumul_time[0][i_r[0]], plan.cumul_time[1][i_r[1]])/move_speed next_time = plan.cumul_time[arm][i_r[arm]]/move_speed i_r[arm] += 1 # if last trajectory has already been sent # if the last trajectory for this arm has already been sent if i_r[arm] == len(plan.trajs[arm]): print("Last Trajectory sent, wait for completion") time.sleep(expected_end_time - time.time()) print("Complete") print("Last Trajectory for this arm has been sent") if arm_complete is None: arm_complete = arm # mark this arm as complete continue else: print("All arms done (unless Logic error), leaving main loop") break t = plan.trajs[arm][i_r[arm]] # wait till time to execute next trajectory # wait till it's time to execute the next trajectory rel_delay_arm = max(0, -relative_delay * (-1)**arm) sleep_time = next_time - (time.time() - s ) + rel_delay_arm if sleep_time > 0: ... ... @@ -70,7 +78,7 @@ def execute_plan(plan, sleep_time)) time.sleep(sleep_time) # Execute next Trajectory # Execute next trajectory print("Printing {}th trajectory of arm {}, which is contour {}".format( i_r[arm], arm, t.contour )) ... ... @@ -96,7 +104,7 @@ def execute_plan(plan, # Traj started now, measure delay delays[arm] = (time.time() - s ) - next_time print("Arm {} is behind by {} when executing {}th Traj".format( print("Arm {} is behind by {}s when executing {}th Traj".format( arm, delays[arm], i_r[arm])) ... ...
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!