frc971/control_loops/python/claw.py

#!/usr/bin/python

import control_loop
import controls
import polytope
import polydrivetrain
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
    self.J_top = 2.8
    self.J_bottom = 3.0

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

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

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

    # 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.U_poly = polytope.HPolytope(
        numpy.matrix([[1, 0],
                      [-1, 0],
                      [0, 1],
                      [0, -1]]),
        numpy.matrix([[12],
                      [12],
                      [12],
                      [12]]))

    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]

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

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

  bottom_u = U[0, 0]
  top_u = U[1, 0]

  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:]
    position_error = error[0:2, 0]
    velocity_error = error[2:, 0]

    # 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.

    # First prototype! -> Only cap the position power, leave the velocity power active.

    #u_velocity = velocity_K * velocity_error
    #u_position = position_K * position_error
    #scalar = min(1.0, claw.U_max[0, 0] / max(numpy.abs(u_position[0, 0]), numpy.abs(u_position[1, 0])))
    #return u_velocity + scalar * u_position

    pos_poly = polytope.HPolytope(
        claw.U_poly.H * position_K,
        claw.U_poly.k - claw.U_poly.H * velocity_K * velocity_error)

    P = position_error
    #K = numpy.matrix([[position_error[1, 0], -position_error[0, 0]]])
    L45 = numpy.matrix([[numpy.sign(P[1, 0]) * numpy.sqrt(3), -numpy.sign(P[0, 0])]])
    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))
    #print "Standard", standard
    W = numpy.concatenate((w45, wh))
    #print "W is", W
    intersection = numpy.linalg.inv(standard) * W
    print "intersection point %s" % intersection
    print "Intersection power is ", velocity_K * velocity_error + position_K * intersection
    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
      print "horizontal"
    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 "45"
      print velocity_K * velocity_error + position_K * adjusted_pos_error
      print "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 AverageUFix(claw, U, preserve_v3=False):
  """Clips U as necessary.

    Args:
      claw: claw object containing moments of inertia and U limits.
      U: Input matrix to clip as necessary.
      preserve_v3: There are two ways to attempt to clip the voltages:
        -If you preserve the imaginary v3, then it will attempt to
          preserve the effect on the separation of the two claws.
          If it is not able to do this, then it calls itself with preserve_v3
          set to False.
        -If you preserve the ratio of the voltage of the bottom and the top,
          then it will attempt to preserve the ratio of those two. This is
          equivalent to preserving the ratio of v3 and the bottom voltage.
  """
  bottom_u = U[0, 0]
  top_u = U[1, 0]
  seperation_u = top_u - bottom_u * claw.J_top / claw.J_bottom

  bottom_bad = bottom_u > claw.U_max[0, 0] or bottom_u < claw.U_min[0, 0]
  top_bad = top_u > claw.U_max[0, 0] or top_u < claw.U_min[0, 0]

  scalar = claw.U_max[0, 0] / max(numpy.abs(top_u), numpy.abs(bottom_u))
  if bottom_bad and preserve_v3:
    bottom_u *= scalar
    top_u = seperation_u + bottom_u * claw.J_top / claw.J_bottom
    if abs(top_u) > claw.U_max[0, 0]:
      return AverageUFix(claw, U, preserve_v3=False)
  elif top_bad and preserve_v3:
    top_u *= scalar
    bottom_u = (top_u - seperation_u) * claw.J_bottom / claw.J_top
    if abs(bottom_u) > claw.U_max[0, 0]:
      return AverageUFix(claw, U, preserve_v3=False)
  elif (bottom_bad or top_bad) and not preserve_v3:
    top_u *= scalar
    bottom_u *= scalar
    print "Scaling"

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

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]:
    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 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 
    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
    False if tests fail.
    Args:
      claw: claw object to use.
      initial_X: starting state.
      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."""

  claw.X = initial_X

  # Various lists for graphing things.
  t = []
  x_bottom = []
  x_top = []
  u_bottom = []
  u_top = []
  x_separation = []

  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

  for i in xrange(iterations):
    U = claw.K * (goal - claw.X)
    U = ScaleU(claw, U, claw.K, goal - claw.X)
    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

    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)

  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.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()

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

  initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
  R = numpy.matrix([[-0.05], [2.0], [0.0], [0.0]])
  run_test(claw, initial_X, R, show_graph=True)

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