Added control loops for all subsystems and made tests run.

Change-Id: I66542db4355a89f6d24c1ad4772004182197c863
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index b5ea6a1..1a25f95 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -6,357 +6,99 @@
 import polydrivetrain
 import numpy
 import sys
+import matplotlib
 from matplotlib import pylab
 
 class Claw(control_loop.ControlLoop):
-  def __init__(self, name="RawClaw"):
+  def __init__(self, name="Claw", mass=None):
     super(Claw, self).__init__(name)
     # Stall Torque in N m
-    self.stall_torque = 2.42
+    self.stall_torque = 0.476
     # Stall Current in Amps
-    self.stall_current = 133
+    self.stall_current = 80.730
     # Free Speed in RPM
-    self.free_speed = 5500.0
+    self.free_speed = 13906.0
     # Free Current in Amps
-    self.free_current = 2.7
-    # Moment of inertia of the claw in kg m^2
-    self.J_top = 2.8
-    self.J_bottom = 3.0
+    self.free_current = 5.820
+    # Mass of the claw
+    if mass is None:
+      self.mass = 5.0
+    else:
+      self.mass = mass
 
     # Resistance of the motor
     self.R = 12.0 / self.stall_current
     # Motor velocity constant
     self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
-               (13.5 - self.R * self.free_current))
+               (12.0 - self.R * self.free_current))
     # Torque constant
     self.Kt = self.stall_torque / self.stall_current
     # Gear ratio
-    self.G = 14.0 / 48.0 * 18.0 / 32.0 * 18.0 / 66.0 * 12.0 / 60.0
-    # Control loop time step
-    self.dt = 0.01
+    self.G = (56.0 / 12.0) * (54.0 / 14.0) * (64.0 / 14.0) * (72.0 / 18.0)
+    # Claw length
+    self.r = 18 * 0.0254
 
-    # State is [bottom position, bottom velocity, top - bottom position,
-    #           top - bottom velocity]
-    # Input is [bottom power, top power - bottom power * J_top / J_bottom]
-    # Motor time constants. difference_bottom refers to the constant for how the
-    # bottom velocity affects the difference of the top and bottom velocities.
-    self.common_motor_constant = -self.Kt / self.Kv / (self.G * self.G * self.R)
-    self.bottom_bottom = self.common_motor_constant / self.J_bottom
-    self.difference_bottom = -self.common_motor_constant * (1 / self.J_bottom
-                                                        - 1 / self.J_top)
-    self.difference_difference = self.common_motor_constant / self.J_top
-    # State feedback matrices
+    self.J = self.r * self.mass
+
+    # Control loop time step
+    self.dt = 0.005
+
+    # State is [position, velocity]
+    # Input is [Voltage]
+
+    C1 = self.G * self.G * self.Kt / (self.R  * self.J * self.Kv)
+    C2 = self.Kt * self.G / (self.J * self.R)
 
     self.A_continuous = numpy.matrix(
-        [[0, 0, 1, 0],
-         [0, 0, 0, 1],
-         [0, 0, self.bottom_bottom, 0],
-         [0, 0, self.difference_bottom, self.difference_difference]])
-
-    self.A_bottom_cont = numpy.matrix(
         [[0, 1],
-         [0, self.bottom_bottom]])
+         [0, -C1]])
 
-    self.A_diff_cont = numpy.matrix(
-        [[0, 1],
-         [0, self.difference_difference]])
-
-    self.motor_feedforward = self.Kt / (self.G * self.R)
-    self.motor_feedforward_bottom = self.motor_feedforward / self.J_bottom
-    self.motor_feedforward_difference = self.motor_feedforward / self.J_top
-    self.motor_feedforward_difference_bottom = (
-        self.motor_feedforward * (1 / self.J_bottom - 1 / self.J_top))
+    # Start with the unmodified input
     self.B_continuous = numpy.matrix(
-        [[0, 0],
-         [0, 0],
-         [self.motor_feedforward_bottom, 0],
-         [-self.motor_feedforward_bottom, self.motor_feedforward_difference]])
-
-    print "Cont X_ss", self.motor_feedforward / -self.common_motor_constant
-
-    self.B_bottom_cont = numpy.matrix(
         [[0],
-         [self.motor_feedforward_bottom]])
+         [C2]])
 
