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

updated scheduler works but is slow, need to run wider tests (#70)

parent d21d2e44
...@@ -72,6 +72,7 @@ class CombinedSchedule: ...@@ -72,6 +72,7 @@ class CombinedSchedule:
self.end_times = [[0] for i in range(self.n)] self.end_times = [[0] for i in range(self.n)]
self.t_mul = joint2time_mul self.t_mul = joint2time_mul
self.safe_time = SAFE_TIME # safety buffer for Collisions self.safe_time = SAFE_TIME # safety buffer for Collisions
self.start_next_contour = [0]*self.n
def planLenOrder(self): def planLenOrder(self):
...@@ -98,7 +99,7 @@ class CombinedSchedule: ...@@ -98,7 +99,7 @@ class CombinedSchedule:
return max([self.end_times[r][-1] for r in range(self.n)]) return max([self.end_times[r][-1] for r in range(self.n)])
def append(self, ct, robot): def append(self, ct, robot, force=False):
""" """
add a contour trajectory to the CombinedSchedule add a contour trajectory to the CombinedSchedule
""" """
...@@ -108,11 +109,11 @@ class CombinedSchedule: ...@@ -108,11 +109,11 @@ class CombinedSchedule:
self.trajs[robot].append(ct) self.trajs[robot].append(ct)
self.start_times[robot].append(self.end_times[robot][-1]) self.start_times[robot].append(self.end_times[robot][-1])
self.end_times[robot].append(self.start_times[robot][-1] + ct.time[-1]) self.end_times[robot].append(self.start_times[robot][-1] + ct.time[-1])
return 0 return True
# travel time based on joint_diff # travel time based on joint_diff
trav_time = self.t_mul * joint_diff(ct.positions[-1], self.lastJointPos(robot)) trav_time = self.t_mul * joint_diff(ct.positions[-1], self.lastJointPos(robot))
t_0 = self.end_times[robot][-1] + trav_time t_0 = self.start_next_contour[robot] + trav_time
# for every other robot, get collision free time offsets # for every other robot, get collision free time offsets
allowed_offsets = [] allowed_offsets = []
...@@ -147,11 +148,22 @@ class CombinedSchedule: ...@@ -147,11 +148,22 @@ class CombinedSchedule:
# easy for n = 2 # easy for n = 2
if self.n == 2: if self.n == 2:
t_wait = max(trav_time,-min(0,allowed_offsets[0][-1][1])) t_wait = max(trav_time, -min(0, allowed_offsets[0][-1][1]))
#if this contour starts after other arm is finished
# and it isnt the first contour of the plan
# and it isn't being added regardless
if t_0 + t_wait > 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
self.trajs[robot].append(ct) self.trajs[robot].append(ct)
self.start_times[robot].append(self.end_times[robot][-1] + t_wait) self.start_times[robot].append(t_0 + t_wait)
self.end_times[robot].append(self.start_times[robot][-1] + ct.time[-1]) self.end_times[robot].append(self.start_times[robot][-1] + ct.time[-1])
return t_wait self.start_next_contour[robot] = self.end_times[robot][-1]
return True
# for n > 2 # for n > 2
t_wait_last = -1 t_wait_last = -1
...@@ -211,27 +223,54 @@ def createCombinedSchedule(data, states): ...@@ -211,27 +223,54 @@ def createCombinedSchedule(data, states):
plan = CombinedSchedule(data, len(states)) plan = CombinedSchedule(data, len(states))
cts_left = [copy.copy(s) for s in states.values()] cts_left = [copy.copy(s) for s in states.values()]
if len(cts_left)>2:
raise Exception("New CombinedScheduler does not support >2 arms")
done_contours = [] done_contours = []
t_used = 0 t_used = 0
robot = 0
force=False
assert(len(cts_left[0])>0)
while sum([len(r) for r in cts_left])>0: while sum([len(r) for r in cts_left])>0:
rs = plan.planLenOrder()
for r in rs:
if len(cts_left[r]) != 0:
robot = r
break
# get next cts whose contour hasn't been done # get next cts whose contour hasn't been done
while len(cts_left[robot]) > 0: i = 0
ct = cts_left[robot].popleft() while i < len(cts_left[robot]):
ct = cts_left[robot][i]
if ct.contour not in done_contours:
# place contour in plan and sum wasted time if ct.contour in done_contours:
done_contours.append(ct.contour) del cts_left[robot][i]
plan.append(ct, robot) if len(cts_left[robot]) == 0:
t_used += ct.time[-1] robot = 1-robot
break break
continue
else:
# try adding
if plan.append(ct, robot, force=force):
# log done contours and time
done_contours.append(ct.contour)
t_used += ct.time[-1]
force = False
# get next arm to add to
rs = plan.planLenOrder()
for r in rs:
if len(cts_left[r]) != 0:
robot = r
break
break
# if tried everything, switch robots
elif i == len(cts_left[robot])-1:
# TODO work with more than two robots
robot = 1-robot
force=True
break
else:
# try next contour in list
i+=1
return plan, t_used return plan, t_used
...@@ -241,7 +280,7 @@ def reward(data, states): ...@@ -241,7 +280,7 @@ def reward(data, states):
""" """
plan, t_used = createCombinedSchedule(data, states) plan, t_used = createCombinedSchedule(data, states)
# return t_used/(plan.len()*len(states)), plan # FOR TESTING # return t_used/(plan.len()*len(states)), plan # FOR TESTING
return t_used/(plan.len()*len(states)) return t_used/plan.len()
def decmcts_layer( def decmcts_layer(
...@@ -332,7 +371,7 @@ def decmcts_layer( ...@@ -332,7 +371,7 @@ def decmcts_layer(
# grow the trees # grow the trees
for tree in trees: for tree in trees:
tree.grow(nsims=1, depth=1e6) tree.grow(nsims=1, depth=1e6)
if i%100==0 and i!=0: if i%1==0 and i!=0:
e = time.time() e = time.time()
print("Completed {} nodes in {:.2g}s".format(i, e-s)) print("Completed {} nodes in {:.2g}s".format(i, e-s))
...@@ -382,31 +421,31 @@ def decmcts_layer( ...@@ -382,31 +421,31 @@ def decmcts_layer(
joint_diff(home, traj.positions[0]) joint_diff(home, traj.positions[0])
# if there's enough time to get home in the wait period, do that # if there's enough time to get home in the wait period, do that
athome = list(plan.getLastJoint(arm)) == home #athome = list(plan.getLastJoint(arm)) == home
additional_time_spent = 0 #additional_time_spent = 0
if time_last2home + time_home2next <= decmcts_wait_time: #if time_last2home + time_home2next <= decmcts_wait_time:
# try going to home pos # # try going to home pos
while not athome: # while not athome:
gohome = plan.plan_to(env.home, arm) # gohome = plan.plan_to(env.home, arm)
# if no travel found, stay at last pos # # if no travel found, stay at last pos
if gohome is not None and plan.appendTrajectory([gohome], arm)==0: # if gohome is not None and plan.appendTrajectory([gohome], arm)==0:
print(arm, "returning to home to wait") # print(arm, "returning to home to wait")
athome = True # athome = True
else: # else:
print("There's enough time to get home but couldn't find a path - Unfortunate") # print("There's enough time to get home but couldn't find a path - Unfortunate")
print("Staying put for {}s".format(default_wait)) # print("Staying put for {}s".format(default_wait))
last_js = plan.trajs[arm][-1].positions[-1] # last_js = plan.trajs[arm][-1].positions[-1]
stayput = tu.JTrajectory([last_js, last_js],[0,default_wait], arm) # stayput = tu.JTrajectory([last_js, last_js],[0,default_wait], arm)
if plan.appendTrajectory([stayput], arm)!=0: # if plan.appendTrajectory([stayput], arm)!=0:
print("cannot go home or stay put Logically possible but unlikely - TODO") # print("cannot go home or stay put Logically possible but unlikely - TODO")
raise Exception("cannot go home or stay put Logically possible but unlikely - TODO") # raise Exception("cannot go home or stay put Logically possible but unlikely - TODO")
additional_time_spent += default_wait # additional_time_spent += default_wait
if time_last2home + additional_time_spent + default_wait + time_home2next > decmcts_wait_time: # if time_last2home + additional_time_spent + default_wait + time_home2next > decmcts_wait_time:
# can't go home anymore # # can't go home anymore
break # break
# add to plan # add to plan
traj_added = False traj_added = False
......
...@@ -43,32 +43,17 @@ def test_reward(contour_tracker, combined_schedule, real_plan, arm0=0, arm1=1): ...@@ -43,32 +43,17 @@ def test_reward(contour_tracker, combined_schedule, real_plan, arm0=0, arm1=1):
if __name__ == "__main__": if __name__ == "__main__":
# Read gcode file # Read gcode file
model = "flexirex_big" model = "Kitchen_sponge_holder_small-ascii"
tracker = tu.get_tracker(model) tracker = tu.get_tracker(model)
# 3arms simulation
env_desc3 = tu.PlanningEnv([[0,0,1,0],[0,0,1,0],[0,0,0,1]],
[[0.9, -0.35, 0],[-0.8, -0.35, 0.001],[0, 0.5, 0]],
[[0,0,0,1]]*3,
[[0.15,0,0]]*3) # WARNING - also edit urdf
# 2arms real calibration # 2arms real calibration
env_desc2 = tu.read_env('calibrations/r1_tforms.yaml', env_desc2 = tu.read_env('calibrations/r1_tforms.yaml',
'calibrations/r2_tforms.yaml') 'calibrations/r2_tforms.yaml')
env = tu.SimEnv(env_desc=env_desc3, gui=False, home=home) env = tu.SimEnv(env_desc=env_desc2, gui=True, home=home)
# decmcts plan first n contours # decmcts plan first n contours
c_traj_file = "gcode/" + model + ".c_traj" c_traj_file = "gcode/" + model + ".c_traj"
cs, plan = decmcts(tracker, cs, plan = decmcts(env, tracker, nth_layer=50, nodes_per_layer=1000)
env,
n_layers=1,
mcts_nodes=1000
)
# run test test_reward # run test test_reward
print("Compare arms 0 and 1")
test_reward(tracker, cs, plan, arm0=0, arm1=1) test_reward(tracker, cs, plan, arm0=0, arm1=1)
print("Compare arms 1 and 2")
test_reward(tracker, cs, plan, arm0=1, arm1=2)
print("Compare arms 0 and 2")
test_reward(tracker, cs, plan, arm0=0, arm1=2)
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