Commit 42fa6bb8 authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

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

parent e56bc51e
...@@ -45,24 +45,32 @@ def execute_plan(plan, ...@@ -45,24 +45,32 @@ def execute_plan(plan,
delays = [0,0] # measure delays of each arm delays = [0,0] # measure delays of each arm
relative_delay = 0 # how far arm 0 is behind arm 2 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 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): while time.time() < expected_end_time + max(delays):
print("----------") print("----------")
# Find which arm and which trajectory is next to be executed # Find which arm and which trajectory is next to be executed
arm = np.argmin([plan.cumul_time[0][i_r[0]], plan.cumul_time[1][i_r[1]]]) if arm_complete is not None:
next_time = min(plan.cumul_time[0][i_r[0]], plan.cumul_time[1][i_r[1]])/move_speed 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 = plan.cumul_time[arm][i_r[arm]]/move_speed
i_r[arm] += 1 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]): if i_r[arm] == len(plan.trajs[arm]):
print("Last Trajectory sent, wait for completion") print("Last Trajectory for this arm has been sent")
time.sleep(expected_end_time - time.time()) if arm_complete is None:
print("Complete") arm_complete = arm # mark this arm as complete
break continue
else:
print("All arms done (unless Logic error), leaving main loop")
break
t = plan.trajs[arm][i_r[arm]] 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) rel_delay_arm = max(0, -relative_delay * (-1)**arm)
sleep_time = next_time - (time.time() - s ) + rel_delay_arm sleep_time = next_time - (time.time() - s ) + rel_delay_arm
if sleep_time > 0: if sleep_time > 0:
...@@ -70,7 +78,7 @@ def execute_plan(plan, ...@@ -70,7 +78,7 @@ def execute_plan(plan,
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[arm], arm, t.contour i_r[arm], arm, t.contour
)) ))
...@@ -96,7 +104,7 @@ def execute_plan(plan, ...@@ -96,7 +104,7 @@ def execute_plan(plan,
# Traj started now, measure delay # Traj started now, measure delay
delays[arm] = (time.time() - s ) - next_time 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, arm,
delays[arm], delays[arm],
i_r[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!
Please register or to comment