-    self.B_diff_cont = numpy.matrix(
-        [[0],
-         [self.motor_feedforward_difference]])
-
-    self.C = numpy.matrix([[1, 0, 0, 0],
-                           [1, 1, 0, 0]])
-    self.D = numpy.matrix([[0, 0],
-                           [0, 0]])
+    self.C = numpy.matrix([[1, 0]])
+    self.D = numpy.matrix([[0]])
 
     self.A, self.B = self.ContinuousToDiscrete(
         self.A_continuous, self.B_continuous, self.dt)
 
-    self.A_bottom, self.B_bottom = controls.c2d(
-        self.A_bottom_cont, self.B_bottom_cont, self.dt)
-    self.A_diff, self.B_diff = controls.c2d(
-        self.A_diff_cont, self.B_diff_cont, self.dt)
-
-    self.K_bottom = controls.dplace(self.A_bottom, self.B_bottom, [.75 + 0.1j, .75 - 0.1j])
-    self.K_diff = controls.dplace(self.A_diff, self.B_diff, [.75 + 0.1j, .75 - 0.1j])
-
-    print "K_diff", self.K_diff
-    print "K_bottom", self.K_bottom
-
-    print "A"
     print self.A
-    print "B"
-    print self.B
-
-    
-    self.Q = numpy.matrix([[(1.0 / (0.10 ** 2.0)), 0.0, 0.0, 0.0],
-                           [0.0, (1.0 / (0.06 ** 2.0)), 0.0, 0.0],
-                           [0.0, 0.0, 0.10, 0.0],
-                           [0.0, 0.0, 0.0, 0.1]])
-
-    self.R = numpy.matrix([[(1.0 / (40.0 ** 2.0)), 0.0],
-                           [0.0, (1.0 / (5.0 ** 2.0))]])
-    #self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
-
-    self.K = numpy.matrix([[self.K_bottom[0, 0], 0.0, self.K_bottom[0, 1], 0.0],
-                           [0.0, self.K_diff[0, 0], 0.0, self.K_diff[0, 1]]])
-
-    # Compute the feed forwards aceleration term.
-    self.K[1, 0] = -self.B[1, 0] * self.K[0, 0] / self.B[1, 1]
-
-    lstsq_A = numpy.identity(2)
-    lstsq_A[0, :] = self.B[1, :]
-    lstsq_A[1, :] = self.B[3, :]
-    print "System of Equations coefficients:"
-    print lstsq_A
-    print "det", numpy.linalg.det(lstsq_A)
-
-    out_x = numpy.linalg.lstsq(
-                         lstsq_A,
-                         numpy.matrix([[self.A[1, 2]], [self.A[3, 2]]]))[0]
-    self.K[1, 2] = -lstsq_A[0, 0] * (self.K[0, 2] - out_x[0]) / lstsq_A[0, 1] + out_x[1]
-
-    print "K unaugmented"
-    print self.K
-    print "B * K unaugmented"
-    print self.B * self.K
-    F = self.A - self.B * self.K
-    print "A - B * K unaugmented"
-    print F
-    print "eigenvalues"
-    print numpy.linalg.eig(F)[0]
-
-    self.rpl = .05
-    self.ipl = 0.010
-    self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
-                             self.rpl + 1j * self.ipl,
-                             self.rpl - 1j * self.ipl,
-                             self.rpl - 1j * self.ipl])
-
-    # The box formed by U_min and U_max must encompass all possible values,
-    # or else Austin's code gets angry.
-    self.U_max = numpy.matrix([[12.0], [12.0]])
-    self.U_min = numpy.matrix([[-12.0], [-12.0]])
-
-    # For the tests that check the limits, these are (upper, lower) for both
-    # claws.
-    self.hard_pos_limits = None
-    self.pos_limits = None
-
-    # Compute the steady state velocities for a given applied voltage.
-    # The top and bottom of the claw should spin at the same rate if the
-    # physics is right.
-    X_ss = numpy.matrix([[0], [0], [0.0], [0]])
-    
-    U = numpy.matrix([[1.0],[1.0]])
-    A = self.A
-    B = self.B
-    #X_ss[2, 0] = X_ss[2, 0] * A[2, 2] + B[2, 0] * U[0, 0]
-    X_ss[2, 0] = 1 / (1 - A[2, 2]) * B[2, 0] * U[0, 0]
-    #X_ss[3, 0] = X_ss[3, 0] * A[3, 3] + X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
-    #X_ss[3, 0] * (1 - A[3, 3]) = X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
-    X_ss[3, 0] = 1 / (1 - A[3, 3]) * (X_ss[2, 0] * A[3, 2] + B[3, 1] * U[1, 0] + B[3, 0] * U[0, 0])
-    #X_ss[3, 0] = 1 / (1 - A[3, 3]) / (1 - A[2, 2]) * B[2, 0] * U[0, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
-    X_ss[0, 0] = A[0, 2] * X_ss[2, 0] + B[0, 0] * U[0, 0]
-    X_ss[1, 0] = A[1, 2] * X_ss[2, 0] + A[1, 3] * X_ss[3, 0] + B[1, 0] * U[0, 0] + B[1, 1] * U[1, 0]
-
-    print "X_ss", X_ss
-
-    self.InitializeState()
-
-
-class ClawDeltaU(Claw):
-  def __init__(self, name="Claw"):
-    super(ClawDeltaU, self).__init__(name)
-    A_unaugmented = self.A
-    B_unaugmented = self.B
-    C_unaugmented = self.C
-
-    self.A = numpy.matrix([[0.0, 0.0, 0.0, 0.0, 0.0],
-                           [0.0, 0.0, 0.0, 0.0, 0.0],
-                           [0.0, 0.0, 0.0, 0.0, 0.0],
-                           [0.0, 0.0, 0.0, 0.0, 0.0],
-                           [0.0, 0.0, 0.0, 0.0, 1.0]])
-    self.A[0:4, 0:4] = A_unaugmented
-    self.A[0:4, 4] = B_unaugmented[0:4, 0]
-
-    self.B = numpy.matrix([[0.0, 0.0],
-                           [0.0, 0.0],
-                           [0.0, 0.0],
-                           [0.0, 0.0],
-                           [1.0, 0.0]])
-    self.B[0:4, 1] = B_unaugmented[0:4, 1]
-
-    self.C = numpy.concatenate((C_unaugmented, numpy.matrix([[0.0], [0.0]])),
-                               axis=1)
-    self.D = numpy.matrix([[0.0, 0.0],
-                           [0.0, 0.0]])
-
-    #self.PlaceControllerPoles([0.55, 0.35, 0.55, 0.35, 0.80])
-    self.Q = numpy.matrix([[(1.0 / (0.04 ** 2.0)), 0.0, 0.0, 0.0, 0.0],
-                           [0.0, (1.0 / (0.01 ** 2)), 0.0, 0.0, 0.0],
-                           [0.0, 0.0, 0.01, 0.0, 0.0],
-                           [0.0, 0.0, 0.0, 0.08, 0.0],
-                           [0.0, 0.0, 0.0, 0.0, (1.0 / (10.0 ** 2))]])
-
-    self.R = numpy.matrix([[0.000001, 0.0],
-                           [0.0, 1.0 / (10.0 ** 2.0)]])
-    self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
-
-    self.K = numpy.matrix([[50.0, 0.0, 10.0, 0.0, 1.0],
-                           [50.0, 0.0, 10.0, 0.0, 1.0]])
-    #self.K = numpy.matrix([[50.0, -100.0, 0, -10, 0],
-    #                       [50.0,  100.0, 0, 10, 0]])
 
     controlability = controls.ctrb(self.A, self.B);
     print "Rank of augmented controlability matrix.", numpy.linalg.matrix_rank(controlability)
 
