Commit a6eeb104 authored by Jayant Khatkar's avatar Jayant Khatkar

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

parent ec4b8b6e
......@@ -40,45 +40,60 @@ def execute_plan(plan,
s = time.time()
expected_end_time = s + plan_len
i_r = [0,0]
while time.time() < expected_end_time:
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
while time.time() < expected_end_time + max(delays):
print("----------")
# 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
i_r[next_arm] += 1
i_r[arm] += 1
# 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")
time.sleep(expected_end_time - time.time())
print("Complete")
break
t = plan.trajs[next_arm][i_r[next_arm]]
t = plan.trajs[arm][i_r[arm]]
# 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:
print("Sleeping for {}s until time for next traj".format(
sleep_time
))
sleep_time))
time.sleep(sleep_time)
# Execute next Trajectory
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:
traj2send = speed_multiplier(t, 1/contour_speed)
while not controllers[next_arm].exec_ctraj(
while not controllers[arm].exec_ctraj(
traj2send,
contour=contours[t.contour],
wait=False):
# while loop retries if too early to execute the first time
pass
else:
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
# 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
......
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