Merge pull request #48 from andycui97/geo_planner

Geo planner
BerkeleyAutomation · Apr 16, 2018 · 6fc16f4 · 6fc16f4
2 parents 88e6773 + 01a1386
commit 6fc16f4
Show file tree

Hide file tree

Showing 30 changed files with 61 additions and 280 deletions.
diff --git a/examples/control_with_supervisor.py b/examples/control_with_supervisor.py
@@ -4,9 +4,8 @@
 from gym_urbandriving import *
 from gym_urbandriving.agents import *
 from gym_urbandriving.assets import *
-from gym_urbandriving.planning import Trajectory
+from gym_urbandriving.utils import Trajectory
 import numpy as np
-import IPython
 
 with open('configs/default_config.json') as json_data_file:
     data = json.load(json_data_file)

diff --git a/examples/json_setup.py b/examples/json_setup.py
@@ -4,9 +4,8 @@
 from gym_urbandriving import *
 from gym_urbandriving.agents import *
 from gym_urbandriving.assets import *
-from gym_urbandriving.planning import Trajectory
+from gym_urbandriving.utils import Trajectory
 import numpy as np
-import IPython
 
 with open('configs/default_config.json') as json_data_file:
     data = json.load(json_data_file)

diff --git a/examples/test.py b/examples/test.py
@@ -15,7 +15,6 @@
 def f():
     config = json.load(open('configs/default_config.json'))
     config['environment']['visualize'] = True
-    config['agents']['background_cars'] = 3
     env = uds.UrbanDrivingEnv(config_data=config)
 
     env._reset()

diff --git a/gym_urbandriving/agents/background/planning_pursuit_agent.py b/gym_urbandriving/agents/background/planning_pursuit_agent.py
@@ -1,5 +1,5 @@
 import numpy as np
-from gym_urbandriving.utils.PID import PIDController
+from gym_urbandriving.utils import PIDController
 from gym_urbandriving.agents import PursuitAgent
 from gym_urbandriving.planning import VelocityMPCPlanner,GeometricPlanner
 from copy import deepcopy

diff --git a/gym_urbandriving/agents/background/pursuit_agent.py b/gym_urbandriving/agents/background/pursuit_agent.py
@@ -1,5 +1,5 @@
 import numpy as np
-from gym_urbandriving.utils.PID import PIDController
+from gym_urbandriving.utils import PIDController
 from gym_urbandriving.agents import Agent
 from gym_urbandriving.actions import SteeringAction
 

diff --git a/gym_urbandriving/agents/hierarchical/steering_action_agent.py b/gym_urbandriving/agents/hierarchical/steering_action_agent.py
@@ -1,11 +1,10 @@
 import numpy as np
-from gym_urbandriving.utils.PID import PIDController
+from gym_urbandriving.utils import PIDController
 from gym_urbandriving.agents import PursuitAgent
 from gym_urbandriving.planning import VelocityMPCPlanner,GeometricPlanner
 from copy import deepcopy
 import gym_urbandriving as uds
 from gym_urbandriving.actions import SteeringAction
-import IPython
 
 class SteeringActionAgent(PursuitAgent):
     """

diff --git a/gym_urbandriving/agents/hierarchical/velocity_action_agent.py b/gym_urbandriving/agents/hierarchical/velocity_action_agent.py
@@ -1,5 +1,5 @@
 import numpy as np
-from gym_urbandriving.utils.PID import PIDController
+from gym_urbandriving.utils import PIDController
 from gym_urbandriving.agents import PursuitAgent
 from gym_urbandriving.planning import VelocityMPCPlanner,GeometricPlanner
 from copy import deepcopy

diff --git a/gym_urbandriving/agents/supervisor/steering_supervisor.py b/gym_urbandriving/agents/supervisor/steering_supervisor.py
@@ -1,10 +1,9 @@
 import numpy as np
-from gym_urbandriving.utils.PID import PIDController
+from gym_urbandriving.utils import PIDController
 from gym_urbandriving.agents import PursuitAgent
 from gym_urbandriving.planning import VelocityMPCPlanner,GeometricPlanner
 from copy import deepcopy
 import gym_urbandriving as uds
-import IPython
 
 class SteeringSupervisor(PursuitAgent):
     """

diff --git a/gym_urbandriving/agents/supervisor/velocity_supervisor.py b/gym_urbandriving/agents/supervisor/velocity_supervisor.py
@@ -1,5 +1,5 @@
 import numpy as np
-from gym_urbandriving.utils.PID import PIDController
+from gym_urbandriving.utils import PIDController
 from gym_urbandriving.agents import PursuitAgent
 from gym_urbandriving.planning import VelocityMPCPlanner,GeometricPlanner
 from copy import deepcopy

diff --git a/gym_urbandriving/assets/traffic_light.py b/gym_urbandriving/assets/traffic_light.py
@@ -20,7 +20,7 @@ class TrafficLight(Rectangle):
               "red" : "traffic_red.png"}
     def __init__(self, x, y, angle=0, init_color="green", angle_deg=0, time_in_color = 0):
         angle = np.deg2rad(angle_deg % 360) if angle_deg else angle
-        Rectangle.__init__(self, x, y, 15, 15, angle)
+        Rectangle.__init__(self, x, y, 15, 25, angle)
         self.time_in_color = time_in_color
         self.color = init_color
 

diff --git a/gym_urbandriving/envs/urbandriving_env.py b/gym_urbandriving/envs/urbandriving_env.py
@@ -3,7 +3,7 @@
 from gym_urbandriving.agents import *
 import gym_urbandriving as uds
 import numpy as np
-import IPython
+
 from gym_urbandriving.assets import Car, TrafficLight, CrosswalkLight
 from gym_urbandriving.utils.featurizer import Featurizer
 

