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 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 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 __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, [0.75 + 0.1j, 0.75 - 0.1j])
self.K_diff = controls.dplace(self.A_diff, self.B_diff, [0.75 + 0.1j, 0.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 = 0.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()
