Commit ca5d090d authored by Jayant Khatkar's avatar Jayant Khatkar

fix logic errors in Combined Scheduler

parent 33ce6391
......@@ -143,20 +143,20 @@ class CombinedSchedule:
ao = copy.deepcopy(self.col_table[ct, ct_r])
# apply t_diff and add allowed offset for when other robot is done
allowed_offsets.append([(-1e6, t_0 - self.end_times[r][-1]-self.safe_time)] + \
allowed_offsets.append([(-1e6, t_0 - self.end_times[r][-1]-10)] + \
[(a+t_diff, b+t_diff) for a,b in ao if a+t_diff<=0])
# easy for n = 2
if self.n == 2:
# if collision forces this contour to start after other arm is finished their last contour, reject
if t_0 + allowed_offsets[0][-1][1] > self.end_times[1-robot][-1] \
if t_0 - allowed_offsets[0][-1][1] > self.end_times[1-robot][-1] \
and not (self.end_times[robot][-1] == 0 and self.end_times[1-robot][-1] == 0) \
and not force:
# reject contour if it cannot be parallelised with current plan
self.start_next_contour[robot] = self.end_times[1-robot][-1] + self.safe_time
return False
t_wait = max(trav_time, -min(0, allowed_offsets[0][-1][1]))
t_wait = -min(0, allowed_offsets[0][-1][1])
self.trajs[robot].append(ct)
self.start_times[robot].append(t_0 + t_wait)
self.end_times[robot].append(self.start_times[robot][-1] + ct.time[-1])
......@@ -250,6 +250,7 @@ def createCombinedSchedule(data, states):
for r in rs:
if len(cts_left[r]) != 0:
robot = r
break
# if rejected, switch robots (if possible) and force next contour
else:
......@@ -575,7 +576,7 @@ def decmcts(
if __name__ == "__main__":
# Read gcode file
model = "Top-ascii"
model = "prop_v2-ascii"
tracker = tu.get_tracker(model)
# 3arms simulation
......@@ -590,4 +591,4 @@ if __name__ == "__main__":
sim_env = tu.SimEnv(env_desc=env_desc2, gui=True, home=home)
# decmcts plan first n contours
plan = decmcts(model, sim_env, tracker, nth_layer=50, interactive=False, nodes_per_layer=1000)
plan = decmcts(model, sim_env, tracker, nth_layer=50, interactive=False, nodes_per_layer=3000)
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