Skip to content
Snippets Groups Projects
Commit cf21969b authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

Merge remote-tracking branch 'origin/feature/decmcts-n-arm'

parents 5d7aeba8 e1b45c6a
Branches
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/decmcts.py
+ 15
− 8
View file @ cf21969b
......@@ -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):
......@@ -513,10 +514,16 @@ if __name__ == "__main__":
model = "flexirex_big"
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_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"
......
src/greedy.py
+ 3
− 3
View file @ cf21969b
......@@ -155,8 +155,8 @@ if __name__ == "__main__":
tracker = tu.get_tracker(model)
# 3arms simulation
env_desc3 = tu.PlanningEnv([[0,0,1,0],[0,0,0,1],[0,0,0,1]],
[[0.9, -0.25, 0],[-0.5, -0.25, 0.001],[0, 0.5, 0]],
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
......@@ -165,4 +165,4 @@ if __name__ == "__main__":
'calibrations/r2_tforms.yaml')
# Greedily plan first n contours
plan = greedy(tracker, env_desc2, n_contours=10, gui=True)
plan = greedy(tracker, env_desc3, n_contours=10, gui=True)
src/test_decmcts.py
+ 21
− 10
View file @ cf21969b
......@@ -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':
......@@ -46,10 +46,16 @@ if __name__ == "__main__":
model = "flexirex_big"
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_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"
......@@ -60,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)
src/utils/simEnv.py
+ 5
− 1
View file @ cf21969b
......@@ -61,7 +61,11 @@ _vert_or = [np.array([[-6.74517010e-04, 8.33315658e-04, -9.99999404e-01, -2.905
[ 8.33732192e-04, -9.99999285e-01, -8.33877944e-04, -4.81068309e-05],
[-9.99999404e-01, -8.34294187e-04, 6.73821778e-04, 2.74792069e-06],
[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]),
np.array([[-6.74517010e-04, -8.33315658e-04, 9.99999404e-01, -2.90597873e-05],
np.array([[-6.74517010e-04, -8.33315658e-04, 9.99999404e-01, -2.90597873e-05], # 2nd arm
[ 8.33732192e-04, 9.99999285e-01, 8.33877944e-04, -4.81068309e-05],
[-9.99999404e-01, 8.34294187e-04, -6.73821778e-04, 2.74792069e-06],
[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]),
np.array([[-6.74517010e-04, -8.33315658e-04, 9.99999404e-01, -2.90597873e-05], # 3rd arm
[ 8.33732192e-04, 9.99999285e-01, 8.33877944e-04, -4.81068309e-05],
[-9.99999404e-01, 8.34294187e-04, -6.73821778e-04, 2.74792069e-06],
[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])]
......
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