-    print "K"
+    q_pos = 0.03
+    q_vel = 0.5
+    self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
+                           [0.0, (1.0 / (q_vel ** 2.0))]])
+
+    self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
+    self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
     print self.K
-    print "Placed controller poles are"
+
     print numpy.linalg.eig(self.A - self.B * self.K)[0]
-    print [numpy.abs(x) for x in numpy.linalg.eig(self.A - self.B * self.K)[0]]
 
-    self.rpl = .05
-    self.ipl = 0.008
-    self.PlaceObserverPoles([self.rpl + 1j * self.ipl, 0.10, 0.09,
-                             self.rpl - 1j * self.ipl, 0.90])
-    #print "A is"
-    #print self.A
-    #print "L is"
-    #print self.L
-    #print "C is"
-    #print self.C
-    #print "A - LC is"
-    #print self.A - self.L * self.C
+    self.rpl = 0.20
+    self.ipl = 0.05
+    self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+                             self.rpl - 1j * self.ipl])
 
-    #print "Placed observer poles are"
-    #print numpy.linalg.eig(self.A - self.L * self.C)[0]
-
-    self.U_max = numpy.matrix([[12.0], [12.0]])
-    self.U_min = numpy.matrix([[-12.0], [-12.0]])
+    # The box formed by U_min and U_max must encompass all possible values,
+    # or else Austin's code gets angry.
+    self.U_max = numpy.matrix([[12.0]])
+    self.U_min = numpy.matrix([[-12.0]])
 
     self.InitializeState()
 
