Added bilateral robot controller

bestrobot.py

@@ -6,7 +6,7 @@ def isstable(model):
 
 def tf((f,g,h), w):
     """
-    Return the complex value of linearized transfer function T(jw)
+    Return the complex value of the linearized transfer function T(jw)
     """
     return (g*1j*w + f)/(-w*w + (g+h)*1j*w + f)
 
@@ -24,6 +24,36 @@ def htflogmag(model, w):
     """
     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