add cars fix model and two case study

examples/cars_fix/Car_Dynamic_Single.py

@@ -0,0 +1,178 @@
+# The differential equations of a single car dynamics
+
+from scipy.integrate import odeint
+import numpy as np 
+
+def Car_dynamic(y,t,v_initial,acc,acc_time,turn_indicator,turn_time,turn_back_time):
+    L = 5.0 # the length of the car, make it fix here
+    # make everything double
+    v_initial = float(v_initial)
+    acc = float(acc)
+    y = [float(tmp) for tmp in y]
+    acc_time = float(acc_time)
+    turn_time = float(turn_time)
+    turn_back_time = float(turn_back_time)
+    # end of making float
+
+    # set the velocity
+    if t <= acc_time:
+        v = v_initial
+        #print('20')
+    elif (t > acc_time) and (t <= acc_time + 5.0):
+        v = v_initial + acc*(t-acc_time)
+        #print('23')
+    elif t > acc_time + 5.0:
+        v = v_initial + acc * 5.0
+        #print('25')
+    else:
+        print('Something is wrong with time here when calculting velocity!')
+    # set the steering angle
+    delta_initial = 0.0
+    if turn_indicator == 'Right':
+        # print(t)
+        if t <= turn_time:
+            delta_steer = delta_initial;
+        elif (t > turn_time) and (t <= turn_time + 2.0):
+            delta_steer = delta_initial + 1.0 
+        elif (t > turn_time + 2.0) and (t <= turn_back_time):
+            delta_steer = delta_initial 
+        elif (t > turn_back_time) and (t <= turn_back_time + 2.0):
+            delta_steer = delta_initial - 1.0 
+        elif t > turn_back_time + 2.0:
+            delta_steer = delta_initial
+        else:
+            print('Something is wrong with time here when calculting steering angle!')
+    elif turn_indicator =='Left':
+        if t <= turn_time:
+            delta_steer = delta_initial;
+        elif (t > turn_time) and (t <= turn_time + 2.0):
+            delta_steer = delta_initial + (-1.0) 
+        elif (t > turn_time + 2.0) and (t <= turn_back_time):
+            delta_steer = delta_initial 
+        elif (t > turn_back_time) and (t < turn_back_time + 2.0):
+            delta_steer = delta_initial + (1.0)
+        elif t > turn_back_time + 2.0:
+            delta_steer = delta_initial
+        else:
+            print('Something is wrong with time here when calculting steering angle!')
+    elif turn_indicator == 'Straight':
+        delta_steer = delta_initial
+    else:
+        print('Wrong turn indicator!')
+
+    psi, sx, py = y
+    psi_dot = (v)/L*(np.pi/8.0)*delta_steer
+    w_z = psi_dot
+    sx_dot = v * np.cos(psi) - L/2.0 * w_z * np.sin(psi) 
+    if abs(sx_dot) < 0.0000001:
+        sx_dot = 0.0
+    sy_dot = v * np.sin(psi) + L/2.0 * w_z * np.cos(psi)
+    if abs(sy_dot) < 0.0000001:
+        sy_dot = 0.0
+    #sx_dot = v * np.cos(psi) 
+    #sy_dot = v * np.sin(psi)
+    dydt = [psi_dot, sx_dot, sy_dot]
+    return dydt
+
+
+def Car_simulate(Mode,initial,time_bound):
+    time_step = 0.05;
+    time_bound = float(time_bound)
+    initial = [float(tmp)  for tmp in initial]
+    number_points = int(np.ceil(time_bound/time_step))
+    t = [i*time_step for i in range(0,number_points)]
+    if t[-1] != time_step:
+        t.append(time_bound)
+    newt = []
+    for step in t:
+        newt.append(float(format(step, '.2f')))
+    t = newt
+    # initial = [sx,sy,vx,vy]
+    # set the parameters according to different modes
+    # v_initial,acc,acc_time,turn_indicator,turn_time,turn_back_time
+
+    sx_initial = initial[0]
+    sy_initial = initial[1]
+    vx_initial = initial[2]
+    vy_initial = initial[3]
+
+    v_initial = (vx_initial**2 + vy_initial**2)**0.5
+
+    # calculate the initial angle
+    val = np.arccos(vx_initial/v_initial)
+    if vy_initial < 0:
+        val = 2*np.pi-val
+    psi_initial = val
+    acc = 0.0
+    acc_time = 0.0
+    turn_time = 0.0
+    turn_back_time = 0.0
+
+    # Initialize according to different mode
+    if Mode == 'Const':
+        turn_indicator = 'Straight'
+    elif ((Mode == 'Acc1') or (Mode == 'Acc2')):
+        turn_indicator = 'Straight'
+        acc = 0.2
+        acc_time = 0.0
+    elif (Mode == 'Dec') or (Mode == 'Brk'): 
+        turn_indicator = 'Straight'
+        if v_initial == 0.0:
+            acc = 0.0
+        else:
+            acc = -0.2
+        acc_time = 0.0
+    elif Mode =='TurnLeft':
+        turn_indicator = 'Left'
+        turn_time = 0.0
+        turn_back_time = 5.0
+    elif Mode == 'TurnRight':
+        turn_indicator = 'Right'
+        turn_time = 0.0
+        turn_back_time = 5.0
+    else:
+        print('Wrong Mode name!')
+
+    Initial_ode = [psi_initial, sx_initial, sy_initial]
+    sol = odeint(Car_dynamic,Initial_ode,t,args=(v_initial,acc,acc_time,turn_indicator,turn_time,turn_back_time),hmax = time_step)
+
+    # Construct v
+    v = np.zeros(len(t))
+
+    for i in range(len(t)):
+        if t[i] <= acc_time:
+            v[i] = v_initial
+        elif (t[i] > acc_time) and (t[i] <= acc_time + 5.0):
+            v[i] = v_initial + acc*(t[i]-acc_time)
+        elif (t[i] > acc_time + 5.0):
+            v[i] = v_initial + acc * 5.0
+
+
+    # Construct the final output
+    trace = []
+    for j in range(len(t)):
+        current_psi = sol[j,0]
+        #print t[j], current_psi
+        tmp = []
+        tmp.append(t[j])
+        tmp.append(sol[j,1])
+        tmp.append(sol[j,2])
+        vx = v[j]*np.cos(current_psi)
+        if abs(vx) < 0.0000001:
+            vx = 0.0
+        tmp.append(vx)
+        vy = v[j]*np.sin(current_psi)
+        if abs(vy) < 0.0000001:
+            vy = 0.0
+        tmp.append(vy)
+        trace.append(tmp)
+    return trace
+
+if __name__ == "__main__":
+    print "start test"
+    traj = Car_simulate("TurnLeft", [0.0,0.0,1.0,0.0], "10.0")
+    for line in traj:
+        print line
+
+
+

