test decmcts CombinedScheduler and collision table and ranges (#53)

src/test_decmcts.py

@@ -41,7 +41,7 @@ def test_reward(contour_tracker, n_contours=None, n_arms=2, gui=False):
     data = {"env": env,
             "collisions": {}}
     s = time.time()
-    r,p = reward(data, [arm1_plan, arm2_plan])
+    p,r = createCombinedSchedule(data, {0:arm1_plan, 1:arm2_plan})
     e = time.time()
     print(e-s)
 
@@ -63,7 +63,7 @@ def test_reward(contour_tracker, n_contours=None, n_arms=2, gui=False):
         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]
 
-        vis_col_region(ct_r0, ct_r1, sim_env, res=1, chosen_time=t_wait):
+        tu.vis_col_region(ct_r0, ct_r1, env, res=1, chosen_time=t_wait)
        
         # increment ct we're up to
         if p.end_times[0][i] <= p.end_times[1][j]:
@@ -81,8 +81,8 @@ def test_reward(contour_tracker, n_contours=None, n_arms=2, gui=False):
 if __name__ == "__main__":
 
     # Read gcode file
-    model = "flexirex"
+    model = "flexirex_big"
     tracker = tu.get_tracker(model)
 
     # run test test_reward 
-    data, r, p, c_traj = test_reward(contour_tracker, n_contours=10, n_arms=2, gui=False):
+    data, r, p, c_traj = test_reward(tracker, n_contours=10, n_arms=2, gui=False)

src/utils/planning_tools.py

@@ -9,6 +9,7 @@ from math import pi
 from bisect import bisect_left
 import gcode2contour as gc
 import pickle
+import matplotlib.pyplot as plt
 
 
 class JTrajectory:
@@ -168,7 +169,7 @@ def safe_ranges(time_ranges, safety_dist):
     for trange in time_ranges:
         if abs(trange[1] - trange[0]) > 2*safety_dist:
             safe_range.append((trange[0] + safety_dist, trange[1] - safety_dist))
-    return safe_ranges
+    return safe_range
 
 
 def col_free_offsets(t1, t2, sim_env, freq=1, min_range=2, safe_time=0):
@@ -237,8 +238,8 @@ def col_region(t1, t2, sim_env, freq=1):
 
     for i,t2_t in enumerate(t2_steps):
         for j,t1_t in enumerate(t1_steps):
-            jointss[t1.robot] = tu.joints_at_time(t1, t1_t)
-            jointss[t2.robot] = tu.joints_at_time(t2, t2_t)
+            jointss[t1.robot] = joints_at_time(t1, t1_t)
+            jointss[t2.robot] = joints_at_time(t2, t2_t)
             cols[j,i] = sim_env.colCheck(jointss)
 
     return cols
@@ -250,9 +251,10 @@ def vis_col_region(t1, t2, sim_env, res=1, chosen_time=None):
     their collision free offsets
     """
     col_table = col_region(t1, t2, sim_env, freq=1)
-    #t_wait = p.start_times[1][j] - p.start_times[0][i]
+    ranges = col_free_offsets(t1, t2, sim_env)
 
     # Main plot
+    fig, ax = plt.subplots()
     im = ax.imshow(np.flip(col_table, axis=0),
             cmap='RdYlGn_r', vmin=0, vmax=1,
             extent=[0,col_table.shape[1],0,col_table.shape[0]])
@@ -262,10 +264,10 @@ def vis_col_region(t1, t2, sim_env, res=1, chosen_time=None):
         print("whole region blocked")
 
     for r in ranges:
-        ax.plot([0,col_table.shape[1]],[-r[0],-r[0]+col_table.shape[1]],
+        ax.plot([0,col_table.shape[1]],[r[0],r[0]+col_table.shape[1]],
                 linewidth=1,
                 color='black')
-        ax.plot([0,col_table.shape[1]],[-r[1],-r[1]+col_table.shape[1]],
+        ax.plot([0,col_table.shape[1]],[r[1],r[1]+col_table.shape[1]],
                 linewidth=1,
                 color='black')
 
@@ -416,7 +418,6 @@ def get_tracker(model):
     Get contour tracker object for a model
     """
     # Read gcode file
-    model = "flexirex"
     pkl = "gcode/" + model + ".pkl"
     try:
         with open(pkl, 'rb') as f: