New sweeps and controllers

python/acc_sweep.py

@@ -14,7 +14,7 @@
     hybridParams = copy.copy(humanParams)
     hybridParams["name"] = "hybrid"
 
-    for count in [20, 25, 30, 35, 40, 45, 50]:
+    for count in [20, 25, 30, 35, 40, 45, 50, 55, 60, 70, 80]:
 
         # IDM sweep
         if defaults.RANDOM_SEED:

python/agent_types.py

@@ -1,4 +1,7 @@
-from carfns import randomChangeLaneFn, ACCFnBuilder, changeFasterLaneBuilder, MidpointFnBuilder, SwitchVTypeFn
+import copy
+
+from carfns import randomChangeLaneFn, ACCFnBuilder, changeFasterLaneBuilder,\
+    MidpointFnBuilder, SwitchVTypeFn, FillGapFnBuilder, FillGapMidpointFnBuilder
 import config as defaults
 
 changeFasterLane = changeFasterLaneBuilder()
@@ -19,37 +22,39 @@
     # "laneChangeModel": 'LC2013',
 }
 
-# In the literature, the IDM model is used as adaptive cruise control
-basicIDMParams = {
-    "name"        : "idm",
-    "count"       :  0,
-    "maxSpeed"    :  defaults.MAX_SPEED,
-    "accel"       :   4,
-    "decel"       :   6,
-    "carFollowModel": "IDM",
-    "laneSpread"  : 0,
-    "tau"         : 0.5,
-}
-
-basicACCParams = {
-    "name"        : "acc",
-    "count"       :  0,
-    "maxSpeed"    :  defaults.MAX_SPEED,
-    "accel"       :   4,
-    "decel"       :   6,
-    # "function"    : MidpointFnBuilder(max_speed=40, gain=0.1, beta=0.9, duration=250, bias=1.0, ratio=0.25),
-    "function"    : ACCFnBuilder(follow_sec=1.0, max_speed=defaults.MAX_SPEED, gain=0.1, beta=0.9),
-    "laneSpread"  : 0,
-    "tau"         : 0.5,
-}
-
 basicRobotParams = {
     "name"        : "robot",
     "count"       :  0,
     "maxSpeed"    :  defaults.MAX_SPEED,
     "accel"       :   4,
     "decel"       :   6,
-    "function"    : MidpointFnBuilder(max_speed=40, gain=0.1, beta=0.9, duration=250, bias=1.0, ratio=0.25),
     "laneSpread"  : 0,
     "tau"         : 0.5,
 }
+
+# In the literature, the IDM model is used as adaptive cruise control
+basicIDMParams = copy.copy(basicRobotParams)
+basicIDMParams["name"] = "idm"
+basicIDMParams["carFollowModel"] = "IDM"
+
+basicACCParams = copy.copy(basicRobotParams)
+basicACCParams["name"] = "acc"
+basicACCParams["function"] = ACCFnBuilder(follow_sec=1.0,
+                                          max_speed=defaults.MAX_SPEED,
+                                          gain=0.1, beta=0.9)
+
+basicGapFillerParams = copy.copy(basicRobotParams)
+basicGapFillerParams["name"] = "gapfiller"
+basicGapFillerParams["function"] = FillGapFnBuilder(duration=250)
+
+basicFillGapMidpointParams = copy.copy(basicRobotParams)
+basicFillGapMidpointParams["name"] = "fillgapmidpoint"
+basicFillGapMidpointParams["function"] = \
+    FillGapMidpointFnBuilder(max_speed=40, gain=0.1, beta=0.9, duration=250,
+                             bias=1.0, ratio=0.25)
+
+basicMidpointParams = copy.copy(basicRobotParams)
+basicMidpointParams["name"] = "midpoint"
+basicMidpointParams["function"] = MidpointFnBuilder(max_speed=40, gain=0.1,
+                                                    beta=0.9, duration=250,
+                                                    bias=1.0, ratio=0.25)

python/carfns.py

@@ -9,6 +9,7 @@ def randomChangeLaneFn((idx, car), sim, step):
     if random.random() > .99:
         traci.vehicle.changeLane(car["id"], 1-li, 1000)
 
+
 def changeFasterLaneBuilder(speedThreshold = 5, likelihood = 0.5, 
                             dxBack = 0, dxForward = 60, 
                             gapBack = 10, gapForward = 5):
@@ -44,6 +45,7 @@ def carFn((idx, car), sim, step):
             traci.vehicle.changeLane(car["id"], maxl, 10000)
     return carFn
 