-def ScaleU(claw, U, K, error):
-  """Clips U as necessary.
 
-    Args:
-      claw: claw object containing moments of inertia and U limits.
-      U: Input matrix to clip as necessary.
-  """
+def run_test(claw, initial_X, goal, max_separation_error=0.01,
+             show_graph=True, iterations=200, controller_claw=None,
+             observer_claw=None):
+  """Runs the claw plant with an initial condition and goal.
 
-  bottom_u = U[0, 0]
-  top_u = U[1, 0]
-  position_error = error[0:2, 0]
-  velocity_error = error[2:, 0]
-
-  U_poly = polytope.HPolytope(
-      numpy.matrix([[1, 0],
-                    [-1, 0],
-                    [0, 1],
-                    [0, -1]]),
-      numpy.matrix([[12],
-                    [12],
-                    [12],
-                    [12]]))
-
-  if (bottom_u > claw.U_max[0, 0] or bottom_u < claw.U_min[0, 0] or
-      top_u > claw.U_max[0, 0] or top_u < claw.U_min[0, 0]):
-
-    position_K = K[:, 0:2]
-    velocity_K = K[:, 2:]
-
-    # H * U <= k
-    # U = UPos + UVel
-    # H * (UPos + UVel) <= k
-    # H * UPos <= k - H * UVel
-    #
-    # Now, we can do a coordinate transformation and say the following.
-    #
-    # UPos = position_K * position_error
-    # (H * position_K) * position_error <= k - H * UVel
-    #
-    # Add in the constraint that 0 <= t <= 1
-    # Now, there are 2 ways this can go.  Either we have a region, or we don't
-    # have a region.  If we have a region, then pick the largest t and go for it.
-    # If we don't have a region, we need to pick a good comprimise.
-
-    pos_poly = polytope.HPolytope(
-        U_poly.H * position_K,
-        U_poly.k - U_poly.H * velocity_K * velocity_error)
-
-    # The actual angle for the line we call 45.
-    angle_45 = numpy.matrix([[numpy.sqrt(3), 1]])
-    if claw.pos_limits and claw.hard_pos_limits and claw.X[0, 0] + claw.X[1, 0] > claw.pos_limits[1]:
-      angle_45 = numpy.matrix([[1, 1]])
-
-    P = position_error
-    L45 = numpy.multiply(numpy.matrix([[numpy.sign(P[1, 0]), -numpy.sign(P[0, 0])]]), angle_45)
-    if L45[0, 1] == 0:
-      L45[0, 1] = 1
-    if L45[0, 0] == 0:
-      L45[0, 0] = 1
-    w45 = numpy.matrix([[0]])
-
-    if numpy.abs(P[0, 0]) > numpy.abs(P[1, 0]):
-      LH = numpy.matrix([[0, 1]])
-    else:
-      LH = numpy.matrix([[1, 0]])
-    wh = LH * P
-    standard = numpy.concatenate((L45, LH))
-    W = numpy.concatenate((w45, wh))
-    intersection = numpy.linalg.inv(standard) * W
-    adjusted_pos_error_h, is_inside_h = polydrivetrain.DoCoerceGoal(pos_poly,
-        LH, wh, position_error)
-    adjusted_pos_error_45, is_inside_45 = polydrivetrain.DoCoerceGoal(pos_poly,
-        L45, w45, intersection)
-    if pos_poly.IsInside(intersection):
-      adjusted_pos_error = adjusted_pos_error_h
-    else:
-      if is_inside_h:
-        if numpy.linalg.norm(adjusted_pos_error_h) > numpy.linalg.norm(adjusted_pos_error_45):
-          adjusted_pos_error = adjusted_pos_error_h
-        else:
-          adjusted_pos_error = adjusted_pos_error_45
-      else:
-        adjusted_pos_error = adjusted_pos_error_45
-    #print adjusted_pos_error
-
-    #print "Actual power is ", velocity_K * velocity_error + position_K * adjusted_pos_error
-    return velocity_K * velocity_error + position_K * adjusted_pos_error
-
-    #U = Kpos * poserror + Kvel * velerror
-      
-    #scalar = claw.U_max[0, 0] / max(numpy.abs(top_u), numpy.abs(bottom_u))
-
-    #top_u *= scalar
-    #bottom_u *= scalar
-
-  return numpy.matrix([[bottom_u], [top_u]])
-
-def run_test(claw, initial_X, goal, max_separation_error=0.01, show_graph=False, iterations=200):
-  """Runs the claw plant on a given claw (claw) with an initial condition (initial_X) and goal (goal).
-
-    The tests themselves are not terribly sophisticated; I just test for 
+    The tests themselves are not terribly sophisticated; I just test for
     whether the goal has been reached and whether the separation goes
     outside of the initial and goal values by more than max_separation_error.
     Prints out something for a failure of either condition and returns
