Merge remote-tracking branch 'origin/figures'

bestrobot.py

 from scipy import log, sqrt, linspace, logspace, meshgrid, zeros, absolute
 from scipy.optimize import minimize
+import numpy as np
+
 
 def isstable(model):
-    return 2*model[1]*model[2] + model[2]*model[2] - 2*model[0]
+    """
+
+    :param model: (F,G,H)
+    :return: 2GH + H^2 - 2F, > 0 if stable; < 0 if unstable
+    """
+    return 2 * model[1] * model[2] + model[2] * model[2] - 2 * model[0]
 
 def tf((f,g,h), w):
     """
@@ -60,97 +67,139 @@ def kstable(human, robot, w, eta=2):
         that can be string-stabilized by a single robot car
         for an oscillatory perturbation of frequency w
     """
-    return -tflogmag(robot,w)/tflogmag(human,w)
+    return -tflogmag(robot, w) / tflogmag(human, w)
+
 
 def ksafe(human, robot, w, eta):
     """
-    Return the maximum number of human cars 
+    Return the maximum number of human cars
         that can be "safely" followed by a single robot car
         for an oscillatory perturbation of frequency w
-        by 
+        by a safety parameter eta
+    :param human:
+    :param robot:
+    :param w:
+    :param eta: safety margin
+    :return:
     """
-    return (log(eta)-htflogmag(robot,w)) / tflogmag(human,w)
+    return (log(eta) - htflogmag(robot, w)) / tflogmag(human, w)
+
 
 TOL = 1e-6
 
 def maxkfn(human, robot, eta, kfn):
+    """
+    Find the maximum number of humans cars that can be handled by a
+    single robot car for any frequency (all w)
+    :param human:
+    :param robot:
+    :param eta:
+    :param kfn: function { ksafe for safety | kstable for string stability }
+    :return:
+    """
     f,g,h = human
     maxw = sqrt(-isstable(human))
     return minimize(lambda w : kfn(human, robot, w, eta), 1, bounds=[(TOL,maxw-TOL)])#.fun[0]
 
 def maxk(human, robot, eta):
+    """
+    Min of maxksafe and maxkstable
+    :param human:
+    :param robot:
+    :param eta:
+    :return:
+    """
     return min(maxkfn(human, robot, eta, kstable), maxkfn(human, robot, eta, ksafe))
 