+
 def ACCFnBuilder(follow_sec = 3.0, max_speed = 26.8, gain = 0.01, beta = 0.5):
     """
     Basic adaptive cruise control (ACC) controller
@@ -67,18 +69,13 @@ def ACCFn((idx, car), sim, step):
         # TODO(cathywu) Setting tau to any value seems to cause collisions
         # traci.vehicle.setTau(vehID, 0.01)
 
-        if step < sim.simSteps/2:
-            # changeFasterLaneBuilder()((idx, car), sim, step)
-            return
-
         try:
             [back_car, front_car] = sim.getCars(idx, numBack=1, numForward=1, lane=car["lane"])
         except ValueError:
             # Not enough cars on lane
             return
 
         front_dist = (front_car["x"] - car["x"]) % sim.length
-        back_dist = (car["x"] - back_car["x"]) % sim.length
 
         curr_speed = car["v"]
         front_speed = front_car["v"]
@@ -100,6 +97,7 @@ def ACCFn((idx, car), sim, step):
 
     return ACCFn
 
+
 def MidpointFnBuilder(max_speed = 26.8, gain = 0.1, beta = 0.5, duration = 500, bias = 1.0, ratio = 0.5):
     """
     Basic adaptive cruise control (ACC) controller
@@ -152,6 +150,118 @@ def MidpointFn((idx, car), sim, step):
 
     return MidpointFn
 
+
+def FillGapFnBuilder(duration=500, gap_back=10, gap_forward=5, gap_threshold=10):
+    """
+    Opportunistically filled gaps in neighboring lanes
+    :param duration: duration for transitioning to new speed (ms)
+    :param gap_back: Minimum required clearance behind car
+    :param gap_forward: Minimum required clearance in front car
+    :param gap_threshold: Minimum required gap difference between current lane and next lane
+    :return: carFn to input to a carParams
+    """
+    def carFn((idx, car), sim, step):
+        gap = [0] * sim.numLanes
+        new_speed = [0] * sim.numLanes
+        for lane in range(sim.numLanes):
+            if sim.getCars(idx, dxBack=gap_back, dxForward=gap_forward, lane=lane):
+                # cars too close, no lane changing allowed
+                gap[lane] = 0
+                continue
+
+            try:
+                [back_car, front_car] = sim.getCars(idx, numBack=1, numForward=1, lane=car["lane"])
+            except ValueError:
+                # Not enough cars on lane
+                gap[lane] = 0
+                continue
+
+            gap[lane] = (front_car["x"] - back_car["x"]) % sim.length
+            new_speed[lane] = (front_car["v"] + back_car["v"]) / 2
+        max_gap = max(gap)
+        max_lane = gap.index(max_gap)
+
+        if max_lane != car["lane"] and max_gap-gap[car["lane"]] > gap_threshold:
+            traci.vehicle.slowDown(car["id"], new_speed[max_lane], duration)
+            traci.vehicle.changeLane(car["id"], max_lane, 10000)
+
+    return carFn
+
+
+def FillGapMidpointFnBuilder(duration=500, gap_back=10, gap_forward=5,
+                             gap_threshold=10, max_speed=26.8, gain=0.1,
+                             beta=0.5, bias=1.0, ratio=0.5):
+    """
+    Opportunistically filled gaps in neighboring lanes
+    :param duration: duration for transitioning to new speed (ms)
+    :param gap_back: Minimum required clearance behind car
+    :param gap_forward: Minimum required clearance in front car
+    :param gap_threshold: Minimum required gap difference between current lane and next lane
+    :param max_speed: 26.8 m/s = 60 mph
+    :param gain: gain for tracking following distance
+    :param beta: gain for tracking speed of front vehicle
+    :param bias: additive speed bias term (m/s)
+    :param ratio: ratio of distance between front and back vehicles to track
+            as following distance (default is 0.5=midpoint)
+    :return: carFn to input to a carParams
+    """
+    def carFn((idx, car), sim, step):
+        gap = [0] * sim.numLanes
+        new_speed = [0] * sim.numLanes
+        for lane in range(sim.numLanes):
+            if sim.getCars(idx, dxBack=gap_back, dxForward=gap_forward, lane=lane):
+                # cars too close, no lane changing allowed
+                gap[lane] = 0
+                continue
+
+            try:
+                [back_car, front_car] = sim.getCars(idx, numBack=1, numForward=1, lane=car["lane"])
+            except ValueError:
+                # Not enough cars on lane
+                gap[lane] = 0
+                continue
+
+            gap[lane] = (front_car["x"] - back_car["x"]) % sim.length
+            new_speed[lane] = (front_car["v"] + back_car["v"]) / 2
+        max_gap = max(gap)
+        max_lane = gap.index(max_gap)
+
+        if max_lane != car["lane"] and max_gap-gap[car["lane"]] > gap_threshold:
+            traci.vehicle.slowDown(car["id"], new_speed[max_lane], duration)
+            traci.vehicle.changeLane(car["id"], max_lane, 10000)
+        else:
+            vehID = car["id"]
+
+            try:
+                [back_car, front_car] = sim.getCars(idx, numBack=1, numForward=1, lane=car["lane"])
+            except ValueError:
+                # Not enough cars on lane
+                return
+
+            front_dist = (front_car["x"] - car["x"]) % sim.length
+            back_dist = (car["x"] - back_car["x"]) % sim.length
+
+            curr_speed = car["v"]
+            front_speed = front_car["v"]
+            follow_dist = (front_dist + back_dist) * ratio
+            delta = front_dist - follow_dist
+            # print delta, curr_speed, front_speed, curr_speed-front_speed
+            if follow_dist < front_dist and curr_speed < max_speed:
+                # speed up
+                new_speed = min(curr_speed + beta * (front_speed-curr_speed) + gain * delta + bias, max_speed)
+                traci.vehicle.slowDown(vehID, new_speed, duration) # 2.5 sec
+                # print "t=%d, FASTER, %0.1f -> %0.1f (%0.1f) | d=%0.2f = %0.2f vs %0.2f" % \
+                #       (step, curr_speed, new_speed, front_speed, delta, front_dist, follow_dist)
+            elif follow_dist > front_dist:
+                # slow down
+                new_speed = max(curr_speed + beta * (front_speed-curr_speed) + gain * delta + bias, 0)
+                traci.vehicle.slowDown(vehID, new_speed, duration) # 2.5 sec
+                # print "t=%d, SLOWER, %0.1f -> %0.1f (%0.1f) | d=%0.2f = %0.2f vs %0.2f" % \
+                #       (step, curr_speed, new_speed, front_speed, delta, front_dist, follow_dist)
+
+    return carFn
+
+
 def SwitchVTypeFn(car_type, switch_point, initCarFn=None):
     """
     Switches vehicle type from initialized to car_type.
@@ -172,6 +282,7 @@ def CarFn((idx, car), sim, step):
 
     return CarFn
 
+
 def SwitchFn(switchList):
     """
     Switches between car functions