@@ -367,112 +109,76 @@
       goal: goal state.
       show_graph: Whether or not to display a graph showing the changing
            states and voltages.
-      iterations: Number of timesteps to run the model for."""
+      iterations: Number of timesteps to run the model for.
+      controller_claw: claw object to get K from, or None if we should
+          use claw.
+      observer_claw: claw object to use for the observer, or None if we should
+          use the actual state.
+  """
 
   claw.X = initial_X
 
+  if controller_claw is None:
+    controller_claw = claw
+
+  if observer_claw is not None:
+    observer_claw.X_hat = initial_X + 0.01
+    observer_claw.X_hat = initial_X
+
   # Various lists for graphing things.
   t = []
-  x_bottom = []
-  x_top = []
-  u_bottom = []
-  u_top = []
-  x_separation = []
+  x = []
+  v = []
+  x_hat = []
+  u = []
 
-  tests_passed = True
-
-  # Bounds which separation should not exceed.
-  lower_bound = (initial_X[1, 0] if initial_X[1, 0] < goal[1, 0]
-                 else goal[1, 0]) - max_separation_error
-  upper_bound = (initial_X[1, 0] if initial_X[1, 0] > goal[1, 0]
-                 else goal[1, 0]) + max_separation_error
+  sep_plot_gain = 100.0
 
   for i in xrange(iterations):
-    U = claw.K * (goal - claw.X)
-    U = ScaleU(claw, U, claw.K, goal - claw.X)
+    X_hat = claw.X
+    if observer_claw is not None:
+      X_hat = observer_claw.X_hat
+      x_hat.append(observer_claw.X_hat[0, 0])
+    U = controller_claw.K * (goal - X_hat)
+    U[0, 0] = numpy.clip(U[0, 0], -12, 12)
+    x.append(claw.X[0, 0])
+    v.append(claw.X[1, 0])
+    if observer_claw is not None:
+      observer_claw.PredictObserver(U)
     claw.Update(U)
-
-    if claw.X[1, 0] > upper_bound or claw.X[1, 0] < lower_bound:
-      tests_passed = False
-      print "Claw separation was", claw.X[1, 0]
-      print "Should have been between", lower_bound, "and", upper_bound
-
-    if claw.hard_pos_limits and \
-      (claw.X[0, 0] > claw.hard_pos_limits[1] or
-          claw.X[0, 0] < claw.hard_pos_limits[0] or
-          claw.X[0, 0] + claw.X[1, 0] > claw.hard_pos_limits[1] or
-          claw.X[0, 0] + claw.X[1, 0] < claw.hard_pos_limits[0]):
-      tests_passed = False
-      print "Claws at %f and %f" % (claw.X[0, 0], claw.X[0, 0] + claw.X[1, 0])
-      print "Both should be in %s, definitely %s" % \
-          (claw.pos_limits, claw.hard_pos_limits)
+    if observer_claw is not None:
+      observer_claw.Y = claw.Y
+      observer_claw.CorrectObserver(U)
 
     t.append(i * claw.dt)
