Initial commit

.project

+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>repeated_driving_games</name>
+	<comment></comment>
+	<projects>
+		<project>game_theoretic_planner</project>
+	</projects>
+	<buildSpec>
+		<buildCommand>
+			<name>org.python.pydev.PyDevBuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+	</buildSpec>
+	<natures>
+		<nature>org.python.pydev.pythonNature</nature>
+	</natures>
+</projectDescription>

.pydevproject

+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<?eclipse-pydev version="1.0"?><pydev_project>
+<pydev_pathproperty name="org.python.pydev.PROJECT_SOURCE_PATH">
+<path>/${PROJECT_DIR_NAME}</path>
+</pydev_pathproperty>
+<pydev_property name="org.python.pydev.PYTHON_PROJECT_VERSION">python 3.6</pydev_property>
+<pydev_property name="org.python.pydev.PYTHON_PROJECT_INTERPRETER">conda</pydev_property>
+</pydev_project>

animation_example.py

+"""General Numerical Solver for the 1D Time-Dependent Schrodinger's equation.
+
+adapted from code at http://matplotlib.sourceforge.net/examples/animation/double_pendulum_animated.py
+
+Double pendulum formula translated from the C code at
+http://www.physics.usyd.edu.au/~wheat/dpend_html/solve_dpend.c
+
+author: Jake Vanderplas
+email: vanderplas@astro.washington.edu
+website: http://jakevdp.github.com
+license: BSD
+Please feel free to use and modify this, but keep the above information. Thanks!
+"""
+
+from numpy import sin, cos
+import numpy as np
+import matplotlib.pyplot as plt
+import scipy.integrate as integrate
+import matplotlib.animation as animation
+
+class DoublePendulum:
+    """Double Pendulum Class
+
+    init_state is [theta1, omega1, theta2, omega2] in degrees,
+    where theta1, omega1 is the angular position and velocity of the first
+    pendulum arm, and theta2, omega2 is that of the second pendulum arm
+    """
+    def __init__(self,
+                 init_state = [120, 0, -20, 0],
+                 L1=1.0,  # length of pendulum 1 in m
+                 L2=1.0,  # length of pendulum 2 in m
+                 M1=1.0,  # mass of pendulum 1 in kg
+                 M2=1.0,  # mass of pendulum 2 in kg
+                 G=9.8,  # acceleration due to gravity, in m/s^2
+                 origin=(0, 0)): 
+        self.init_state = np.asarray(init_state, dtype='float')
+        self.params = (L1, L2, M1, M2, G)
+        self.origin = origin
+        self.time_elapsed = 0
+
+        self.state = self.init_state * np.pi / 180.
+    
+    def position(self):
+        """compute the current x,y positions of the pendulum arms"""
+        (L1, L2, M1, M2, G) = self.params
+
+        x = np.cumsum([self.origin[0],
+                       L1 * sin(self.state[0]),
+                       L2 * sin(self.state[2])])
+        y = np.cumsum([self.origin[1],
+                       -L1 * cos(self.state[0]),
+                       -L2 * cos(self.state[2])])
+        return (x, y)
+
+    def energy(self):
+        """compute the energy of the current state"""
+        (L1, L2, M1, M2, G) = self.params
+
+        x = np.cumsum([L1 * sin(self.state[0]),
+                       L2 * sin(self.state[2])])
+        y = np.cumsum([-L1 * cos(self.state[0]),
+                       -L2 * cos(self.state[2])])
+        vx = np.cumsum([L1 * self.state[1] * cos(self.state[0]),
+                        L2 * self.state[3] * cos(self.state[2])])
+        vy = np.cumsum([L1 * self.state[1] * sin(self.state[0]),
+                        L2 * self.state[3] * sin(self.state[2])])
+
+        U = G * (M1 * y[0] + M2 * y[1])
+        K = 0.5 * (M1 * np.dot(vx, vx) + M2 * np.dot(vy, vy))
+
+        return U + K
+
+    def dstate_dt(self, state, t):
+        """compute the derivative of the given state"""
+        (M1, M2, L1, L2, G) = self.params
+
+        dydx = np.zeros_like(state)
+        dydx[0] = state[1]
+        dydx[2] = state[3]
+
+        cos_delta = cos(state[2] - state[0])
+        sin_delta = sin(state[2] - state[0])
+
+        den1 = (M1 + M2) * L1 - M2 * L1 * cos_delta * cos_delta
+        dydx[1] = (M2 * L1 * state[1] * state[1] * sin_delta * cos_delta
+                   + M2 * G * sin(state[2]) * cos_delta
+                   + M2 * L2 * state[3] * state[3] * sin_delta
+                   - (M1 + M2) * G * sin(state[0])) / den1
+
+        den2 = (L2 / L1) * den1
+        dydx[3] = (-M2 * L2 * state[3] * state[3] * sin_delta * cos_delta
+                   + (M1 + M2) * G * sin(state[0]) * cos_delta
+                   - (M1 + M2) * L1 * state[1] * state[1] * sin_delta
+                   - (M1 + M2) * G * sin(state[2])) / den2
+        
+        return dydx
+
+    def step(self, dt):
+        """execute one time step of length dt and update state"""
+        self.state = integrate.odeint(self.dstate_dt, self.state, [0, dt])[1]
+        self.time_elapsed += dt
+
+#------------------------------------------------------------
+# set up initial state and global variables
+pendulum = DoublePendulum([180., 0.0, -20., 0.0])
+dt = 1./30 # 30 fps
+
+#------------------------------------------------------------
+# set up figure and animation
+fig = plt.figure()
+ax = fig.add_subplot(111, aspect='equal', autoscale_on=False,
+                     xlim=(-2, 2), ylim=(-2, 2))
+ax.grid()
+
+line, = ax.plot([], [], 'o-', lw=2)
+time_text = ax.text(0.02, 0.95, '', transform=ax.transAxes)
+energy_text = ax.text(0.02, 0.90, '', transform=ax.transAxes)
+
+def init():
+    """initialize animation"""
+    line.set_data([], [])
+    time_text.set_text('')
+    energy_text.set_text('')
+    return line, time_text, energy_text
+
+def animate(i):
+    """perform animation step"""
+    global pendulum, dt
+    pendulum.step(dt)
+    
+    line.set_data(*pendulum.position())
+    time_text.set_text('time = %.1f' % pendulum.time_elapsed)
+    energy_text.set_text('energy = %.3f J' % pendulum.energy())
+    return line, time_text, energy_text
+
+# choose the interval based on dt and the time to animate one step
+from time import time
+t0 = time()
+animate(0)
+t1 = time()
+interval = 1000 * dt - (t1 - t0)
+
+ani = animation.FuncAnimation(fig, animate, frames=300,
+                              interval=interval, blit=True, init_func=init)
+
+# save the animation as an mp4.  This requires ffmpeg or mencoder to be
+# installed.  The extra_args ensure that the x264 codec is used, so that
+# the video can be embedded in html5.  You may need to adjust this for
+# your system: for more information, see
+# http://matplotlib.sourceforge.net/api/animation_api.html
+#ani.save('double_pendulum.mp4', fps=30, extra_args=['-vcodec', 'libx264'])
+
+plt.show()
\ No newline at end of file