python/few_robots_sweep.py

@@ -0,0 +1,57 @@
+import copy
+import random
+
+from carfns import SwitchVTypeFn, changeFasterLaneBuilder
+from loopsim import LoopSim
+from agent_types import basicHumanParams as humanParams, \
+    basicIDMParams as IDMParams, basicACCParams as ACCParams, \
+    basicGapFillerParams as gapFillerParams, \
+    basicFillGapMidpointParams as fillGapMidpointParams,\
+    basicMidpointParams as midpointParams
+import config as defaults
+
+
+# this is the main entry point of this script
+if __name__ == "__main__":
+    # This sweep demonstrates that our ACC implementation is about as good as
+    # IDM in the loop setting
+    simStepLength = 0.5
+    numLanes = 2
+    length = 1000
+    simSteps = 500
+
+    nRobotss = [40, 60]
+    vtypes = ["idm", "acc", "midpoint", "gapfiller", "fillgapmidpoint"]
+
+    humanParams["count"] = 50
+    changeFasterLane = changeFasterLaneBuilder()
+    hybridParams = copy.copy(humanParams)
+    hybridParams["name"] = "hybrid"
+
+    # for nRobots in range(1, 22, 3):
+    for nRobots in nRobotss:
+        for vtype in vtypes:
+            # Sweep through each type
+
+            # reset the random seed so that the first half of the sim is held constant per nRobots
+            if defaults.RANDOM_SEED:
+                print "Setting random seed to ", defaults.RANDOM_SEED
+                random.seed(defaults.RANDOM_SEED)
+
+            hybridParams["count"] = nRobots
+            hybridParams["function"] = SwitchVTypeFn(vtype, 0.5,
+                                                     initCarFn=changeFasterLane)
+
+            opts = {
+                "paramsList" : [humanParams, IDMParams, ACCParams,
+                                gapFillerParams, fillGapMidpointParams,
+                                hybridParams, midpointParams],
+                "simSteps"   : simSteps,
+                "tag"        : "few-%s-sweep" % vtype,
+            }
+
+            sim = LoopSim("loopsim", length=length, numLanes=numLanes,
+                          simStepLength=simStepLength)
+
+            sim.simulate(opts)
+            sim.plot(show=True, save=True)

python/sugiyama_jam.py

@@ -0,0 +1,41 @@
+import random
+
+import config as defaults
+from loopsim import LoopSim
+from agent_types import basicHumanParams as humanParams, \
+    basicIDMParams as IDMParams, basicACCParams as ACCParams, \
+    basicGapFillerParams as gapFillerParams, \
+    basicFillGapMidpointParams as fillGapMidpointParams, \
+    basicMidpointParams as midpointParams
+
+
+if __name__ == "__main__":
+    numLanes = 1
+    vTypeParams = humanParams
+    vTypeParams["tau"] = 0.5
+    vTypeParams["sigma"] = 0.5
+    vTypeParams["speedDev"] = 0
+    vTypeParams["speedFactor"] = 1
+
+    # vTypeParams = fillGapMidpointParams
+    # vTypeParams = gapFillerParams
+    # vTypeParams = midpointParams
+    # vTypeParams = IDMParams
+    # vTypeParams = ACCParams
+
+    if defaults.RANDOM_SEED:
+        print "Setting random seed to ", defaults.RANDOM_SEED
+        random.seed(defaults.RANDOM_SEED)
+
+    vTypeParams["count"] = 22 * numLanes
+
+    opts = {
+        "paramsList" : [vTypeParams],
+        "simSteps"   : 500,
+        "tag"        : "SugiyamaJam",
+    }
+
+    sim = LoopSim("loopsim", length=230, numLanes=numLanes, simStepLength=0.5)
+
+    sim.simulate(opts)
+    sim.plot(show=True, save=True)