-    x_bottom.append(claw.X[0, 0] * 10.0)
-    x_top.append((claw.X[1, 0] + claw.X[0, 0]) * 10.0)
-    u_bottom.append(U[0, 0])
-    u_top.append(U[1, 0])
-    x_separation.append(claw.X[1, 0] * 10.0)
+    u.append(U[0, 0])
 
   if show_graph:
-    pylab.plot(t, x_bottom, label='x bottom * 10')
-    pylab.plot(t, x_top, label='x top * 10')
-    pylab.plot(t, u_bottom, label='u bottom')
-    pylab.plot(t, u_top, label='u top')
-    pylab.plot(t, x_separation, label='separation * 10')
+    pylab.subplot(2, 1, 1)
+    pylab.plot(t, x, label='x')
+    if observer_claw is not None:
+      pylab.plot(t, x_hat, label='x_hat')
+    pylab.legend()
+
+    pylab.subplot(2, 1, 2)
+    pylab.plot(t, u, label='u')
     pylab.legend()
     pylab.show()
 
-  # Test to make sure that we are near the goal.
-  if numpy.max(abs(claw.X - goal)) > 1e-4:
-    tests_passed = False
-    print "X was", claw.X, "Expected", goal
-
-  return tests_passed
 
 def main(argv):
-  claw = Claw()
+  loaded_mass = 5
+  #loaded_mass = 0
+  claw = Claw(mass=5 + loaded_mass)
+  claw_controller = Claw(mass=5 + 5)
+  observer_claw = Claw(mass=5 + 5)
+  #observer_claw = None
 
   # Test moving the claw with constant separation.
-  initial_X = numpy.matrix([[-1.0], [0.0], [0.0], [0.0]])
-  R = numpy.matrix([[1.0], [0.0], [0.0], [0.0]])
-  run_test(claw, initial_X, R)
-
-  # Test just changing separation.
-  initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
-  R = numpy.matrix([[0.0], [1.0], [0.0], [0.0]])
-  run_test(claw, initial_X, R)
-
-  # Test changing both separation and position at once.
-  initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
-  R = numpy.matrix([[1.0], [1.0], [0.0], [0.0]])
-  run_test(claw, initial_X, R)
-
-  # Test a small separation error and a large position one.
-  initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
-  R = numpy.matrix([[2.0], [0.05], [0.0], [0.0]])
-  run_test(claw, initial_X, R)
-
-  # Test a small separation error and a large position one.
-  initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
-  R = numpy.matrix([[-0.5], [1.0], [0.0], [0.0]])
-  run_test(claw, initial_X, R)
-
-  # Test opening with the top claw at the limit.
-  initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
-  R = numpy.matrix([[-1.5], [1.5], [0.0], [0.0]])
-  claw.hard_pos_limits = (-1.6, 0.1)
-  claw.pos_limits = (-1.5, 0.0)
-  run_test(claw, initial_X, R)
-  claw.pos_limits = None
-
-  # Test opening with the bottom claw at the limit.
-  initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
-  R = numpy.matrix([[0], [1.5], [0.0], [0.0]])
-  claw.hard_pos_limits = (-0.1, 1.6)
-  claw.pos_limits = (0.0, 1.6)
-  run_test(claw, initial_X, R)
-  claw.pos_limits = None
+  initial_X = numpy.matrix([[0.0], [0.0]])
+  R = numpy.matrix([[1.0], [0.0]])
+  run_test(claw, initial_X, R, controller_claw=claw_controller,
+           observer_claw=observer_claw)
 
   # Write the generated constants out to a file.
   if len(argv) != 3:
@@ -480,8 +186,6 @@
   else:
     claw = Claw("Claw")
     loop_writer = control_loop.ControlLoopWriter("Claw", [claw])
-    loop_writer.AddConstant(control_loop.Constant("kClawMomentOfInertiaRatio",
-      "%f", claw.J_top / claw.J_bottom))
     if argv[1][-3:] == '.cc':
       loop_writer.Write(argv[2], argv[1])
     else: