frc971/control_loops/python/claw.py

#!/usr/bin/python

import control_loop
import controls
import numpy
import sys
from matplotlib import pylab

class Claw(control_loop.ControlLoop):
  def __init__(self, name="RawClaw"):
    super(Claw, self).__init__(name)
    # Stall Torque in N m
    self.stall_torque = 2.42
    # Stall Current in Amps
    self.stall_current = 133
    # Free Speed in RPM
    self.free_speed = 5500.0
    # Free Current in Amps
    self.free_current = 2.7
    # Moment of inertia of the claw in kg m^2
    # measured from CAD
    self.J_top = 0.3
    self.J_bottom = 0.9
    # 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))
    # 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

    # 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.A_continuous = numpy.matrix(
        [[0, 0, 0, 0],
         [0, 0, 0, 0],
         [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]])

    self.A_diff_cont = numpy.matrix(
        [[0, 1],
         [0, self.difference_difference]])

    self.A_continuous[0:2, 0:2] = self.A_bottom_cont
    self.A_continuous[2:4, 2:4] = self.A_diff_cont
    self.A_continuous[3, 1] = self.difference_bottom

    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))
    self.B_continuous = numpy.matrix(
        [[0, 0],
         [self.motor_feedforward_bottom, 0],
         [0, 0],
         [0,#self.motor_feedforward_difference_bottom,
          self.motor_feedforward_difference]])

    self.B_bottom_cont = numpy.matrix(
        [[0],
         [self.motor_feedforward_bottom]])

    self.B_diff_cont = numpy.matrix(
        [[0],
         [self.motor_feedforward_difference]])

    self.C = numpy.matrix([[1, 0, 0, 0],
                           [1, 0, 1, 0]])
    self.D = numpy.matrix([[0, 0],
                           [0, 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)

    print "A, A_bot, A_diff:"
    print self.A, self.A_bottom, self.A_diff
    print "B, B_bot, B_diff:"
    print self.B, self.B_bottom, self.B_diff
    
    # If B should equal [[B_bot, 0],[0, B_diff]], then the B
    # generated by ContinuousToDiscrete adds in a couple extra
    # numbers which make it impossible to control 4 values in K.
    # Here, I make B equal what I had thought it should, thereby
    # allowing us to control 4 values in K.
    self.B_actual = numpy.matrix(self.B)
    self.B[2, 0] = 0.0
    self.B[3, 0] = 0.0
    # If we do the above, with setting values of B to zero,
    # then we can no longer make all the necessary values of A - B * K
    # zero, because the discrete transform added a term affecting position
    # over the timestep and that term can no longer be cancelled out if we
    # are to also cancel out the term where the velocity of the bottom
    # affects the velocity of the separation.
    self.A_actual = numpy.matrix(self.A)
    self.A[2, 1] = self.A[3, 1] * self.B[2, 1] / self.B[3, 1]

    #controlability = controls.ctrb(self.A, self.B);
    #print "Rank of controlability matrix.", numpy.linalg.matrix_rank(controlability)

    self.Q = numpy.matrix([[(1.0 / (0.40 ** 2.0)), 0.0, 0.0, 0.0],
                           [0.0, (1.0 / (0.007 ** 2.0)), 0.0, 0.0],
                           [0.0, 0.0, 0.2, 0.0],
                           [0.0, 0.0, 0.0, 2.00]])

    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)

    # TODO(james): Fix this for discrete time domain.
    self.K = numpy.matrix([[0, 0, 0.0, 0.0],
                           [0.0, self.A[3, 1] / self.B[3, 1], 0, 0]])
    self.K_bottom = controls.dplace(self.A_bottom, self.B_bottom, [0.6, 0.7])
    self.K_diff = controls.dplace(self.A_diff, self.B_diff, [0.3, 0.5])
    self.K[0, 0:2] = self.K_bottom
    self.K[1, 2:4] = self.K_diff
   #lstsq_A = numpy.identity(2)
   #lstsq_A[0] = self.B[1]
   #lstsq_A[1] = self.B[3]
   #self.K[0:2, 0] = numpy.linalg.lstsq(lstsq_A, numpy.matrix([[0.0], [0.0]]))[0]
   #self.K[0:2, 2] = numpy.linalg.lstsq(
   #                     lstsq_A,
   #                     numpy.matrix([[self.A[1, 2]], [self.A[3, 2]]]))[0]

    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.008
    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], [24.0]])
    self.U_min = numpy.matrix([[-12.0], [-24.0]])

    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"
    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

    #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]])

    self.InitializeState()


