Commit a6eeb104 by Jayant Khatkar

### watch over a minimize relative delay, cleanup (#5)

parent ec4b8b6e
 ... @@ -40,45 +40,60 @@ def execute_plan(plan, ... @@ -40,45 +40,60 @@ def execute_plan(plan, s = time.time() s = time.time() expected_end_time = s + plan_len expected_end_time = s + plan_len i_r = [0,0] delays = [0,0] # measure delays of each arm while time.time() < expected_end_time: 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 while time.time() < expected_end_time + max(delays): print("----------") # Find which arm and which trajectory is next to be executed # Find which arm and which trajectory is next to be executed next_arm = np.argmin([plan.cumul_time[0][i_r[0]], plan.cumul_time[1][i_r[1]]]) 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 = min(plan.cumul_time[0][i_r[0]], plan.cumul_time[1][i_r[1]])/move_speed i_r[next_arm] += 1 i_r[arm] += 1 # if last trajectory has already been sent # if last trajectory has already been sent if i_r[next_arm] == len(plan.trajs[next_arm]): if i_r[arm] == len(plan.trajs[arm]): print("Last Trajectory sent, wait for completion") print("Last Trajectory sent, wait for completion") time.sleep(expected_end_time - time.time()) time.sleep(expected_end_time - time.time()) print("Complete") print("Complete") break break t = plan.trajs[next_arm][i_r[next_arm]] t = plan.trajs[arm][i_r[arm]] # wait till time to execute next trajectory # wait till time to execute next trajectory sleep_time = next_time - (time.time() - s ) rel_delay_arm = max(0, -relative_delay * (-1)**arm) sleep_time = next_time - (time.time() - s ) + rel_delay_arm if sleep_time > 0: if sleep_time > 0: print("Sleeping for {}s until time for next traj".format( print("Sleeping for {}s until time for next traj".format( sleep_time sleep_time)) )) time.sleep(sleep_time) time.sleep(sleep_time) # Execute next Trajectory # Execute next Trajectory print("Printing {}th trajectory of arm {}, which is contour {}".format( print("Printing {}th trajectory of arm {}, which is contour {}".format( i_r[next_arm], next_arm, t.contour i_r[arm], arm, t.contour )) )) if t.contour is not None and contours is not None: if t.contour is not None and contours is not None: traj2send = speed_multiplier(t, 1/contour_speed) traj2send = speed_multiplier(t, 1/contour_speed) while not controllers[next_arm].exec_ctraj( while not controllers[arm].exec_ctraj( traj2send, traj2send, contour=contours[t.contour], contour=contours[t.contour], wait=False): wait=False): # while loop retries if too early to execute the first time pass pass else: else: traj2send = speed_multiplier(t, 1/move_speed) traj2send = speed_multiplier(t, 1/move_speed) while not controllers[next_arm].exec_ctraj(traj2send, wait=False): while not controllers[arm].exec_ctraj(traj2send, wait=False): pass pass # Traj started now, measure delay delays[arm] = (time.time() - s ) - next_time print("Arm {} is behind by {} when executing {}th Traj".format( arm, delays[arm], i_r[arm])) relative_delay = delays[0]-delays[1] print("RELATIVE DELAY: arm 0 is {}s behind".format(relative_delay)) return return ... ...
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!