from scipy import log, sqrt, linspace, logspace, meshgrid, zeros, absolute
from scipy.optimize import minimize
import numpy as np
def isstable(model):
"""
: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):
"""
Return the complex value of the linearized transfer function T(jw)
"""
return (g * 1j * w + f) / (-w * w + (g + h) * 1j * w + f)
def tflogmag(model, w):
"""
Return the log of the magnitude of the linearized transfer function
log[ |T(jw)| ]
"""
return log(absolute(tf(model, w)))
def htflogmag(model, w):
"""
Return the log of the magnitude of the linearized headway transfer function
log[ |1-T(jw)| ]
"""
return log(absolute(1 - tf(model, w)))
def btf(human, robot, gamma, w):
"""
Return the complex value of the linearized transfer function T(jw)
of a bilateral robot controller [ followed by a human ]:
Y(robot) = gamma Tr Y(forward) + (1-gamma) Tr Y(behind)
Y(behind) = Th Y(robot)
Y(robot) = gamma Tr Y(forward) + (1-gamma) Tr Th Y(robot)
Y(robot) = [ gamma Tr ] / [ 1 - (1-gamma) Tr Th ] Y(forward)
"""
tr = tf(robot, w)
th = tf(human, w)
t = gamma * tr / (1 - (1 - gamma) * tr * th)
return t
def btflogmag(human, robot, gamma, w):
"""
Return the log of the magnitude of the linearized transfer function
log[ |T(jw)| ]
of a bilateral robot controller [ followed by a human ]
"""
return log(absolute(btf(human, robot, gamma, w)))
def bhtflogmag(human, robot, gamma, w):
"""
Return the log of the magnitude of the linearized headway transfer function
log[ |1-T(jw)| ]
of a bilateral robot controller [ followed by a human ]
"""
return log(absolute(1 - btf(human, robot, gamma, w)))
def kstable(human, robot, w, eta=2):
"""
Return the maximum number of human cars
that can be string-stabilized by a single robot car
for an oscillatory perturbation of frequency w
"""
return -tflogmag(robot, w) / tflogmag(human, w)
def ksafe(human, robot, w, eta):
"""
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 a safety parameter eta
:param human:
:param robot:
:param w:
:param eta: safety margin
:return:
"""
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 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.go(0.5)
human = (hmodel.f, hmodel.g, hmodel.h)
maxw = sqrt(-isstable(human))
eta = 2
robotopt = bestrobot(human, eta)
plot_kmaxs_vs_FGH(human, robotopt.x)