def FullSeparationPriority(claw, U):
  bottom_u = U[0, 0]
  top_u = U[1, 0] + bottom_u

  #print "Bottom is", new_unclipped_bottom_u, "Top is", top_u
  if bottom_u > claw.U_max[0, 0]:
    #print "Bottom is too big.  Was", new_unclipped_bottom_u, "changing top by", new_unclipped_bottom_u - claw.U_max[0, 0]
    top_u -= bottom_u - claw.U_max[0, 0]
    if top_u < claw.U_min[1, 0]:
      top_u = claw.U_min[1, 0]

    bottom_u = claw.U_max[0, 0]
  if top_u > claw.U_max[1, 0]:
    bottom_u -= top_u - claw.U_max[1, 0]
    if bottom_u < claw.U_min[0, 0]:
      bottom_u = claw.U_min[0, 0]

    top_u = claw.U_max[1, 0]
  if top_u < claw.U_min[1, 0]:
    bottom_u -= top_u - claw.U_min[1, 0]
    if bottom_u > claw.U_max[0, 0]:
      bottom_u = claw.U_max[0, 0]

    top_u = claw.U_min[1, 0]
  if bottom_u < claw.U_min[0, 0]:
    top_u -= bottom_u - claw.U_min[0, 0]
    if top_u > claw.U_max[1, 0]:
      top_u = claw.U_max[1, 0]

    bottom_u = claw.U_min[0, 0]

  return numpy.matrix([[bottom_u], [top_u - bottom_u]])

def AverageUFix(claw, U):
  bottom_u = U[0, 0]
  top_u = bottom_u + U[1, 0]
  top_u = bottom_u * claw.J_top / claw.J_bottom + U[1, 0] 

  #print "Bottom is", new_unclipped_bottom_u, "Top is", top_u
  if (bottom_u > claw.U_max[0, 0] or top_u > claw.U_max[0, 0] or
      top_u < claw.U_min[0, 0] or bottom_u < claw.U_min[0, 0]):
    scalar = 12.0 / max(numpy.abs(top_u), numpy.abs(bottom_u))
    top_u *= scalar
    bottom_u *= scalar

 #return numpy.matrix([[bottom_u], [top_u - bottom_u]])
  return numpy.matrix([[bottom_u], [top_u - bottom_u * claw.J_top / claw.J_bottom]])

def ClipDeltaU(claw, U):
  delta_u = U[0, 0]
  top_u = U[1, 0]
  old_bottom_u = claw.X[4, 0]

  # TODO(austin): Preserve the difference between the top and bottom power.
  new_unclipped_bottom_u = old_bottom_u + delta_u

  #print "Bottom is", new_unclipped_bottom_u, "Top is", top_u
  if new_unclipped_bottom_u > claw.U_max[0, 0]:
    #print "Bottom is too big.  Was", new_unclipped_bottom_u, "changing top by", new_unclipped_bottom_u - claw.U_max[0, 0]
    top_u -= new_unclipped_bottom_u - claw.U_max[0, 0]
    new_unclipped_bottom_u = claw.U_max[0, 0]
  if top_u > claw.U_max[1, 0]:
    new_unclipped_bottom_u -= top_u - claw.U_max[1, 0]
    top_u = claw.U_max[1, 0]
  if top_u < claw.U_min[1, 0]:
    new_unclipped_bottom_u -= top_u - claw.U_min[1, 0]
    top_u = claw.U_min[1, 0]
  if new_unclipped_bottom_u < claw.U_min[0, 0]:
    top_u -= new_unclipped_bottom_u - claw.U_min[0, 0]
    new_unclipped_bottom_u = claw.U_min[0, 0]

  new_bottom_u = numpy.clip(new_unclipped_bottom_u, claw.U_min[0, 0], claw.U_max[0, 0])
  new_top_u = numpy.clip(top_u, claw.U_min[1, 0], claw.U_max[1, 0])

  return numpy.matrix([[new_bottom_u - old_bottom_u], [new_top_u]])