examples/cars_fix/Car_Sim.py

@@ -0,0 +1,22 @@
+from Car_Dynamic_Single import *
+
+
+def TC_Simulate(Modes,initialCondition,time_bound):
+	Modes = Modes.split(';')
+	num_cars = len(Modes)
+	#print initialCondition
+
+	if len(initialCondition) == 4*num_cars:
+		for car_numer in range(num_cars):
+			Current_Initial = initialCondition[car_numer*4:car_numer*4+4]
+			trace = Car_simulate(Modes[car_numer],Current_Initial,time_bound)
+			trace = np.array(trace)
+			if car_numer == 0:
+				Final_trace = np.zeros(trace.size)
+				Final_trace = trace
+			else:
+				Final_trace = np.concatenate((Final_trace, trace[:,1:5]), axis=1)
+	else:
+		print('Number of cars does not match the initial condition')
+
+	return Final_trace

examples/cars_fix/InOutput.py

@@ -0,0 +1,66 @@
+# Write data to file
+def write_to_file(Sim_Result, write_path, type):
+    # Write bloat file
+    if type == 'simulation':
+        with open(write_path, 'w') as write_file:
+            for interval in Sim_Result:
+                for item in interval:
+                    write_file.write(str(item) + ' ')
+                write_file.write('\n')
+
+
+
+
+def read_from_file(read_path, type):
+	if type == 'simulation':
+		trace = []
+		with open(read_path, 'r') as trace_file:
+			for line in trace_file:
+				# Extract data and append to trace
+				data = [float(x) for x in line.split()]
+				trace.append(data)    
+	if type == 'reachtube_single':
+		tube = []
+		with open(read_path, 'r') as trace_file:
+			next(trace_file)
+			for line in trace_file:
+				# Extract data and append to trace
+				data = [float(x) for x in line.split()]
+				tube.append(data)    
+		return tube
+	return trace          
+
+def Reachtube_trunk(reachtube, time_interval):
+	reachtube_length = len(reachtube)
+	dimensions = len(reachtube[0])
+	if reachtube_length % 2 != 0:
+		print('Reachtube length is not even, please check!')
+		return None
+
+
+	find_flag = 0
+	lower_bound = []
+	upper_bound = []
+	for i in range(1,dimensions):
+		lower_bound.append('nan')
+		upper_bound.append('nan')
+
+	for i in range(0, reachtube_length, 2):
+		if (reachtube[i][0] >= time_interval[0]) & (reachtube[i][0] <= time_interval[1]):
+			if find_flag == 0:
+				find_flag = 1
+				for dim in range(1,dimensions):
+					lower_bound[dim-1] = reachtube[i][dim]
+					upper_bound[dim-1] = reachtube[i+1][dim]
+			else:
+				for dim in range(1,dimensions):
+					lower_bound[dim-1] = min(lower_bound[dim-1], reachtube[i][dim])
+					upper_bound[dim-1] = max(upper_bound[dim-1], reachtube[i+1][dim])
+
+	# double check output
+	for dim in range(1,dimensions):
+		if lower_bound[dim-1] > upper_bound[dim-1]:
+			print('Find reach set in given time interval is wrong. The computed lower bound is greater than the upper bound!')
+			return None
+
+	return lower_bound, upper_bound