-'''
-def bestrobotstable(human):
-    cons = ({'type': 'ineq', 'fun': lambda x: 2*x[1]*x[2] + x[2]*x[2] - 2*x[0] },)
-    return minimize(lambda robot : -maxkstable(human, robot), 
-            (0.1, 0, 0.1), 
-            bounds=[(0, 0.2), (0, 0), (0, 0.2)],
-            constraints=cons)
-
-def bestrobotsafe(human, eta):
-    cons = ({'type': 'ineq', 'fun': lambda x: 2*x[1]*x[2] + x[2]*x[2] - 2*x[0] },)
-    return minimize(lambda robot : -maxksafe(human, robot, eta), 
-            (0.1, 0, 0.1), 
-            bounds=[(0, 0.2), (0, 0), (0, 0.2)],
-            constraints=cons)
-'''
-
-def bestrobot(human, eta):
-    #cons = ({'type': 'ineq', 'fun': lambda x: 2*x[1]*x[2] + x[2]*x[2] - 2*x[0] },)
-    return minimize(lambda robot : -maxk(human, robot, eta).fun[0], 
-            [0.001, 0, 0.001], 
-            #constraints=cons,
-            bounds=[(0, 0.2), (0, 0), (0, 0.2)])
+
+def best_robot_at_w(human, w, optfun, eta=2, fbound=(0, 0.2), gbound=(0, 0.2),
+                    hbound=(0, 0.2)):
+    cons = ({'type': 'ineq', 'fun': lambda x: isstable(x)},)
+    return minimize(lambda robot: -optfun(human, robot, w, eta),
+                    [0.001, 0.001, 0.001], constraints=cons,
+                    bounds=[fbound, gbound, hbound])
+
+
+def bestrobot(human, eta=2, fbound=(0, 0.2), gbound=(0, 0.2), hbound=(0, 0.2)):
+    cons = ({'type': 'ineq', 'fun': lambda x: isstable(x)},)
+    mink = minimize(lambda robot: -maxk(human, robot, eta).fun[0],
+                    [0.001, 0.001, 0.001], constraints=cons,
+                    bounds=[fbound, gbound, hbound])
+    # robot = mink.x
+    # print maxksafe(human, robot, eta=eta)
+    # print maxkstable(human, robot, eta=eta)
+    return mink
+
+
+def plot_kmax_vs_w(human, eta=2):
+    """
+    I think this plot is not meaningful (after the fact)
+    :param human:
+    :param eta:
+    :return:
+    """
+    # ws = linspace(0,1,10)
+
+    maxw = sqrt(-isstable(human))
+
+    ws = linspace(TOL, maxw - TOL, 101)
+    kstablerobots = [-best_robot_at_w(human, w, kstable).fun for w in ws]
+    ksaferobots = [-best_robot_at_w(human, w, ksafe, eta).fun for w in ws]
+    # kst = kstable(human, robot, ws)
+    # ksf = ksafe(human, robot, ws, eta)
+    # print "kstable = ", kstable(human, robot, ws)
+    print ksaferobots, kstablerobots
+    plt.plot(ws, kstablerobots, 'k.')
+    plt.plot(ws, ksaferobots, 'r.')
+    plt.axis([0, maxw, 0, 200])
+    plt.show()
+
+
+def plot_kmaxs_vs_FGH(human, robotopt, eta=2, res=100, fbound=(0, 0.2),
+                      gbound=(0, 0.2), hbound=(0, 0.3)):
+    """
+    How do the two optimality conditions (stability and safety) vary as the
+    robot parameters are deviated from the optimal robot controller?
+    :param human:
+    :param robotopt: (F,G,H) for the optimal robot controller given human
+    :param eta:
+    :param res: resolution for plotting, how fine to plot along the x-axis
+    :param fbound:
+    :param gbound:
+    :param hbound:
+    :return:
+    """
+    robotFs = linspace(fbound[0], fbound[1], res)
+    robotGs = linspace(gbound[0], gbound[1], res)
+    robotHs = linspace(hbound[0], hbound[1], res)
+
+    for i, robotXs in enumerate([robotFs, robotGs, robotHs]):
+        robots = np.tile(robotopt, (res, 1))
+        robots[:, i] = robotXs
+        kstablerobots = np.array(
+            [maxkfn(human, robot, eta, kstable).fun[0] for robot in robots])
+        ksaferobots = [maxkfn(human, robot, eta, ksafe).fun[0] for robot in robots]
+        kstablerobots[kstablerobots < 0] = 0
+        plt.plot(robotXs, kstablerobots, 'k.')
+        plt.plot(robotXs, ksaferobots, 'r.')
+        plt.vlines(robotopt[i], 0, max(max(kstablerobots), max(ksaferobots)))
+        plt.axis([0, max(robotXs), 0, 100])
+        plt.show()
+
 
 if __name__ == "__main__":
     from mpl_toolkits.mplot3d import Axes3D
     from matplotlib import pyplot as plt
     from cfm import IDM, CFM
 
-    hmodel = IDM(a=0.3,b=3,t=1.5,s0=2,v0=30)
+    hmodel = IDM(a=0.3, b=3, t=1.5, s0=2, v0=30)
     hmodel.go(0.5)
     human = (hmodel.f, hmodel.g, hmodel.h)
 
     maxw = sqrt(-isstable(human))
 
-    eta=2
-
-    print bestrobot(human, eta)
-
-    '''
-    ws = linspace(TOL, maxw-TOL, 101)
-    kst = kstable(human, robot, ws)
-    ksf = ksafe(human, robot, ws, eta)
-    # print "kstable = ", kstable(human, robot, ws)
-    plt.plot(ws, kst)
-    plt.plot(ws, ksf)
-    plt.axis([0, maxw, 0, 100])
-    plt.show()
-
-    frng = linspace(0, 0.1, 21)
-    hrng = linspace(0, 0.1, 21)
-    fs, hs = meshgrid(frng, hrng)
-    ks = zeros(fs.shape)
-    for fi, f in enumerate(frng):
-        for hi, h in enumerate(hrng):
-            ks[fi, hi] = maxk(human, (f, 0, h), eta)
-
-    fig = plt.figure()
-    ax = fig.add_subplot(111, projection='3d')
-    ax.plot_surface(fs, hs, ks)
-    plt.show()
-
-    # print maxkstable(human, robot)
-    # print maxksafe(human, robot, eta)
-    print maxk(human, robot, eta)
+    eta = 2
 
-    print 
-    print "***"
-    print 
+    robotopt = bestrobot(human, eta)
+    plot_kmaxs_vs_FGH(human, robotopt.x)
 
-    print bestrobotstable(human)
-    print bestrobotsafe(human, eta)
-    # print bestrobot(human, eta)
-    '''

cfm.py

@@ -92,6 +92,9 @@ class Wang(CFM):
         self.h = 0
 
 class IDM(CFM):
+    """
+    Linearization from learning-traffic working document.
+    """
     D = 4
 
     def __init__(self, t, a, b, s0, v0):