diff --git a/src/decmcts.py b/src/decmcts.py
index d962c73c2f1ff1a960af72c7e7031f1aa6acaafa..53ab40873d10790e5b2d77a467a0c9501adc72d1 100644
--- a/src/decmcts.py
+++ b/src/decmcts.py
@@ -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"
diff --git a/src/greedy.py b/src/greedy.py
index a9bf1ba9e78bf1f20ae1c2c5881e9e4536be3bf2..2df61d8c3c92e5f86e4ee32998e4637316b1db31 100644
--- a/src/greedy.py
+++ b/src/greedy.py
@@ -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)
diff --git a/src/test_decmcts.py b/src/test_decmcts.py
index 14f68a29403119f61cc8c7fd14bf63d7affd9432..57b3ae9f2a6f30d94458cc3fa3ebc7f062c20edc 100644
--- a/src/test_decmcts.py
+++ b/src/test_decmcts.py
@@ -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)
diff --git a/src/utils/simEnv.py b/src/utils/simEnv.py
index b8ea2290b3e58921f3f268ff5081d74e4463d490..efc06f7a03e1413ddbc15dca0c89c47523061590 100644
--- a/src/utils/simEnv.py
+++ b/src/utils/simEnv.py
@@ -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]])]