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:
self.end_times = [[0] for i in range(self.n)]
self.t_mul = joint2time_mul
self.safe_time = SAFE_TIME # safety buffer for Collisions
self.start_next_contour = [0]*self.n
def planLenOrder(self):
......@@ -98,7 +99,7 @@ class CombinedSchedule:
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
"""
......@@ -108,11 +109,11 @@ class CombinedSchedule:
self.trajs[robot].append(ct)
self.start_times[robot].append(self.end_times[robot][-1])
self.end_times[robot].append(self.start_times[robot][-1] + ct.time[-1])
return 0
return True
# travel time based on joint_diff
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
allowed_offsets = []
......@@ -147,11 +148,22 @@ class CombinedSchedule:
# easy for 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.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])
return t_wait
self.start_next_contour[robot] = self.end_times[robot][-1]
return True
# for n > 2
t_wait_last = -1
......@@ -211,27 +223,54 @@ def createCombinedSchedule(data, states):
plan = CombinedSchedule(data, len(states))
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 = []
t_used = 0
robot = 0
force=False
assert(len(cts_left[0])>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
while len(cts_left[robot]) > 0:
ct = cts_left[robot].popleft()
if ct.contour not in done_contours:
# place contour in plan and sum wasted time
done_contours.append(ct.contour)
plan.append(ct, robot)
t_used += ct.time[-1]
break
i = 0
while i < len(cts_left[robot]):
ct = cts_left[robot][i]
if ct.contour in done_contours:
del cts_left[robot][i]
if len(cts_left[robot]) == 0:
robot = 1-robot
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
......@@ -241,7 +280,7 @@ def reward(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))
return t_used/plan.len()
def decmcts_layer(
......@@ -332,7 +371,7 @@ def decmcts_layer(
# grow the trees
for tree in trees:
tree.grow(nsims=1, depth=1e6)
if i%100==0 and i!=0:
if i%1==0 and i!=0:
e = time.time()
print("Completed {} nodes in {:.2g}s".format(i, e-s))
......@@ -382,31 +421,31 @@ def decmcts_layer(
joint_diff(home, traj.positions[0])
# if there's enough time to get home in the wait period, do that
athome = list(plan.getLastJoint(arm)) == home
additional_time_spent = 0
if time_last2home + time_home2next <= decmcts_wait_time:
# try going to home pos
while not athome:
gohome = plan.plan_to(env.home, arm)
# if no travel found, stay at last pos
if gohome is not None and plan.appendTrajectory([gohome], arm)==0:
print(arm, "returning to home to wait")
athome = True
else:
print("There's enough time to get home but couldn't find a path - Unfortunate")
print("Staying put for {}s".format(default_wait))
last_js = plan.trajs[arm][-1].positions[-1]
stayput = tu.JTrajectory([last_js, last_js],[0,default_wait], arm)
if plan.appendTrajectory([stayput], arm)!=0:
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")
additional_time_spent += default_wait
if time_last2home + additional_time_spent + default_wait + time_home2next > decmcts_wait_time:
# can't go home anymore
break
#athome = list(plan.getLastJoint(arm)) == home
#additional_time_spent = 0
#if time_last2home + time_home2next <= decmcts_wait_time:
# # try going to home pos
# while not athome:
# gohome = plan.plan_to(env.home, arm)
# # if no travel found, stay at last pos
# if gohome is not None and plan.appendTrajectory([gohome], arm)==0:
# print(arm, "returning to home to wait")
# athome = True
# else:
# print("There's enough time to get home but couldn't find a path - Unfortunate")
# print("Staying put for {}s".format(default_wait))
# last_js = plan.trajs[arm][-1].positions[-1]
# stayput = tu.JTrajectory([last_js, last_js],[0,default_wait], arm)
# if plan.appendTrajectory([stayput], arm)!=0:
# 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")
# additional_time_spent += default_wait
# if time_last2home + additional_time_spent + default_wait + time_home2next > decmcts_wait_time:
# # can't go home anymore
# break
# add to plan
traj_added = False
......
......@@ -43,32 +43,17 @@ def test_reward(contour_tracker, combined_schedule, real_plan, arm0=0, arm1=1):
if __name__ == "__main__":
# Read gcode file
model = "flexirex_big"
model = "Kitchen_sponge_holder_small-ascii"
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
env_desc2 = tu.read_env('calibrations/r1_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
c_traj_file = "gcode/" + model + ".c_traj"
cs, plan = decmcts(tracker,
env,
n_layers=1,
mcts_nodes=1000
)
cs, plan = decmcts(env, tracker, nth_layer=50, nodes_per_layer=1000)
# run test test_reward
print("Compare arms 0 and 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