diff --git a/gym_urbandriving/planning/__init__.py b/gym_urbandriving/planning/__init__.py
@@ -1,11 +1,4 @@
-from gym_urbandriving.planning.trajectory import Trajectory
 from gym_urbandriving.planning.vel_mpc_planner import VelocityMPCPlanner
 from gym_urbandriving.planning.ped_vel_planner import PedestrianVelPlanner
-
-
-try:
-    from gym_urbandriving.planning.rrt_multi_planner import RRTMPlanner
-    from gym_urbandriving.planning.geometric_planner import GeometricPlanner
-except ImportError:
-    from gym_urbandriving.planning.geometric_planner_lite import GeometricPlanner
-    print("OMPL not supported. RRT unavailable")
+from gym_urbandriving.planning.geometric_planner_lite import GeometricPlanner
+#from gym_urbandriving.planning.geometric_planner import GeometricPlanner
diff --git a/gym_urbandriving/planning/geometric_planner.py b/gym_urbandriving/planning/geometric_planner.py
@@ -3,7 +3,7 @@
 from ompl import geometric as og
 import numpy as np
 from copy import deepcopy
-from gym_urbandriving.planning import Trajectory
+from gym_urbandriving.utils import Trajectory
 
 import scipy as sc
 import scipy.interpolate

diff --git a/gym_urbandriving/planning/geometric_planner_lite.py b/gym_urbandriving/planning/geometric_planner_lite.py
@@ -1,6 +1,6 @@
 import numpy as np
 import math
-from gym_urbandriving.planning import Trajectory
+from gym_urbandriving.utils import Trajectory
 
 class GeometricPlanner:
     """
@@ -15,7 +15,23 @@ class GeometricPlanner:
     #TODO: remove other parameters when we end up ditching OMPL
     def __init__(self,state, inter_point_d=1.0, planning_time=1.0, optional_targets = None, num_cars = 0):
         if optional_targets is None:
-            self.optional_targets = [[450,375,-np.pi/2],
+            self.optional_targets_left = [[475,375,-np.pi/2],
+                           [525,375,np.pi/2],
+                           [625,475,-np.pi],
+                           [625,525,0.0],
+                           [475,625,-np.pi/2],
+                           [525,625,np.pi/2],
+                           [375,475,-np.pi],
+                           [375,525,0.0]]
+            self.optional_targets_right = [[440,375,-np.pi/2],
+                           [560,375,np.pi/2],
+                           [625,440,-np.pi],
+                           [625,560,0.0],
+                           [440,625,-np.pi/2],
+                           [560,625,np.pi/2],
+                           [375,440,-np.pi],
+                           [375,560,0.0]]
+            self.optional_targets_straight = [[450,375,-np.pi/2],
                            [550,375,np.pi/2],
                            [625,450,-np.pi],
                            [625,550,0.0],
@@ -33,8 +49,8 @@ def interpolate(p0,p1,p2,p3,t):
 
         distance_between_points = math.sqrt((x0-x1)**2+(y0-y1)**2)
         p0 = [x0,y0]
-        p1 = [x0+.5*distance_between_points*np.cos(a0), y0 - .5*distance_between_points*np.sin(a0)]
-        p2 = [x1-.5*distance_between_points*np.cos(a1), y1 + .5*distance_between_points*np.sin(a1)]
+        p1 = [x0+.3*distance_between_points*np.cos(a0), y0 - .3*distance_between_points*np.sin(a0)]
+        p2 = [x1-.3*distance_between_points*np.cos(a1), y1 + .3*distance_between_points*np.sin(a1)]
         p3 = [x1,y1]
 
         first_point = interpolate(p0,p1,p2,p3,0)
@@ -54,10 +70,16 @@ def interpolate(p0,p1,p2,p3,t):
     def plan_for_agents(self, state,type_of_agent='background_cars',agent_num=0):
 
         obj =  state.dynamic_objects[type_of_agent][str(agent_num)]
-
-        target1 = sorted(self.optional_targets, key=lambda p: (p[0]-obj.x)**2 + (p[1]-obj.y)**2)[0]
-        target2 = sorted(self.optional_targets, key=lambda p: (p[0]-obj.destination[0])**2 + (p[1]-obj.destination[1])**2)[0]
-
+        if abs(((obj.destination[3]- obj.angle)+2*np.pi)%(2*np.pi)-(np.pi)/2)<np.pi/4:
+            target1 = sorted(self.optional_targets_left, key=lambda p: (p[0]-obj.x)**2 + (p[1]-obj.y)**2)[0]
+            target2 = sorted(self.optional_targets_left, key=lambda p: (p[0]-obj.destination[0])**2 + (p[1]-obj.destination[1])**2)[0]
+        elif abs(((obj.destination[3]- obj.angle)+2*np.pi)%(2*np.pi)-(3*np.pi)/2)<np.pi/4:
+            target1 = sorted(self.optional_targets_right, key=lambda p: (p[0]-obj.x)**2 + (p[1]-obj.y)**2)[0]
+            target2 = sorted(self.optional_targets_right, key=lambda p: (p[0]-obj.destination[0])**2 + (p[1]-obj.destination[1])**2)[0]
+        else:
+            target1 = sorted(self.optional_targets_straight, key=lambda p: (p[0]-obj.x)**2 + (p[1]-obj.y)**2)[0]
+            target2 = sorted(self.optional_targets_straight, key=lambda p: (p[0]-obj.destination[0])**2 + (p[1]-obj.destination[1])**2)[0]
+
         traj = Trajectory(mode = 'xyv')
         for p in self.plan(obj.x, obj.y, obj.vel, obj.angle, target1[0], target1[1], 1, target1[2]):
             traj.add_point(p)