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

decmcts creating sensible plan for 3 arms (#43)

parent de1ef531
......@@ -153,23 +153,24 @@ class CombinedSchedule:
self.end_times[robot].append(self.start_times[robot][-1] + ct.time[-1])
return t_wait
raise Exception("function not implemented for more than two robots yet")
# for n > 2
t_wait_last = 1
t_wait_last = -1
while True:
t_wait = min([0] + [o[-1][1] for o in allowed_offsets])
t_wait = max(trav_time,-min([0] + [o[-1][1] for o in allowed_offsets]))
if t_wait_last == t_wait:
Exception("Logic error, this should never happen")
for ro in allowed_offsets:
while ro[-1][0] > t_wait:
while ro[-1][0] > -t_wait:
ro.pop()
valid = [ro[-1][0] <= t_wait <= ro[-1][1] for ro in allowed_offsets]
valid = [ro[-1][0] <= -t_wait <= ro[-1][1] for ro in allowed_offsets]
if all(valid):
break
t_wait_last = t_wait
# t_wait is a negative number
return trav_time - t_wait
self.trajs[robot].append(ct)
self.start_times[robot].append(self.end_times[robot][-1] + t_wait)
self.end_times[robot].append(self.start_times[robot][-1] + ct.time[-1])
return t_wait
def visualiseCombinedSchedule(combined_schedule):
......
......@@ -4,7 +4,7 @@ Test Functions in decmcts.py
from decmcts import *
def test_reward(contour_tracker, combined_schedule, real_plan):
def test_reward(contour_tracker, combined_schedule, real_plan, arm0=0, arm1=1):
"""
test _reward function
"""
......@@ -13,23 +13,23 @@ def test_reward(contour_tracker, combined_schedule, real_plan):
print("orange horizonatl line means looking for start time above" + \
", black line means looking for a start time below\n")
i,j = 1, 1
robot = 1
while i<len(p.start_times[0]) and j<len(p.start_times[1]):
robot = arm1
while i<len(p.start_times[arm0]) and j<len(p.start_times[arm1]):
# run for a pair of trajectories
ct_r0, ct_r1 = p.trajs[0][i-1], p.trajs[1][j-1]
t_wait = p.start_times[1][j] - p.start_times[0][i]
ct_r0, ct_r1 = p.trajs[arm0][i-1], p.trajs[arm1][j-1]
t_wait = p.start_times[arm1][j] - p.start_times[arm0][i]
print("({},{})".format(i,j))
tu.vis_col_region(ct_r0, ct_r1, env, res=1, chosen_time=t_wait, safe_time = SAFE_TIME)
# increment ct we're up to
if p.end_times[0][i] <= p.end_times[1][j]:
if p.end_times[arm0][i] <= p.end_times[arm1][j]:
i +=1
robot = 0
robot = arm0
else:
j +=1
robot = 1
robot = arm1
user_input = input('enter for next, "p" for show plan and next, or "q" for quit: ')
if user_input =='q':
......@@ -55,7 +55,7 @@ if __name__ == "__main__":
# 2arms real calibration
env_desc2 = tu.read_env('calibrations/r1_tforms.yaml',
'calibrations/r2_tforms.yaml')
env = tu.SimEnv(env_desc=env_desc2, gui=False, home=home)
env = tu.SimEnv(env_desc=env_desc3, gui=False, home=home)
# decmcts plan first n contours
c_traj_file = "gcode/" + model + ".c_traj"
......@@ -66,4 +66,9 @@ if __name__ == "__main__":
)
# run test test_reward
test_reward(tracker, cs, plan)
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