Commit a6eeb104 authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

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!
Please register or to comment