Merge branch 'main' of gitlab.engr.illinois.edu:chsieh16/cs598mp-fall2021-proj into main

data/filter_data.py

+import pickle
+
+import numpy as np
+
+# Constants for Stanley controller for GEM
+WHEEL_BASE = 1.75  # m
+K_P = 0.45
+CYCLE_TIME = 0.05  # second
+FORWARD_VEL = 2.8  # m/s
+STEERING_LIM = 0.61  # rad
+
+
+def g(cte, phi):
+    error = phi + np.arctan(K_P*cte/FORWARD_VEL)
+    steer = np.clip(error, -STEERING_LIM, STEERING_LIM)
+    return (steer,)
+
+
+def f(x, y, theta, steer):
+    new_x = x + FORWARD_VEL*np.cos(theta+steer)*CYCLE_TIME
+    new_y = y + FORWARD_VEL*np.sin(theta+steer)*CYCLE_TIME
+    new_theta = theta - FORWARD_VEL*np.sin(steer)/WHEEL_BASE*CYCLE_TIME
+    return new_x, new_y, new_theta
+
+
+def v(x, y, theta) -> float:
+    return y**2 + theta**2
+
+
+def pred(sample) -> bool:
+    x, y, theta, d, phi = sample
+    return v(*f(x, y, theta, *g(d, phi))) <= v(x, y, theta)
+
+
+in_file_name = "collect_images_2021-11-22-17-59-46.cs598.pickle"
+out_file_name = "collect_images_2021-11-22-17-59-46.cs598.filtered.pickle"
+
+with open(in_file_name, "rb") as in_file:
+    pkl_data = pickle.load(in_file)
+    truth_samples_seq = pkl_data["truth_samples"]
+
+
+filtered_truth_samples = []
+
+for truth, samples in truth_samples_seq:
+    cte, phi = truth
+    filtered_samples = [s for s in samples if pred(s)]
+    if abs(len(samples) - len(filtered_samples)) < 20:
+        print("#Original:", len(samples), "#Filtered:",  len(filtered_samples))
+        filtered_truth_samples.append((truth, samples))
+    else:
+        continue
+
+with open(out_file_name, "wb") as out_file:
+    pkl_data["truth_samples"] = filtered_truth_samples
+    pickle.dump(pkl_data, out_file)

dataCollectedFrom20Rounds.sl