scenarios.py

+'''
+Created on Feb 8, 2021
+
+@author: Atrisha
+'''
+import sqlite3
+import numpy as np
+import constants
+import ast 
+
+def right_turn_scenarios():
+    for file_id in constants.ALL_FILE_IDS:
+        constants.CURRENT_FILE_ID = file_id
+        conn = sqlite3.connect('D:\\intersections_dataset\\dataset\\'+constants.CURRENT_FILE_ID+'\\uni_weber_'+constants.CURRENT_FILE_ID+'.db')
+        c = conn.cursor()
+        q_string = "SELECT * from TRAJECTORIES_0"+str(file_id)+"_EXT"
+        c.execute(q_string)
+        res = c.fetchall()
+        scenario_dict = dict()
+        ct,N = 0,len(res)
+        for row in res:
+            ct += 1
+            #print(file_id,ct,'/',N)
+            if round(row[1]) not in scenario_dict:
+                scenario_dict[round(row[1])] = [(row[0],row[2])]
+            else:
+                scenario_dict[round(row[1])].append((row[0],row[2]))
+        for k in list(scenario_dict.keys()):
+            if 'l_n_s_r' in [x[1] for x in scenario_dict[k]] and ('rt_prep-turn_w' in [x[1] for x in scenario_dict[k]] or 'rt_exec-turn_w' in [x[1] for x in scenario_dict[k]]):
+                continue
+            else:
+                del scenario_dict[k]
+        print(file_id)
+        print(list(scenario_dict.keys()))
+        conn.close()
+        
+right_turn_scenarios()    
\ No newline at end of file