examples/cars_fix/TC_Graph.py

@@ -0,0 +1,96 @@
+from igraph import *
+
+def takeOverGraph():
+	g = Graph(directed = True)
+	g.add_vertices(6)
+	g.add_edges([(0,1),(1,2),(2,3),(3,4),(4,5)])
+	transtime =[(5,6),(10,12),(5,6),(5,6),(10,12)]
+	divid_intervals = [2,1,1,1,2]
+	if g.is_dag()==True:
+		print("Graph provided is a DAG")
+	else:
+		print("Graph provided is not a DAG, quit()")
+	g.vs["label"] = [("Acc1;Const"),("TurnLeft;Const"),("Acc2;Const"),
+	("Dec;Const"),("TurnRight;Const"),("Const;Const")]
+	# g.vs["label"] = [("Acc1;Const"),("TurnRight;Const"),("Acc2;Const")]
+	g.vs["name"] = g.vs["label"]
+	#edges_labels = [str(t) for t in transtime]
+	g.es["label"] = transtime
+	g.es["trans_time"] = transtime
+	g.es["divid"] = divid_intervals
+	PT_Pic = plot(g,"CarOverTake.png",margin=40)
+	PT_Pic.save()
+	#print("The structure of the powertrain graph has been saved in the folder as a .png picture")
+	return g
+
+def carOneGraph():
+	g = Graph(directed = True)
+	g.add_vertices(12)
+	g.add_edges([(0,1),(1,2),  (2,3),(3,4),(4,5),  (1,6),  (6,7),(7,8),  (8,5),  (1,9),  (9,10),(10,11),(11,5)])
+	transtime = [(5,6),(10,12),(5,6),(5,6),(10,12),(10,12),(5,6),(14,16),(10,12),(10,12),(5,6), (5,6),  (10,12)]
+	divid_intervals = [1,1,1,1,1,1,1,1,1,1,1,1,1]
+
+	if g.is_dag()==True:
+		print("Graph provided is a DAG")
+	else:
+		print("Graph provided is not a DAG, quit()")
+
+	#layout = g.layout("kk")
+	g.vs["label"] = [("Acc1;Const"),("TurnLeft;Const"),("Acc2;Const"),
+	("Dec;Const"),("TurnRight;Const"),("Const;Const"),("Acc1;Acc1"),
+	("Dec;Acc1"),("TurnRight;Acc1"),("Acc1;Dec"),("Const;Dec"),("TurnRight;Const")]
+	g.vs["name"] = g.vs["label"]
+	#edges_labels = [str(t) for t in transtime]
+	g.es["label"] = transtime
+	g.es["trans_time"] = transtime
+	g.es["divid"] = divid_intervals
+	PT_Pic = plot(g,"Car.png",margin=40)
+	PT_Pic.save()
+	#print("The structure of the powertrain graph has been saved in the folder as a .png picture")
+	return g
+
+def mergeGraph():
+	g = Graph(directed = True)
+	g.add_vertices(7)
+	g.add_edges([(0,1),(1,2),(2,3),(3,4),(0,5),(5,6),(6,4)])
+	transtime=[(1,2),(5,6),(5,6),(10,12),(1,2),(5,6),(10,12)]
+	divid_intervals = [1,1,1,1,1,1,1]
+
+	if g.is_dag()==True:
+		print("Graph provided is a DAG")
+	else:
+		print("Graph provided is not a DAG, quit()")
+
+	g.vs["label"] = [("Const;Const"),("Acc1;Acc1"),("Dec;Acc1"),
+	("TurnRight;Const"),("Const;Const"),("Acc1;Const"),('TurnRight;Const')]
+	g.vs["name"] = g.vs["label"]
+	g.es["label"] = transtime
+	g.es["trans_time"] = transtime
+	g.es["divid"] = divid_intervals
+	PT_Pic = plot(g,"Car2.png",margin=40)
+	PT_Pic.save()
+
+	return g
+
+
+def mergeGraphSimple():
+	g = Graph(directed = True)
+	g.add_vertices(4)
+	g.add_edges([(0,1),(1,2),(2,3)])
+	transtime=[(1,2),(5,6),(5,6),(10,12),(1,2),(5,6),(10,12)]
+	divid_intervals = [1,1,1,1,1,1,1]
+
+	if g.is_dag()==True:
+		print("Graph provided is a DAG")
+	else:
+		print("Graph provided is not a DAG, quit()")
+
+	g.vs["label"] = [("Const;Const"),("Acc1;Const"),("TurnRight;Const"),("Const;Const")]
+	g.vs["name"] = g.vs["label"]
+	g.es["label"] = transtime
+	g.es["trans_time"] = transtime
+	g.es["divid"] = divid_intervals
+	PT_Pic = plot(g,"Car2.png",margin=40)
+	PT_Pic.save()
+
+	return g

examples/cars_fix/__init__.py

@@ -0,0 +1 @@
+from Car_Sim import *