frc971/control_loops/python/control_loop.py

import controls
import numpy
import os

class Constant(object):
  def __init__ (self, name, formatt, value):
    self.name = name
    self.formatt = formatt
    self.value = value
    self.formatToType = {}
    self.formatToType['%f'] = "double"
    self.formatToType['%d'] = "int"
  def __str__ (self):
    return str("\nstatic constexpr %s %s = "+ self.formatt +";\n") % \
        (self.formatToType[self.formatt], self.name, self.value)


class ControlLoopWriter(object):
  def __init__(self, gain_schedule_name, loops, namespaces=None, write_constants=False):
    """Constructs a control loop writer.

    Args:
      gain_schedule_name: string, Name of the overall controller.
      loops: array[ControlLoop], a list of control loops to gain schedule
        in order.
      namespaces: array[string], a list of names of namespaces to nest in
        order.  If None, the default will be used.
    """
    self._gain_schedule_name = gain_schedule_name
    self._loops = loops
    if namespaces:
      self._namespaces = namespaces
    else:
      self._namespaces = ['frc971', 'control_loops']

    self._namespace_start = '\n'.join(
        ['namespace %s {' % name for name in self._namespaces])

    self._namespace_end = '\n'.join(
        ['}  // namespace %s' % name for name in reversed(self._namespaces)])

    self._constant_list = []

  def AddConstant(self, constant):
    """Adds a constant to write.

    Args:
      constant: Constant, the constant to add to the header.
    """
    self._constant_list.append(constant)

  def _TopDirectory(self):
    return self._namespaces[0]

  def _HeaderGuard(self, header_file):
    return ('_'.join([namespace.upper() for namespace in self._namespaces]) + '_' +
            os.path.basename(header_file).upper()
                .replace('.', '_').replace('/', '_') + '_')

  def Write(self, header_file, cc_file):
    """Writes the loops to the specified files."""
    self.WriteHeader(header_file)
    self.WriteCC(os.path.basename(header_file), cc_file)

  def _GenericType(self, typename):
    """Returns a loop template using typename for the type."""
    num_states = self._loops[0].A.shape[0]
    num_inputs = self._loops[0].B.shape[1]
    num_outputs = self._loops[0].C.shape[0]
    return '%s<%d, %d, %d>' % (
        typename, num_states, num_inputs, num_outputs)

  def _ControllerType(self):
    """Returns a template name for StateFeedbackController."""
    return self._GenericType('StateFeedbackController')

  def _ObserverType(self):
    """Returns a template name for StateFeedbackObserver."""
    return self._GenericType('StateFeedbackObserver')

  def _LoopType(self):
    """Returns a template name for StateFeedbackLoop."""
    return self._GenericType('StateFeedbackLoop')

  def _PlantType(self):
    """Returns a template name for StateFeedbackPlant."""
    return self._GenericType('StateFeedbackPlant')

  def _PlantCoeffType(self):
    """Returns a template name for StateFeedbackPlantCoefficients."""
    return self._GenericType('StateFeedbackPlantCoefficients')

  def _ControllerCoeffType(self):
    """Returns a template name for StateFeedbackControllerCoefficients."""
    return self._GenericType('StateFeedbackControllerCoefficients')

  def _ObserverCoeffType(self):
    """Returns a template name for StateFeedbackObserverCoefficients."""
    return self._GenericType('StateFeedbackObserverCoefficients')

  def WriteHeader(self, header_file, double_appendage=False, MoI_ratio=0.0):
    """Writes the header file to the file named header_file.
       Set double_appendage to true in order to include a ratio of
       moments of inertia constant. Currently, only used for 2014 claw."""
    with open(header_file, 'w') as fd:
      header_guard = self._HeaderGuard(header_file)
      fd.write('#ifndef %s\n'
               '#define %s\n\n' % (header_guard, header_guard))
      fd.write('#include \"frc971/control_loops/state_feedback_loop.h\"\n')
      fd.write('\n')

      fd.write(self._namespace_start)

      for const in self._constant_list:
          fd.write(str(const))

      fd.write('\n\n')
      for loop in self._loops:
        fd.write(loop.DumpPlantHeader())
        fd.write('\n')
        fd.write(loop.DumpControllerHeader())
        fd.write('\n')
        fd.write(loop.DumpObserverHeader())
        fd.write('\n')

      fd.write('%s Make%sPlant();\n\n' %
               (self._PlantType(), self._gain_schedule_name))

      fd.write('%s Make%sController();\n\n' %
               (self._ControllerType(), self._gain_schedule_name))

      fd.write('%s Make%sObserver();\n\n' %
               (self._ObserverType(), self._gain_schedule_name))

      fd.write('%s Make%sLoop();\n\n' %
               (self._LoopType(), self._gain_schedule_name))

      fd.write(self._namespace_end)
      fd.write('\n\n')
      fd.write("#endif  // %s\n" % header_guard)

  def WriteCC(self, header_file_name, cc_file):
    """Writes the cc file to the file named cc_file."""
    with open(cc_file, 'w') as fd:
      fd.write('#include \"%s/%s\"\n' %
               (os.path.join(*self._namespaces), header_file_name))
      fd.write('\n')
      fd.write('#include <vector>\n')
      fd.write('\n')
      fd.write('#include \"frc971/control_loops/state_feedback_loop.h\"\n')
      fd.write('\n')
      fd.write(self._namespace_start)
      fd.write('\n\n')
      for loop in self._loops:
        fd.write(loop.DumpPlant())
        fd.write('\n')

      for loop in self._loops:
        fd.write(loop.DumpController())
        fd.write('\n')

      for loop in self._loops:
        fd.write(loop.DumpObserver())
        fd.write('\n')

      fd.write('%s Make%sPlant() {\n' %
               (self._PlantType(), self._gain_schedule_name))
      fd.write('  ::std::vector< ::std::unique_ptr<%s>> plants(%d);\n' % (
          self._PlantCoeffType(), len(self._loops)))
      for index, loop in enumerate(self._loops):
        fd.write('  plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
                 (index, self._PlantCoeffType(), self._PlantCoeffType(),
                  loop.PlantFunction()))
      fd.write('  return %s(&plants);\n' % self._PlantType())
      fd.write('}\n\n')

      fd.write('%s Make%sController() {\n' %
               (self._ControllerType(), self._gain_schedule_name))
      fd.write('  ::std::vector< ::std::unique_ptr<%s>> controllers(%d);\n' % (
          self._ControllerCoeffType(), len(self._loops)))
      for index, loop in enumerate(self._loops):
        fd.write('  controllers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
                 (index, self._ControllerCoeffType(), self._ControllerCoeffType(),
                  loop.ControllerFunction()))
      fd.write('  return %s(&controllers);\n' % self._ControllerType())
      fd.write('}\n\n')

      fd.write('%s Make%sObserver() {\n' %
               (self._ObserverType(), self._gain_schedule_name))
      fd.write('  ::std::vector< ::std::unique_ptr<%s>> observers(%d);\n' % (
          self._ObserverCoeffType(), len(self._loops)))
      for index, loop in enumerate(self._loops):
        fd.write('  observers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
                 (index, self._ObserverCoeffType(), self._ObserverCoeffType(),
                  loop.ObserverFunction()))
      fd.write('  return %s(&observers);\n' % self._ObserverType())
      fd.write('}\n\n')

      fd.write('%s Make%sLoop() {\n' %
               (self._LoopType(), self._gain_schedule_name))
      fd.write('  return %s(Make%sPlant(), Make%sController(), Make%sObserver());\n' %
          (self._LoopType(), self._gain_schedule_name,
           self._gain_schedule_name, self._gain_schedule_name))
      fd.write('}\n\n')

      fd.write(self._namespace_end)
      fd.write('\n')