def main(argv):
  # Simulate the response of the system to a step input.
  #claw = ClawDeltaU()
  #simulated_x = []
  #for _ in xrange(100):
  #  claw.Update(numpy.matrix([[12.0]]))
  #  simulated_x.append(claw.X[0, 0])

  #pylab.plot(range(100), simulated_x)
  #pylab.show()

  # Simulate the closed loop response of the system.
  claw = Claw("TopClaw")
  t = []
  close_loop_x_bottom = []
  close_loop_x_sep = []
  actual_sep = []
  actual_x_bottom = []
  close_loop_x_top = []
  close_loop_u_bottom = []
  close_loop_u_top = []
  R = numpy.matrix([[0.0], [0.00], [0.0], [0.0]])
  claw.X[0, 0] = 1
  claw.X_hat[0, 0] = 1
  X_actual = claw.X
  print "B actual"
  print claw.B_actual
  for i in xrange(100):
    #print "Error is", (R - claw.X_hat)
    U = claw.K * (R - claw.X_hat)
    #U = numpy.clip(claw.K * (R - claw.X_hat), claw.U_min, claw.U_max)
    #U = FullSeparationPriority(claw, U)
   #U = AverageUFix(claw, U)
    #U = claw.K * (R - claw.X_hat)
    #U = ClipDeltaU(claw, U)
    claw.UpdateObserver(U)
   #claw.Update(U)
    X_actual = claw.A_actual * X_actual + claw.B_actual * U
    claw.Y = claw.C * X_actual
    close_loop_x_bottom.append(claw.X_hat[0, 0] * 10)
    close_loop_u_bottom.append(U[0, 0])
    actual_sep.append(X_actual[2, 0] * 100)
    actual_x_bottom.append(X_actual[0, 0] * 10)
    close_loop_x_sep.append(claw.X_hat[2, 0] * 100)
    close_loop_x_top.append((claw.X_hat[2, 0] + claw.X_hat[0, 0]) * 10)
    close_loop_u_top.append(U[1, 0] + U[0, 0] * claw.J_top / claw.J_bottom)
    t.append(0.01 * i)

  pylab.plot(t, close_loop_x_bottom, label='x bottom')
  pylab.plot(t, close_loop_x_sep, label='separation')
  pylab.plot(t, actual_x_bottom, label='true x bottom')
  pylab.plot(t, actual_sep, label='true separation')
  pylab.plot(t, close_loop_x_top, label='x top')
  pylab.plot(t, close_loop_u_bottom, label='u bottom')
  pylab.plot(t, close_loop_u_top, label='u top')
  pylab.legend()
  pylab.show()

  # Write the generated constants out to a file.
  if len(argv) != 3:
    print "Expected .h file name and .cc file name for the claw."
  else:
    claw = Claw("Claw")
    loop_writer = control_loop.ControlLoopWriter("Claw", [claw])
    if argv[1][-3:] == '.cc':
      loop_writer.Write(argv[2], argv[1])
    else:
      loop_writer.Write(argv[1], argv[2])

if __name__ == '__main__':
  sys.exit(main(sys.argv))