simulation_sketch.py

+'''
+Created on Feb 26, 2021
+
+@author: Atrisha
+'''
+import numpy as np
+import matplotlib.pyplot as plt
+from scipy.interpolate import CubicSpline, UnivariateSpline
+import matplotlib.patches as patches
+import matplotlib as mpl
+import matplotlib.animation as animation
+import math
+import scipy.integrate
+
+
+class TrajectoryPlanner:
+    
+    def __init__(self,centerline,vel_pts):
+        self.v0 = vel_pts[0]
+        self.vel_pts = vel_pts
+        self.centerline = centerline
+        
+        
+    def generate_path(self):
+        
+        ''' 
+        generate path with an index [0,1] that will 
+        be later scaled to the arc length
+        '''
+        
+        indx = [round(i/(len(self.centerline)-1),1) for i in np.arange(len(self.centerline))]
+        self.cs_x = CubicSpline(indx,[x[0] for x in self.centerline])
+        self.cs_y = CubicSpline(indx,[x[1] for x in self.centerline])
+        self.path = [(x,self.cs_x(x),self.cs_y(x)) for x in indx]
+        self.indx = indx
+        return self.path
+    
+    def generate_trajectory(self,horizon):
+        self.generate_path()
+        
+        indx = self.indx
+        ''' 
+        calculate the arc length of the generated path 
+        '''
+        
+        f_dx = self.cs_x.derivative(1)
+        f_dy = self.cs_y.derivative(1)
+        f = lambda x : math.hypot(f_dx(x),f_dy(x))
+        arcl = scipy.integrate.quad(f,0,1)[0]
+        '''
+        scale an axis with respect to the arc length
+        '''
+        s_pts = [arcl*x for x in indx]
+        
+        '''
+        initial and target velocity points.
+        same length as the index points
+        '''
+        v0 = self.v0
+        vel_pts_targ = self.vel_pts
+        
+        ''' main iteration loop '''
+        for iter in np.arange(0,100):
+            ''' some logic to find an alternate target 
+            velocity points since the previous iteration 
+            would have generated an infeasible trajectory 
+            '''
+            vel_pts = [vel_pts_targ[0],vel_pts_targ[1]+iter,vel_pts_targ[2]+iter]
+            
+            print('-------iter',iter)
+            print('target vels',vel_pts)
+            
+            ''' time scaling i.e. mapping time to arc length'''
+            t_max = horizon
+            time_st = np.arange(0,t_max+.1,.1)
+            time_pts = np.linspace(start=0, stop=t_max, num=len(s_pts))
+            self.cs_t_s = CubicSpline(time_pts,s_pts)
+            self.t_s_map = {t:self.cs_t_s(t) for t in time_st}
+            
+            ''' fit the time scaled velocity curve'''
+            self.cs_v = CubicSpline(time_pts,vel_pts)
+            self.cs_a = self.cs_v.derivative(1)
+            self.cs_j = self.cs_v.derivative(2)
+            
+            
+            traj = []
+            for t in time_st:
+                if t <= horizon:
+                    s = self.t_s_map[t]
+                    traj.append((t,self.cs_x(s/23),self.cs_y(s/23),self.cs_v(t),self.cs_a(t),self.cs_j(t)))
+                else:
+                    break
+            max_vel,max_acc,max_jerk = max([x[3] for x in traj]),max([x[4] for x in traj]),max([x[5] for x in traj])
+            
+            print('max_vel:',max_vel)
+            print('max_acc:',max_acc)
+            print('max_jerk:',max_jerk)
+            
+            if max_acc < 3.6 and max_jerk < 4:
+                break 
+            
+        time_ax_x = np.linspace(start=0, stop=time_pts[-1], num=100)
+        plt.plot(time_ax_x,[self.cs_v(x) for x in time_ax_x])
+        plt.title('velocity')
+        plt.show()
+        plt.plot(time_ax_x,[self.cs_a(x) for x in time_ax_x])
+        plt.title('acceleration')
+        plt.show()
+        plt.plot(time_ax_x,[self.cs_j(x) for x in time_ax_x])
+        plt.title('jerk')
+        plt.show()
+        self.trajectory = [(x[0],x[1],x[2]) for x in traj]
+        f=1
+        
+        '''
+        dsdt = self.cs_t_s.derivative(1)
+        ts_x = np.linspace(0,time_st[-1],200)
+        f = dsdt(0)
+        vel = [float(dsdt(x))*arcl for x in np.linspace(0,time_st[-1],200)]
+        #plt.plot(ts_x,vel_y)
+        #plt.title('velocity')
+        #plt.show()
+        #plt.plot([x[0] for x in self.t_s_map],[x[1]*arcl for x in self.t_s_map])
+        #plt.show()
+        self.time_st = time_st
+        '''
+        
+
+        
+        
+    
+        
+    
+    
+def draw_canvas():
+    
+    vehicle_lane = [(0,4), (13.5,4), (13.5,24), (6.5,24), (6.5,12), (0,12), (0,4)]
+    ped_lane = [(3,4), (6.5,4), (6.5,12), (3,12), (3,4)]
+    veh_centerline = [(8.25,24), (8.25,10), (0,10)]
+    ped_centerline = [(4.75,0),(4.75,8),(4.75,16)]
+    
+    lane_divider = [(0,8),(5,8)]
+    indx = np.arange(len(veh_centerline))
+    veh_motion = TrajectoryPlanner(veh_centerline,[10,3,6])
+    veh_motion.generate_trajectory(6)
+    
+    ped_motion = TrajectoryPlanner(ped_centerline,[3,2,3])
+    ped_motion.generate_trajectory(6)
+    
+    
+    #path_indx = np.arange(indx[0],indx[-1]+.01,.01)
+    rect = patches.Circle((veh_motion.trajectory[0][1], veh_motion.trajectory[0][1]), 1)
+    #rectm = patches.Rectangle((cs_x(path_indx[0])-.4, cs_y(path_indx[0])), .5, .75, linewidth=1, edgecolor='r', facecolor='none', fill= True)
+    #rectl = patches.Rectangle((cs_x(path_indx[0])-.4, cs_y(path_indx[0])), .5, .75, linewidth=1, edgecolor='r', facecolor='none', fill= True)
+    
+    
+    fig, ax = plt.subplots()
+    #plt.plot([x[0] for x in ped_centerline], [x[1] for x in ped_centerline],'-')
+    plt.plot([x[0] for x in vehicle_lane], [x[1] for x in vehicle_lane],'--')
+    plt.plot([x[0] for x in ped_lane], [x[1] for x in ped_lane],'--')
+    plt.plot([x[0] for x in lane_divider], [x[1] for x in lane_divider],'--')
+    plt.plot([x[0] for x in veh_centerline], [x[1] for x in veh_centerline],'x')
+    #plt.plot([cs_x(x) for x in path_indx],[cs_y(x) for x in path_indx])
+    veh_path, = ax.plot([], [])
+    veh_pathm, = ax.plot([], [])
+    veh_pathl, = ax.plot([], [])
+    ped_path, = ax.plot([], [])
+    #plt.plot(ped_centerline[0],ped_centerline[1],marker="^") 
+    hinge, = ax.plot(ped_motion.trajectory[0][1],ped_motion.trajectory[0][2],'^')
+    #ax.add_patch(rect)
+    plt.axis('equal')
+    
+    
+    
+    
+    def init():
+        """initialize animation"""
+        veh_path.set_data([x[1] for x in veh_motion.trajectory],[x[2] for x in veh_motion.trajectory])
+        '''
+        veh_pathm.set_data([cs_x(x) for x in path_indx],[cs_y(x) for x in path_indx])
+        veh_pathl.set_data([cs_x(x) for x in path_indx],[cs_y(x) for x in path_indx])
+        '''
+        ped_path.set_data([x[1] for x in ped_motion.trajectory],[x[2] for x in ped_motion.trajectory])
+        hinge.set_data(ped_motion.trajectory[0][1],ped_motion.trajectory[0][2])
+        ax.add_patch(rect)
+        '''
+        ax.add_patch(rectm)
+        ax.add_patch(rectl)
+        '''
+        return veh_path,rect,ped_path,hinge,
+
+    def animate(i):
+        """perform animation step"""
+        
+        if i < len(veh_motion.trajectory)-1:
+            yaw_line = np.asarray([veh_motion.trajectory[i+1][1]-veh_motion.trajectory[i][1],veh_motion.trajectory[i+1][2]-veh_motion.trajectory[i][2]])
+            xax = np.asarray([1,0])
+            cp = np.dot(yaw_line,xax)
+            ang = np.rad2deg(np.arccos(cp/(np.linalg.norm(yaw_line)*np.linalg.norm(xax))))
+            '''
+            yaw_linem = np.asarray([cs_x(path_indx[i//2+1])-cs_x(path_indx[i//2]),cs_y(path_indx[i//2+1])-cs_y(path_indx[i//2])])
+            xaxm = np.asarray([1,0])
+            cpm = np.dot(yaw_linem,xaxm)
+            angm = np.rad2deg(np.arccos(cpm/(np.linalg.norm(yaw_linem)*np.linalg.norm(xaxm))))
+            
+            yaw_linel = np.asarray([cs_x(path_indx[i//3+1])-cs_x(path_indx[i//3]),cs_y(path_indx[i//3+1])-cs_y(path_indx[i//3])])
+            xaxl = np.asarray([1,0])
+            cpl = np.dot(yaw_linel,xaxl)
+            angl = np.rad2deg(np.arccos(cpl/(np.linalg.norm(yaw_linel)*np.linalg.norm(xaxl))))
+            '''
+        rot_ang = 180-((ang-90)%360)
+        new_corn = veh_motion.trajectory[i][1]+.4*np.cos(np.deg2rad(rot_ang)), veh_motion.trajectory[i][2]+.4*np.sin(np.deg2rad(rot_ang))
+        '''
+        rot_angm = 180-((angm-90)%360)
+        rot_angl = 180-((angl-90)%360)
+        
+        new_cornm = cs_x(path_indx[i//2])+.4*np.cos(np.deg2rad(rot_angm)), cs_y(path_indx[i//2])+.4*np.sin(np.deg2rad(rot_angm))
+        new_cornl = cs_x(path_indx[i//3])+.4*np.cos(np.deg2rad(rot_angl)), cs_y(path_indx[i//3])+.4*np.sin(np.deg2rad(rot_angl))
+        '''
+        print('time:',round(i/10,1),'s')
+        #t2 = mpl.transforms.Affine2D().rotate_deg_around(new_corn[0],new_corn[1],(90-ang)) + ax.transData
+        '''
+        #t2 = mpl.transforms.Affine2D().rotate_deg_around(cs_x(path_indx[i]), cs_y(path_indx[i]),(90-ang)) + ax.transData
+        
+        t2m = mpl.transforms.Affine2D().rotate_deg_around(new_cornm[0],new_cornm[1],(90-angm)) + ax.transData
+        t2l = mpl.transforms.Affine2D().rotate_deg_around(new_cornl[0],new_cornl[1],(90-angl)) + ax.transData
+        '''
+        #rect.set_transform(t2)
+        
+        rect.center = veh_motion.trajectory[i][1], veh_motion.trajectory[i][2]
+        #rectm.set_transform(t2m)
+        #rectm.set_xy((cs_x(path_indx[i//2])+(.4*np.cos(np.deg2rad(rot_angm))), cs_y(path_indx[i//2])+(.4*np.sin(np.deg2rad(rot_angm)))))
+        #rectl.set_transform(t2l)
+        #rectl.set_xy((cs_x(path_indx[i//3])+(.4*np.cos(np.deg2rad(rot_angl))), cs_y(path_indx[i//3])+(.4*np.sin(np.deg2rad(rot_angl)))))
+        veh_path.set_data([],[])
+        veh_path.set_data([x[1] for x in veh_motion.trajectory[i:]],[x[2] for x in veh_motion.trajectory[i:]])
+        '''
+        veh_pathm.set_data([],[])
+        veh_pathm.set_data([cs_x(x) for x in path_indx[i//2:]],[cs_y(x) for x in path_indx[i//2:]])
+        veh_pathl.set_data([],[])
+        veh_pathl.set_data([cs_x(x) for x in path_indx[i//3:]],[cs_y(x) for x in path_indx[i//3:]])
+        '''
+        ped_path.set_data([],[])
+        ped_path.set_data([x[1] for x in ped_motion.trajectory[i:]],[x[2] for x in ped_motion.trajectory[i:]])
+        hinge.set_data(None,None)
+        hinge.set_data(ped_motion.trajectory[i][1], ped_motion.trajectory[i][2])
+        return veh_path,rect,ped_path,hinge,
+
+    ani = animation.FuncAnimation(fig, animate, frames=len(ped_motion.trajectory),
+                              interval=100, blit=True, init_func=init)
+    plt.show()
+    
+
+draw_canvas()
\ No newline at end of file

