Use numpy absolute

bestrobot.py

-from scipy import log, sqrt, linspace, logspace, meshgrid, zeros
+from scipy import log, sqrt, linspace, logspace, meshgrid, zeros, absolute
 from scipy.optimize import minimize
 
 def isstable(model):
     return 2*model[1]*model[2] + model[2]*model[2] - 2*model[0]
 
-def tflogmag((f,g,h), w):
+def tf((f,g,h), w):
+    """
+    Return the complex value of 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)| ]
     """
-    #f,g,h = model.f, model.g, model.h
-    return log((g*g*w*w + f*f)/((f - w*w)**2 + (g+h)**2*w*w))/2
+    return log(absolute(tf(model, w)))
 
-def htflogmag((f,g,h), w):
+def htflogmag(model, w):
     """
     Return the log of the magnitude of the linearized headway transfer function
         log[ |1-T(jw)| ]
     """
-    #f,g,h = model.f, model.g, model.h
-    return log((h*h*w*w + w*w*w*w)/((f - w*w)**2 + (g+h)**2*w*w))/2
+    return log(absolute(1-tf(model,w)))
 
 def kstable(human, robot, w, eta=2):
     """
@@ -39,18 +43,13 @@ def ksafe(human, robot, w, eta=2):
 
 TOL = 1e-6
 
-def maxkstable(human, robot, eta=2):
+def maxkfn(human, robot, eta, kfn):
     f,g,h = human
-    maxw = sqrt(2*f - 2*g*h - h*h)
-    return minimize(lambda w : kstable(human, robot, w), 1, bounds=[(TOL,maxw-TOL)])#.fun[0]
-
-def maxksafe(human, robot, eta=2):
-    f,g,h = human
-    maxw = sqrt(2*f - 2*g*h - h*h)
-    return minimize(lambda w : ksafe(human, robot, w, eta), 1, bounds=[(TOL,maxw-TOL)])#.fun[0]
+    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):
-    return min(maxkstable(human, robot), maxksafe(human, robot, eta))
+    return min(maxkfn(human, robot, eta, kstable), maxkfn(human, robot, eta, ksafe))
 
 '''
 def bestrobotstable(human):
@@ -84,21 +83,12 @@ if __name__ == "__main__":
     hmodel.go(0.5)
     human = (hmodel.f, hmodel.g, hmodel.h)
 
-    maxw = sqrt(2*hmodel.f - 2*hmodel.g*hmodel.h - hmodel.h*hmodel.h)
-    # print "maxw = ", maxw
-    resw = sqrt(-isstable(human))
-    # print "resw = ", resw
-
-    robot = (0.018, 0, 0.19)
-    # print 2*robot[0] - 2*robot[1]*robot[2] - robot[2]*robot[2]
+    maxw = sqrt(-isstable(human))
 
     eta=2
 
     print bestrobot(human, eta)
 
-    import IPython
-    IPython.embed()
-
     '''
     ws = linspace(TOL, maxw-TOL, 101)
     kst = kstable(human, robot, ws)