+(constraint (inShape  26.6152701008448 0.9678239104943458 0.21114638865049587 2.345564365386963 (- 0.14508944749832153)))
+(constraint (inShape  26.61529433389539 0.967828326685563 0.21114590159461266 2.4930434226989746 (- 0.1531599760055542)))
+(constraint (inShape  26.61534953561334 0.9678393284980444 0.21114628135388033 2.510728359222412 (- 0.15152986347675323)))
+(constraint (inShape  19.375177724969767 0.967824356037088 0.21114631866326478 3.4791932106018066 (- 0.22473394870758057)))
+(constraint (inShape  19.37517508326423 0.9678238569539328 0.21114658296554917 3.5184550285339355 (- 0.22579112648963928)))
+(constraint (inShape  19.375182233263544 0.967825497133969 0.21114633517139236 3.5097079277038574 (- 0.2239835113286972)))
+(constraint (inShape  19.37518834132608 0.9678268184284384 0.21114626941534817 3.5298871994018555 (- 0.22639496624469757)))
+(constraint (inShape  10.235571424573456 0.9678239111315996 0.21114638837361643 3.438933849334717 (- 0.21554206311702728)))
+(constraint (inShape  10.235571539824214 0.9678240672511116 0.21114626519871993 3.436929702758789 (- 0.21290093660354614)))
+(constraint (inShape  10.235574707144886 0.9678247544784639 0.21114635158339712 3.445437431335449 (- 0.21383893489837646)))
+(constraint (inShape  10.235580857800862 0.9678260840358703 0.21114631984957144 3.4564030170440674 (- 0.21499930322170258)))
+(constraint (inShape  3.8539052024320735 0.9678239099543191 0.21114638869176813 1.4183670282363892 (- 0.09801661968231201)))
+(constraint (inShape  3.8539075220129626 0.9678245456809051 0.2111461465021146 1.60621976852417 (- 0.11350981146097183)))
+(constraint (inShape  3.8539106462580315 0.9678252255298361 0.21114641134134055 1.6671497821807861 (- 0.11667919158935547)))
+(constraint (inShape  3.853916709485315 0.9678265364059513 0.2111463338846634 2.38948392868042 (- 0.1834786981344223)))
+(constraint (inShape  2.148526343357085 0.9678239101001935 0.21114638873517305 3.5696678161621094 (- 0.2203296720981598)))
+(constraint (inShape  2.148528512694049 0.9678245109850732 0.21114624022727377 3.563772678375244 (- 0.21956664323806763)))
+(constraint (inShape  2.148531637917875 0.9678251862298772 0.2111463826103695 3.5739736557006836 (- 0.22020605206489563)))
+(constraint (inShape  2.1485376981887327 0.9678264979904956 0.2111463365188309 3.59548282623291 (- 0.2222861349582672)))
+(constraint (inShape  30.64333925200089 0.9678237509999992 0.211146388 3.3627007007598877 (- 0.2056111842393875)))
+(constraint (not (inShape  (- 0.13435869420323682) (- 1.899551057941262) (- 0.28484593314155127) 0.0 0.0)))
+(constraint (not (inShape  (- 0.1357230736889143) (- 0.06896000765390979) (- 0.22565356332402542) (- 0.13792001530781958) 0.0)))
+(constraint (not (inShape  0.031336154284762455 (- 0.6144592699844698) 1.485540123743981 2.0 0.0)))
+(constraint (not (inShape  (- 0.13404045455201255) 2.0 (- 0.31717354626777816) 2.0 0.6343470925355563)))
+(constraint (not (inShape  0.054857568457900144 (- 1.7840911757006543) (- 1.3634267523794492) 2.0 (- 0.9427623290582445))))
+(constraint (not (inShape  0.08020144441406736 (- 0.1386182570450704) (- 1.5707963267948966) 2.0 (- 1.2772365140901407))))
+(constraint (not (inShape  0.08020144441406737 1.1247414813760792 (- 1.5707963267948966) 2.0 (- 1.2494829627521584))))
+(constraint (not (inShape  0.07880745577985629 1.9509699384018293 (- 1.5587035427760474) (- 1.1174070855520948) (- 1.0))))
+(constraint (not (inShape  (- 0.12497763257869772) (- 0.02210191139819711) (- 0.23609212684430714) 1.5278157463113857 (- 0.4721842536886143))))
+(constraint (not (inShape  (- 0.02352354216660577) 2.0 1.3555842891942411 0.2888314216115177 0.0)))
+(constraint (not (inShape  (- 0.12238683784721333) (- 0.03674154677771748) (- 0.1983859678838836) (- 1.983859678838836) 0.0)))
+(constraint (not (inShape  (- 0.040430820847654904) 0.0 0.6678319322055949 0.0 0.6643361355888102)))
+(constraint (not (inShape  (- 0.12031953894476077) 0.0 0.02539164591081542 2.0 (- 0.8730417704459228))))
+(constraint (not (inShape  (- 0.1103297666085696) 2.0 0.411967253854291 2.0 (- 0.05983626927145515))))
+(constraint (not (inShape  0.0 0.5076765232177733 (- 1.0227481658397863) 2.0 (- 0.9846469535644533))))
+(constraint (not (inShape  (- 0.11974449498937859) 0.23016658550473063 (- 0.06536763447759253) 1.9346323655224076 1.5396668289905389)))
+(constraint (not (inShape  0.08020144441406735 0.7075142733814768 (- 1.5707963267948966) (- 0.15576778003194303) (- 0.5849714532370465))))
+(constraint (not (inShape  0.08005426932065733 0.5695169602836916 (- 1.5695169602836918) (- 2.0) (- 0.8609660794326169))))
+(constraint (not (inShape  0.08020144441406735 1.451479337228988 (- 1.5707963267948966) (- 0.23863397913181705) (- 0.9029586744579761))))
+(constraint (not (inShape  (- 0.13373748526757237) 1.9998255257227473 (- 0.30976749003865545) 0.0 0.6195349800773109)))

region_synth.py

@@ -24,11 +24,14 @@ from gem_stanley_teacher import GEMStanleyTeacher as Teacher
 
 
 def test_synth_region():
-    pickle_file_io = open("data/collect_images_2021-11-22-17-59-46.cs598.pickle", "rb")
+    pickle_file_io = open("data/collect_images_2021-11-22-17-59-46.cs598.filtered.pickle", "rb")
     pkl_data = pickle.load(pickle_file_io)
 
     truth_samples_seq = pkl_data["truth_samples"]
 
+    i_th = 0  # select only the i-th partition
+    truth_samples_seq = truth_samples_seq[i_th:i_th+1]
+
     truth_samples_seq = [(t, [s for s in raw_samples if not any(np.isnan(s))])
                          for t, raw_samples in truth_samples_seq]
     # Chiao: Read in positive samples
@@ -37,6 +40,7 @@ def test_synth_region():
     ]
     
     ex_dim = len(positive_examples[0])
+    print("#examples: %d" % len(positive_examples))
     print("Dimension of each example: %d" % ex_dim)
     assert all(len(ex) == ex_dim and not any(np.isnan(ex))
                for ex in positive_examples)
@@ -46,8 +50,8 @@ def test_synth_region():
     #  x >= 6.0 and y >= 5
     # teacher.set_old_state_bound(lb=[6.0, 5.0], ub=[11.0, 10.0])
     #[::20]
-    
-    synth_region(positive_examples[20:40:], teacher, num_max_iterations=20)
+
+    synth_region(positive_examples, teacher, num_max_iterations=20)
 
     #Gurobi encoding
     # 0 is diamond