Added the polyhedral set drivetrain code, and fixed a bug with Ben.
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
new file mode 100755
index 0000000..428c29d
--- /dev/null
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -0,0 +1,263 @@
+#!/usr/bin/python
+
+import numpy
+import sys
+import polytope
+import drivetrain
+import controls
+from matplotlib import pylab
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+
+def CoerceGoal(region, K, w, R):
+ """Intersects a line with a region, and finds the closest point to R.
+
+ Finds a point that is closest to R inside the region, and on the line
+ defined by K X = w. If it is not possible to find a point on the line,
+ finds a point that is inside the region and closest to the line. This
+ function assumes that
+
+ Args:
+ region: HPolytope, the valid goal region.
+ K: numpy.matrix (2 x 1), the matrix for the equation [K1, K2] [x1; x2] = w
+ w: float, the offset in the equation above.
+ R: numpy.matrix (2 x 1), the point to be closest to.
+
+ Returns:
+ numpy.matrix (2 x 1), the point.
+ """
+
+ if region.IsInside(R):
+ return R
+
+ perpendicular_vector = K.T / numpy.linalg.norm(K)
+ parallel_vector = numpy.matrix([[perpendicular_vector[1, 0]],
+ [-perpendicular_vector[0, 0]]])
+
+ # We want to impose the constraint K * X = w on the polytope H * X <= k.
+ # We do this by breaking X up into parallel and perpendicular components to
+ # the half plane. This gives us the following equation.
+ #
+ # parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) = X
+ #
+ # Then, substitute this into the polytope.
+ #
+ # H * (parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) <= k
+ #
+ # Substitute K * X = w
+ #
+ # H * parallel * (parallel.T \dot X) + H * perpendicular * w <= k
+ #
+ # Move all the knowns to the right side.
+ #
+ # H * parallel * ([parallel1 parallel2] * X) <= k - H * perpendicular * w
+ #
+ # Let t = parallel.T \dot X, the component parallel to the surface.
+ #
+ # H * parallel * t <= k - H * perpendicular * w
+ #
+ # This is a polytope which we can solve, and use to figure out the range of X
+ # that we care about!
+
+ t_poly = polytope.HPolytope(
+ region.H * parallel_vector,
+ region.k - region.H * perpendicular_vector * w)
+
+ vertices = t_poly.Vertices()
+
+ if vertices.shape[0]:
+ # The region exists!
+ # Find the closest vertex
+ min_distance = numpy.infty
+ closest_point = None
+ for vertex in vertices:
+ point = parallel_vector * vertex + perpendicular_vector * w
+ length = numpy.linalg.norm(R - point)
+ if length < min_distance:
+ min_distance = length
+ closest_point = point
+
+ return closest_point
+ else:
+ # Find the vertex of the space that is closest to the line.
+ region_vertices = region.Vertices()
+ min_distance = numpy.infty
+ closest_point = None
+ for vertex in region_vertices:
+ point = vertex.T
+ length = numpy.abs((perpendicular_vector.T * point)[0, 0])
+ if length < min_distance:
+ min_distance = length
+ closest_point = point
+
+ return closest_point
+
+
+class VelocityDrivetrain(object):
+ def __init__(self):
+ self.drivetrain = drivetrain.Drivetrain()
+
+ # X is [lvel, rvel]
+ self.X = numpy.matrix(
+ [[0.0],
+ [0.0]])
+
+ self.A = numpy.matrix(
+ [[self.drivetrain.A[1, 1], self.drivetrain.A[1, 3]],
+ [self.drivetrain.A[3, 1], self.drivetrain.A[3, 3]]])
+
+ self.B = numpy.matrix(
+ [[self.drivetrain.B[1, 0], self.drivetrain.B[1, 1]],
+ [self.drivetrain.B[3, 0], self.drivetrain.B[3, 1]]])
+
+ # FF * X = U (steady state)
+ self.FF = self.B.I * (numpy.eye(2) - self.A)
+
+ self.U_poly = polytope.HPolytope(
+ numpy.matrix([[1, 0],
+ [-1, 0],
+ [0, 1],
+ [0, -1]]),
+ numpy.matrix([[12],
+ [12],
+ [12],
+ [12]]))
+
+ self.U_max = numpy.matrix(
+ [[12.0],
+ [12.0]])
+ self.U_min = numpy.matrix(
+ [[-12.0000000000],
+ [-12.0000000000]])
+
+ self.K = controls.dplace(self.A, self.B, [0.3, 0.3])
+
+ self.dt = 0.01
+
+ self.R = numpy.matrix(
+ [[0.0],
+ [0.0]])
+
+ self.vmax = (
+ self.U_max[0, 0] * self.B[0, :].sum() / (1 - self.A[0, :].sum()))
+
+ self.xfiltered = 0.0
+
+ # U = self.K[0, :].sum() * (R - xfiltered) + self.FF[0, :].sum() * R
+ # throttle * 12.0 = (self.K[0, :].sum() + self.FF[0, :].sum()) * R
+ # - self.K[0, :].sum() * xfiltered
+
+ # R = (throttle * 12.0 + self.K[0, :].sum() * xfiltered) /
+ # (self.K[0, :].sum() + self.FF[0, :].sum())
+
+ # U = (K + FF) * R - K * X
+ # (K + FF) ^-1 * (U + K * X) = R
+
+ # (K + FF) ^-1 * (throttle * 12.0 + K * throttle * self.vmax) = R
+ # Xn+1 = A * X + B * (throttle * 12.0)
+
+ # xfiltered = self.A[0, :].sum() + B[0, :].sum() * throttle * 12.0
+ self.ttrust = 1.1
+
+ def Update(self, throttle, steering):
+ # Invert the plant to figure out how the velocity filter would have to work
+ # out in order to filter out the forwards negative inertia.
+ # This math assumes that the left and right power and velocity are equals,
+ # and that the plant is the same on the left and right.
+ # TODO(aschuh): Prove that this is right again and figure out what ttrust
+ # means...
+ fvel = ((throttle * 12.0 + self.ttrust * self.K[0, :].sum() * self.xfiltered)
+ / (self.ttrust * self.K[0, :].sum() + self.FF[0, :].sum()))
+ self.xfiltered = (self.A[0, :].sum() * self.xfiltered +
+ self.B[0, :].sum() * throttle * 12.0)
+
+ fvel = 12 / self.FF[0, :].sum() * throttle
+
+ # Constant radius means that angualar_velocity / linear_velocity = constant.
+ # Compute the left and right velocities.
+ left_velocity = fvel - steering * numpy.abs(fvel)
+ right_velocity = fvel + steering * numpy.abs(fvel)
+
+ # Write this constraint in the form of K * R = w
+ # angular velocity / linear velocity = constant
+ # (left - right) / (left + right) = constant
+ # left - right = constant * left + constant * right
+
+ # (fvel - steering * numpy.abs(fvel) - fvel - steering * numpy.abs(fvel)) /
+ # (fvel - steering * numpy.abs(fvel) + fvel + steering * numpy.abs(fvel)) =
+ # constant
+ # (- 2 * steering * numpy.abs(fvel)) / (2 * fvel) = constant
+ # (-steering * sign(fvel)) = constant
+ # (-steering * sign(fvel)) * (left + right) = left - right
+ # (steering * sign(fvel) + 1) * left + (steering * sign(fvel) - 1) * right = 0
+
+ equality_k = numpy.matrix(
+ [[1 + steering * numpy.sign(fvel), -(1 - steering * numpy.sign(fvel))]])
+ equality_w = 0.0
+
+ self.R[0, 0] = left_velocity
+ self.R[1, 0] = right_velocity
+
+ # Construct a constraint on R by manipulating the constraint on U
+ # Start out with H * U <= k
+ # U = FF * R + K * (R - X)
+ # H * (FF * R + K * R - K * X) <= k
+ # H * (FF + K) * R <= k + H * K * X
+ R_poly = polytope.HPolytope(
+ self.U_poly.H * (self.K + self.FF),
+ self.U_poly.k + self.U_poly.H * self.K * self.X)
+
+ # Limit R back inside the box.
+ self.boxed_R = CoerceGoal(R_poly, equality_k, equality_w, self.R)
+
+ FF_volts = self.FF * self.boxed_R
+ self.U_ideal = self.K * (self.boxed_R - self.X) + FF_volts
+
+ # Verify that the steering angle has not flipped.
+ assert((self.boxed_R[1, 0] - self.boxed_R[0, 0]) * steering >= 0)
+
+ self.U = numpy.clip(self.U_ideal, self.U_min, self.U_max)
+ self.X = self.A * self.X + self.B * self.U
+
+
+def main(argv):
+ drivetrain = VelocityDrivetrain()
+
+ vl_plot = []
+ vr_plot = []
+ ul_plot = []
+ ur_plot = []
+ radius_plot = []
+ t_plot = []
+ for t in numpy.arange(0, 1.5, drivetrain.dt):
+ if t < 0.5:
+ drivetrain.Update(throttle=0.60, steering=0.3)
+ elif t < 1.0:
+ drivetrain.Update(throttle=0.60, steering=-0.3)
+ else:
+ drivetrain.Update(throttle=0.00, steering=0.3)
+ t_plot.append(t)
+ vl_plot.append(drivetrain.X[0, 0])
+ vr_plot.append(drivetrain.X[1, 0])
+ ul_plot.append(drivetrain.U[0, 0])
+ ur_plot.append(drivetrain.U[1, 0])
+
+ fwd_velocity = (drivetrain.X[1, 0] + drivetrain.X[0, 0]) / 2
+ turn_velocity = (drivetrain.X[1, 0] - drivetrain.X[0, 0])
+ if fwd_velocity < 0.0000001:
+ radius_plot.append(turn_velocity)
+ else:
+ radius_plot.append(turn_velocity / fwd_velocity)
+
+ pylab.plot(t_plot, vl_plot, label='left velocity')
+ pylab.plot(t_plot, vr_plot, label='right velocity')
+ pylab.plot(t_plot, ul_plot, label='left power')
+ pylab.plot(t_plot, ur_plot, label='right power')
+ pylab.plot(t_plot, radius_plot, label='radius')
+ pylab.legend()
+ pylab.show()
+ return 0
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))