class ControlLoop(object):
  def __init__(self, name):
    """Constructs a control loop object.

    Args:
      name: string, The name of the loop to use when writing the C++ files.
    """
    self._name = name

  def ContinuousToDiscrete(self, A_continuous, B_continuous, dt):
    """Calculates the discrete time values for A and B.

      Args:
        A_continuous: numpy.matrix, The continuous time A matrix
        B_continuous: numpy.matrix, The continuous time B matrix
        dt: float, The time step of the control loop

      Returns:
        (A, B), numpy.matrix, the control matricies.
    """
    return controls.c2d(A_continuous, B_continuous, dt)

  def InitializeState(self):
    """Sets X, Y, and X_hat to zero defaults."""
    self.X = numpy.zeros((self.A.shape[0], 1))
    self.Y = self.C * self.X
    self.X_hat = numpy.zeros((self.A.shape[0], 1))

  def PlaceControllerPoles(self, poles):
    """Places the controller poles.

    Args:
      poles: array, An array of poles.  Must be complex conjegates if they have
        any imaginary portions.
    """
    self.K = controls.dplace(self.A, self.B, poles)

  def PlaceObserverPoles(self, poles):
    """Places the observer poles.

    Args:
      poles: array, An array of poles.  Must be complex conjegates if they have
        any imaginary portions.
    """
    self.L = controls.dplace(self.A.T, self.C.T, poles).T

  def Update(self, U):
    """Simulates one time step with the provided U."""
    #U = numpy.clip(U, self.U_min, self.U_max)
    self.X = self.A * self.X + self.B * U
    self.Y = self.C * self.X + self.D * U

  def PredictObserver(self, U):
    """Runs the predict step of the observer update."""
    self.X_hat = (self.A * self.X_hat + self.B * U)

  def CorrectObserver(self, U):
    """Runs the correct step of the observer update."""
    self.X_hat += numpy.linalg.inv(self.A) * self.L * (
        self.Y - self.C * self.X_hat - self.D * U)

  def UpdateObserver(self, U):
    """Updates the observer given the provided U."""
    self.X_hat = (self.A * self.X_hat + self.B * U +
                  self.L * (self.Y - self.C * self.X_hat - self.D * U))

  def _DumpMatrix(self, matrix_name, matrix):
    """Dumps the provided matrix into a variable called matrix_name.

    Args:
      matrix_name: string, The variable name to save the matrix to.
      matrix: The matrix to dump.

    Returns:
      string, The C++ commands required to populate a variable named matrix_name
        with the contents of matrix.
    """
    ans = ['  Eigen::Matrix<double, %d, %d> %s;\n' % (
        matrix.shape[0], matrix.shape[1], matrix_name)]
    for x in xrange(matrix.shape[0]):
      for y in xrange(matrix.shape[1]):
        ans.append('  %s(%d, %d) = %s;\n' % (matrix_name, x, y, repr(matrix[x, y])))

    return ''.join(ans)

  def DumpPlantHeader(self):
    """Writes out a c++ header declaration which will create a Plant object.

    Returns:
      string, The header declaration for the function.
    """
    num_states = self.A.shape[0]
    num_inputs = self.B.shape[1]
    num_outputs = self.C.shape[0]
    return 'StateFeedbackPlantCoefficients<%d, %d, %d> Make%sPlantCoefficients();\n' % (
        num_states, num_inputs, num_outputs, self._name)

  def DumpPlant(self):
    """Writes out a c++ function which will create a PlantCoefficients object.

    Returns:
      string, The function which will create the object.
    """
    num_states = self.A.shape[0]
    num_inputs = self.B.shape[1]
    num_outputs = self.C.shape[0]
    ans = ['StateFeedbackPlantCoefficients<%d, %d, %d>'
           ' Make%sPlantCoefficients() {\n' % (
        num_states, num_inputs, num_outputs, self._name)]

    ans.append(self._DumpMatrix('A', self.A))
    ans.append(self._DumpMatrix('A_inv', numpy.linalg.inv(self.A)))
    ans.append(self._DumpMatrix('A_continuous', self.A_continuous))
    ans.append(self._DumpMatrix('B', self.B))
    ans.append(self._DumpMatrix('B_continuous', self.B_continuous))
    ans.append(self._DumpMatrix('C', self.C))
    ans.append(self._DumpMatrix('D', self.D))
    ans.append(self._DumpMatrix('U_max', self.U_max))
    ans.append(self._DumpMatrix('U_min', self.U_min))

    ans.append('  return StateFeedbackPlantCoefficients<%d, %d, %d>'
               '(A, A_inv, A_continuous, B, B_continuous, C, D, U_max, U_min);\n' % (
                   num_states, num_inputs, num_outputs))
    ans.append('}\n')
    return ''.join(ans)

  def PlantFunction(self):
    """Returns the name of the plant coefficient function."""
    return 'Make%sPlantCoefficients()' % self._name

  def ControllerFunction(self):
    """Returns the name of the controller function."""
    return 'Make%sControllerCoefficients()' % self._name

  def ObserverFunction(self):
    """Returns the name of the controller function."""
    return 'Make%sObserverCoefficients()' % self._name

  def DumpControllerHeader(self):
    """Writes out a c++ header declaration which will create a Controller object.

    Returns:
      string, The header declaration for the function.
    """
    num_states = self.A.shape[0]
    num_inputs = self.B.shape[1]
    num_outputs = self.C.shape[0]
    return 'StateFeedbackControllerCoefficients<%d, %d, %d> %s;\n' % (
        num_states, num_inputs, num_outputs, self.ControllerFunction())

  def DumpController(self):
    """Returns a c++ function which will create a Controller object.

    Returns:
      string, The function which will create the object.
    """
    num_states = self.A.shape[0]
    num_inputs = self.B.shape[1]
    num_outputs = self.C.shape[0]
    ans = ['StateFeedbackControllerCoefficients<%d, %d, %d> %s {\n' % (
        num_states, num_inputs, num_outputs, self.ControllerFunction())]

    ans.append(self._DumpMatrix('K', self.K))
    if not hasattr(self, 'Kff'):
      self.Kff = numpy.matrix(numpy.zeros(self.K.shape))

    ans.append(self._DumpMatrix('Kff', self.Kff))

    ans.append('  return StateFeedbackControllerCoefficients<%d, %d, %d>'
               '(K, Kff);\n' % (
                   num_states, num_inputs, num_outputs))
    ans.append('}\n')
    return ''.join(ans)

  def DumpObserverHeader(self):
    """Writes out a c++ header declaration which will create a Observer object.

    Returns:
      string, The header declaration for the function.
    """
    num_states = self.A.shape[0]
    num_inputs = self.B.shape[1]
    num_outputs = self.C.shape[0]
    return 'StateFeedbackObserverCoefficients<%d, %d, %d> %s;\n' % (
        num_states, num_inputs, num_outputs, self.ObserverFunction())

  def DumpObserver(self):
    """Returns a c++ function which will create a Observer object.

    Returns:
      string, The function which will create the object.
    """
    num_states = self.A.shape[0]
    num_inputs = self.B.shape[1]
    num_outputs = self.C.shape[0]
    ans = ['StateFeedbackObserverCoefficients<%d, %d, %d> %s {\n' % (
        num_states, num_inputs, num_outputs, self.ObserverFunction())]

    ans.append(self._DumpMatrix('L', self.L))

    ans.append('  return StateFeedbackObserverCoefficients<%d, %d, %d>'
               '(L);\n' % (num_states, num_inputs, num_outputs))
    ans.append('}\n')
    return ''.join(ans)