visualizer.py

+'''
+Created on Mar 2, 2021
+
+@author: Atrisha
+'''
+
+import sqlite3
+import numpy as np
+import matplotlib.pyplot as plt
+from collections import OrderedDict
+from matplotlib import animation
+from scipy.ndimage import gaussian_filter1d
+
+def gaussian_smoothing(a):
+    x, y = a.T
+    t = np.linspace(0, 1, len(x))
+    t2 = np.linspace(0, 1, 100)
+    
+    x2 = np.interp(t2, t, x)
+    y2 = np.interp(t2, t, y)
+    sigma = 10
+    x3 = gaussian_filter1d(x2, sigma)
+    y3 = gaussian_filter1d(y2, sigma)
+    
+    x4 = np.interp(t, t2, x3)
+    y4 = np.interp(t, t2, y3)
+    
+    return x4,y4
+
+
+class Visualization:
+    
+      
+    def show_lanes(self):
+        db = sqlite3.connect('D:\\influence-net\\influence-net\\trajectories.db')
+        cursor = db.cursor()
+        string = "SELECT OBJECT.ID FROM OBJECT,ANNOTATION WHERE ANNOTATION.ID=OBJECT.PID AND ANNOTATION.FILENAME='reference'"
+        cursor.execute(string)
+        res = cursor.fetchone()
+        lanes = []
+        cluster_ids = []
+        while res is not None:
+            list_x,list_y = [],[]
+            cursor_1 = db.cursor()
+            string = "SELECT OBJECT.ID,PT_CAMERA_COOR.T, PT_CAMERA_COOR.X,PT_CAMERA_COOR.Y,PT_CAMERA_COOR_ADD_OPTS.CLUSTER FROM PT_CAMERA_COOR,OBJECT,ANNOTATION,PT_CAMERA_COOR_ADD_OPTS WHERE OBJECT.ID = PT_CAMERA_COOR.PID AND ANNOTATION.ID=OBJECT.PID AND ANNOTATION.FILENAME='reference' AND OBJECT.ID=? AND PT_CAMERA_COOR_ADD_OPTS.ID = PT_CAMERA_COOR.ID ORDER BY CAST(PT_CAMERA_COOR.T AS UNSIGNED) ASC"
+            cursor_1.execute(string,[str(res[0])])
+            res_1 = cursor_1.fetchone()
+            cluster_id = None
+            while res_1 is not None:
+                list_x.append(float(res_1[2]))
+                list_y.append(float(res_1[3]))
+                if cluster_id == None:
+                    cluster_id = res_1[4]
+                res_1 = cursor_1.fetchone()
+            
+            '''smooth_list_x = s.smooth(np.asarray(list_x), window_len=3,window='flat')
+            smooth_list_y = s.smooth(np.asarray(list_y), window_len=3,window='flat')'''
+            z = [[e1,e2] for e1,e2 in zip(list_x,list_y)]
+            a = np.asarray(z)
+            smooth_list_x, smooth_list_y = gaussian_smoothing(a)
+            cluster_ids.append(cluster_id)
+            data_points_path = [(a,b) for a,b in zip(smooth_list_x,smooth_list_y)]
+            lanes.append(data_points_path)
+            res = cursor.fetchone()
+        
+        db.close()
+        for l in lanes:
+            plt.plot([x[0] for x in l],[x[1] for x in l])
+        plt.show()
+        return lanes,cluster_ids 
+    
+    def show_nyc_trajectories_static(self):
+        
+        conn = sqlite3.connect('D:\\influence-net\\influence-net\\trajectories.db')
+        c = conn.cursor()
+        q_string = "SELECT ANNOTATION.id AS FILE_ID,OBJECT.id AS OBJECT_ID, PT_CAMERA_COOR.* \
+                    FROM OBJECT INNER JOIN \
+                            ANNOTATION \
+                    ON \
+                            OBJECT.pid=ANNOTATION.id \
+                        INNER JOIN PT_CAMERA_COOR ON PT_CAMERA_COOR.pid=OBJECT.id"
+        c.execute(q_string)
+        res = c.fetchall()
+        trajs = dict()
+        for row in res:
+            if (row[0],row[1]) not in trajs: 
+                trajs[(row[0],row[1])] = []
+            trajs[(row[0],row[1])].append((float(row[4]),float(row[5])))
+        for k,v in trajs.items():
+            plt.scatter([x[0] for x in v],[x[1] for x in v],s=5)
+        
+        plt.show()
+        
+    def  show_nyc_trajectories_dynamic(self):
+        conn = sqlite3.connect('D:\\influence-net\\influence-net\\trajectories.db')
+        c = conn.cursor()
+        q_string = "SELECT ANNOTATION.id AS FILE_ID,OBJECT.id AS OBJECT_ID, PT_CAMERA_COOR.*, PT_CAMERA_COOR_ADD_OPTS.cluster, PT_CAMERA_COOR_ADD_OPTS.x_v, PT_CAMERA_COOR_ADD_OPTS.y_v, PT_CAMERA_COOR_ADD_OPTS.theta \
+                    FROM OBJECT INNER JOIN \
+                            ANNOTATION \
+                    ON \
+                            OBJECT.pid=ANNOTATION.id \
+                        INNER JOIN PT_CAMERA_COOR ON PT_CAMERA_COOR.pid=OBJECT.id INNER JOIN \
+                            PT_CAMERA_COOR_ADD_OPTS \
+                        ON \
+                            PT_CAMERA_COOR_ADD_OPTS.id=PT_CAMERA_COOR.id \
+                        order by file_id,CAST(t AS INTEGER)"
+        c.execute(q_string)
+        res = c.fetchall()
+        pts = OrderedDict()
+        for row in res:
+            if (row[0],row[6]) not in pts: 
+                pts[(row[0],row[6])] = []
+            pts[(row[0],row[6])].append((float(row[4]),float(row[5])))
+            
+        
+        fig, ax = plt.subplots()
+        ax = plt.axes(xlim=(-13, 7), ylim=(0, 56))
+        scat = ax.scatter([], [], s=5)
+        
+        def init():
+            scat.set_offsets([])
+            return scat,
+        
+        def animate(i):
+            scat.set_offsets(list(pts.values())[i])
+            print(i,list(pts.values())[i])
+            return scat,
+        
+        anim = animation.FuncAnimation(fig, animate, init_func=init, frames=len(pts)+1, 
+                               interval=20, blit=False, repeat=False)
+        plt.show()
+        
+viz = Visualization()
+viz.show_lanes()
\ No newline at end of file

whiteboard.py

+'''
+Created on Jan 26, 2021
+
+@author: Atrisha
+'''
+import numpy as np
+import matplotlib.pyplot as plt
+from scipy.stats import binom
+
+from equilibria import equilibria_core
+
+def fun1():
+    ''' prog, veh_inh, ped_inh'''
+    payoff_dict = {('lt_go','ls_go','rs_go'):[(1,-1,1),(1,-1,1),(1,-1,1)],
+                     ('lt_go','ls_wait','rs_go'):[(1,-1,1),(1,.5,1),(1,-1,1)],
+                     ('lt_wait','ls_go','rs_go'):[(.5,1,1),(1,1,1),(1,1,1)],
+                     ('lt_wait','ls_wait','rs_go'):[(.5,1,1),(.5,1,1),(1,1,1)],
+                     ('lt_go','ls_go','rs_wait'):[(1,-1,1),(1,-1,1),(.5,1,1)],
+                     ('lt_go','ls_wait','rs_wait'):[(1,1,1),(.5,1,1),(.5,1,1)],
+                     ('lt_wait','ls_go','rs_wait'):[(.5,1,1),(1,1,1),(.5,1,1)],
+                     ('lt_wait','ls_wait','rs_wait'):[(.5,1,1),(.5,1,1),(.5,1,1)]}
+    lt_table = {('lt_go','rs_go'):[(1,-1,1),(1,-1,1)],
+                     ('lt_wait','rs_go'):[(.5,1,1),(1,1,1)],
+                     ('lt_go','rs_wait'):[(1,1,1),(.5,1,1)],
+                     ('lt_wait','rs_wait'):[(.5,1,1),(.5,1,1)],
+                     }
+    
+    rs_table = {('lt_go','ls_go','rs_go'):[(1,-1,1),(1,-1,1),(1,-1,1)],
+                     ('lt_go','ls_wait','rs_go'):[(1,-1,1),(1,.5,1),(1,-1,1)],
+                     ('lt_wait','ls_go','rs_go'):[(.5,1,1),(1,1,1),(1,1,1)],
+                     ('lt_wait','ls_wait','rs_go'):[(.5,1,1),(.5,1,1),(1,1,1)],
+                     ('lt_go','ls_go','rs_wait'):[(1,-1,1),(1,-1,1),(.5,1,1)],
+                     ('lt_go','ls_wait','rs_wait'):[(1,1,1),(.5,1,1),(.5,1,1)],
+                     ('lt_wait','ls_go','rs_wait'):[(.5,1,1),(1,1,1),(.5,1,1)],
+                     ('lt_wait','ls_wait','rs_wait'):[(.5,1,1),(.5,1,1),(.5,1,1)]}
+    
+    ls_table = {('ls_go','rs_go'):[(1,1,1),(1,1,1)],
+                     ('ls_wait','rs_go'):[(0,1,1),(1,1,1)],
+                     ('ls_go','rs_wait'):[(1,1,1),(0,1,1)],
+                     ('ls_wait','rs_wait'):[(0,1,1),(0,1,1)],
+                     }
+    w = [0.25,0.25,0.5]
+    payoff_dict = {k:[w[0]*x[0]+w[1]*x[1]+w[2]*x[2] for x in v] for k,v in payoff_dict.items()}
+    lt_table = {k:[w[0]*x[0]+w[1]*x[1]+w[2]*x[2] for x in v] for k,v in lt_table.items()}
+    ls_table = {k:[w[0]*x[0]+w[1]*x[1]+w[2]*x[2] for x in v] for k,v in ls_table.items()}
+    split_dicts = [dict(),dict()]
+    for k,v in payoff_dict.items():
+        if (k[0],k[1]) not in split_dicts[0]:
+            split_dicts[0][(k[0],k[1])] = [v[0],v[1]]
+        if (k[1],k[2]) not in split_dicts[1]:
+            split_dicts[1][(k[1],k[2])] = [v[1],v[2]]
+    eq_core = equilibria_core.calc_pure_strategy_nash_equilibrium_exhaustive(payoff_dict, True)
+    print(eq_core)
+    print('------------split 1-----------')
+    eq_core = equilibria_core.calc_pure_strategy_nash_equilibrium_exhaustive(lt_table, True)
+    print(eq_core)
+    print('------------split 2-----------')
+    eq_core = equilibria_core.calc_pure_strategy_nash_equilibrium_exhaustive(ls_table, True)
+    print(eq_core)
+
+
+def plot_action_sample_eq_chart():
+    cl,cr,du,dd = .5,.8,.8,.9
+    n = 7
+    
+    def _alpha_u(k):
+        if k/n < cr / (cl+cr):
+            return 1
+        elif k/n > cr / (cl+cr):
+            return 0
+        else:
+            return 0.5
+        
+    def _alpha_l(m):
+        if m/n > du / (du+dd):
+            return 1
+        elif m/n < du / (du+dd):
+            return 0
+        else:
+            return 0.5
+    for n in [3,7]:
+        plt.figure()
+        X,Y = [],[]
+        for ql in np.arange(0,1.01,.01):
+            Y.append(ql)
+            sum = 0
+            for k in np.arange(n+1):
+                rv = binom(n, ql)
+                sum += rv.pmf(k)*_alpha_u(k)
+            X.append(sum)
+        plt.plot(X,Y,c='blue')
+        X,Y = [],[]
+        n = n+1
+        for pu in np.arange(0,1.01,.01):
+            X.append(pu)
+            sum = 0
+            for m in np.arange(n+1):
+                rv = binom(n, pu)
+                sum += rv.pmf(m)*_alpha_l(m)
+            Y.append(1-sum)
+        plt.plot(X,Y,c='green')   
+        plt.xlabel('prob of speeding up')
+        plt.ylabel('prob of turning')
+        plt.title('N='+str(n)+' samples')     
+    plt.show()
+
+def cournot_motion():
+    x1,x2 = 1,1
+    u_s1 = ((1-x1)**2 + (1-x2)**2)**.5
+    u_p1 = x1
+    u_p2 = x2
+    u1 = u_p1 + u_s1
+    
+
+
+#plot_action_sample_eq_chart()
\ No newline at end of file