copied everything over from 2012 and removed all of the actual robot code except the drivetrain stuff
git-svn-id: https://robotics.mvla.net/svn/frc971/2013/trunk/src@4078 f308d9b7-e957-4cde-b6ac-9a88185e7312
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
new file mode 100644
index 0000000..7d34a85
--- /dev/null
+++ b/frc971/control_loops/python/controls.py
@@ -0,0 +1,83 @@
+#!/usr/bin/python
+
+"""
+Control loop pole placement library.
+
+This library will grow to support many different pole placement methods.
+Currently it only supports direct pole placement.
+"""
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+import numpy
+import slycot
+
+class Error (Exception):
+ """Base class for all control loop exceptions."""
+
+
+class PolePlacementError(Error):
+ """Exception raised when pole placement fails."""
+
+
+# TODO(aschuh): dplace should take a control system object.
+# There should also exist a function to manipulate laplace expressions, and
+# something to plot bode plots and all that.
+def dplace(A, B, poles, alpha=1e-6):
+ """Set the poles of (A - BF) to poles.
+
+ Args:
+ A: numpy.matrix(n x n), The A matrix.
+ B: numpy.matrix(n x m), The B matrix.
+ poles: array(imaginary numbers), The poles to use. Complex conjugates poles
+ must be in pairs.
+
+ Raises:
+ ValueError: Arguments were the wrong shape or there were too many poles.
+ PolePlacementError: Pole placement failed.
+
+ Returns:
+ numpy.matrix(m x n), K
+ """
+ # See http://www.icm.tu-bs.de/NICONET/doc/SB01BD.html for a description of the
+ # fortran code that this is cleaning up the interface to.
+ n = A.shape[0]
+ if A.shape[1] != n:
+ raise ValueError("A must be square")
+ if B.shape[0] != n:
+ raise ValueError("B must have the same number of states as A.")
+ m = B.shape[1]
+
+ num_poles = len(poles)
+ if num_poles > n:
+ raise ValueError("Trying to place more poles than states.")
+
+ out = slycot.sb01bd(n=n,
+ m=m,
+ np=num_poles,
+ alpha=alpha,
+ A=A,
+ B=B,
+ w=numpy.array(poles),
+ dico='D')
+
+ A_z = numpy.matrix(out[0])
+ num_too_small_eigenvalues = out[2]
+ num_assigned_eigenvalues = out[3]
+ num_uncontrollable_eigenvalues = out[4]
+ K = numpy.matrix(-out[5])
+ Z = numpy.matrix(out[6])
+
+ if num_too_small_eigenvalues != 0:
+ raise PolePlacementError("Number of eigenvalues that are too small "
+ "and are therefore unmodified is %d." %
+ num_too_small_eigenvalues)
+ if num_assigned_eigenvalues != num_poles:
+ raise PolePlacementError("Did not place all the eigenvalues that were "
+ "requested. Only placed %d eigenvalues." %
+ num_assigned_eigenvalues)
+ if num_uncontrollable_eigenvalues != 0:
+ raise PolePlacementError("Found %d uncontrollable eigenvlaues." %
+ num_uncontrollable_eigenvalues)
+
+ return K
diff --git a/frc971/control_loops/python/libcdd.py b/frc971/control_loops/python/libcdd.py
new file mode 100644
index 0000000..a217728
--- /dev/null
+++ b/frc971/control_loops/python/libcdd.py
@@ -0,0 +1,129 @@
+#!/usr/bin/python
+
+"""Wrapper around libcdd, a polytope manipulation library."""
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+import ctypes
+
+# Wrapper around PyFile_AsFile so that we can print out the error messages.
+# Set the arg type and return types of the function call.
+class FILE(ctypes.Structure):
+ pass
+
+ctypes.pythonapi.PyFile_AsFile.argtypes = [ctypes.py_object]
+ctypes.pythonapi.PyFile_AsFile.restype = ctypes.POINTER(FILE)
+
+# Load and init libcdd. libcdd is a C library that implements algorithm to
+# manipulate half space and vertex representations of polytopes.
+# Unfortunately, the library was compiled with C++ even though it has a lot of C
+# code in it, so all the symbol names are mangled. Ug.
+libcdd = ctypes.cdll.LoadLibrary('libcdd.so')
+libcdd._Z23dd_set_global_constantsv()
+
+# The variable type mytype that libcdd defines (double[1])
+# See http://docs.python.org/2/library/ctypes.html#arrays for the documentation
+# explaining why ctypes.c_double * 1 => double[1]
+# libcdd defines mytype to various things so it can essentially template its
+# functions. What a weird library.
+mytype = ctypes.c_double * 1
+
+
+# Forward declaration for the polyhedra data structure.
+class dd_polyhedradata(ctypes.Structure):
+ pass
+
+
+# Definition of dd_matrixdata
+class dd_matrixdata(ctypes.Structure):
+ _fields_ = [
+ ("rowsize", ctypes.c_long),
+ ("linset", ctypes.POINTER(ctypes.c_ulong)),
+ ("colsize", ctypes.c_long),
+ ("representation", ctypes.c_int),
+ ("numbtype", ctypes.c_int),
+ ("matrix", ctypes.POINTER(ctypes.POINTER(mytype))),
+ ("objective", ctypes.c_int),
+ ("rowvec", ctypes.POINTER(mytype)),
+ ]
+
+# Define the input and output types for a bunch of libcdd functions.
+libcdd._Z15dd_CreateMatrixll.restype = ctypes.POINTER(dd_matrixdata)
+libcdd._Z9ddd_get_dPd.argtypes = [mytype]
+libcdd._Z9ddd_get_dPd.restype = ctypes.c_double
+
+libcdd._Z17dd_CopyGeneratorsP16dd_polyhedradata.argtypes = [
+ ctypes.POINTER(dd_polyhedradata)
+]
+libcdd._Z17dd_CopyGeneratorsP16dd_polyhedradata.restype = ctypes.POINTER(dd_matrixdata)
+
+libcdd._Z16dd_DDMatrix2PolyP13dd_matrixdataP12dd_ErrorType.argtypes = [
+ ctypes.POINTER(dd_matrixdata),
+ ctypes.POINTER(ctypes.c_int)
+]
+libcdd._Z16dd_DDMatrix2PolyP13dd_matrixdataP12dd_ErrorType.restype = (
+ ctypes.POINTER(dd_polyhedradata))
+
+libcdd._Z13dd_FreeMatrixP13dd_matrixdata.argtypes = [
+ ctypes.POINTER(dd_matrixdata)
+]
+
+libcdd._Z16dd_FreePolyhedraP16dd_polyhedradata.argtypes = [
+ ctypes.POINTER(dd_polyhedradata)
+]
+
+libcdd._Z9ddd_set_dPdd.argtypes = [
+ mytype,
+ ctypes.c_double
+]
+
+
+# Various enums.
+DD_INEQUALITY = 1
+DD_REAL = 1
+DD_NO_ERRORS = 17
+
+
+def dd_CreateMatrix(rows, cols):
+ return libcdd._Z15dd_CreateMatrixll(
+ ctypes.c_long(rows),
+ ctypes.c_long(cols))
+
+
+def dd_set_d(mytype_address, double_value):
+ libcdd._Z9ddd_set_dPdd(mytype_address,
+ ctypes.c_double(double_value))
+
+
+def dd_CopyGenerators(polyhedraptr):
+ return libcdd._Z17dd_CopyGeneratorsP16dd_polyhedradata(polyhedraptr)
+
+
+def dd_get_d(mytype_address):
+ return libcdd._Z9ddd_get_dPd(mytype_address)
+
+
+def dd_FreeMatrix(matrixptr):
+ libcdd._Z13dd_FreeMatrixP13dd_matrixdata(matrixptr)
+
+
+def dd_FreePolyhedra(polyhedraptr):
+ libcdd._Z16dd_FreePolyhedraP16dd_polyhedradata(polyhedraptr)
+
+
+def dd_DDMatrix2Poly(matrixptr):
+ error = ctypes.c_int()
+ polyhedraptr = libcdd._Z16dd_DDMatrix2PolyP13dd_matrixdataP12dd_ErrorType(
+ matrixptr,
+ ctypes.byref(error))
+
+ # Return None on error.
+ # The error values are enums, so they aren't exposed.
+ if error.value != NO_ERRORS:
+ # Dump out the errors to stderr
+ libcdd._Z21dd_WriteErrorMessagesP8_IO_FILE12dd_ErrorType(
+ ctypes.pythonapi.PyFile_AsFile(ctypes.py_object(sys.stdout)),
+ error)
+ dd_FreePolyhedra(polyhedraptr)
+ return None
+ return polyhedraptr
diff --git a/frc971/control_loops/python/polytope.py b/frc971/control_loops/python/polytope.py
new file mode 100644
index 0000000..e08a160
--- /dev/null
+++ b/frc971/control_loops/python/polytope.py
@@ -0,0 +1,224 @@
+#!/usr/bin/python
+
+"""
+Polyhedral set library.
+
+This library implements convex regions of the form H x <= k, where H, x, and k
+are matricies. It also provides convenient methods to find all the verticies.
+"""
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+
+import libcdd
+import numpy
+import string
+import sys
+
+
+def _PiecewiseConcat(*args):
+ """Concatenates strings inside lists, elementwise.
+
+ Given ['a', 's'] and ['d', 'f'], returns ['ad', 'sf']
+ """
+ return map(''.join, zip(*args))
+
+
+def _SplitAndPad(s):
+ """Splits a string on newlines, and pads the lines to be the same width."""
+ split_string = s.split('\n')
+ width = max(len(stringpiece) for stringpiece in split_string) + 1
+
+ padded_strings = [string.ljust(stringpiece, width, ' ')
+ for stringpiece in split_string]
+ return padded_strings
+
+
+def _PadHeight(padded_array, min_height):
+ """Adds lines of spaces to the top and bottom of an array symmetrically."""
+ height = len(padded_array)
+ if height < min_height:
+ pad_array = [' ' * len(padded_array[0])]
+ height_error = min_height - height
+ return (pad_array * ((height_error) / 2) +
+ padded_array +
+ pad_array * ((height_error + 1) / 2))
+ return padded_array
+
+
+class HPolytope(object):
+ """This object represents a H-polytope.
+
+ Polytopes are convex regions in n-dimensional space.
+ For H-polytopes, this is represented as the intersection of a set of half
+ planes. The mathematic equation that represents this is H x <= k.
+ """
+
+ def __init__(self, H, k):
+ """Constructs a H-polytope from the H and k matricies.
+
+ Args:
+ H: numpy.Matrix (n by k), where n is the number of constraints, and k is
+ the number of dimensions in the space. Does not copy the matrix.
+ k: numpy.Matrix (n by 1). Does not copy the matrix.
+ """
+ self._H = H
+ self._k = k
+
+ @property
+ def k(self):
+ """Returns the k in H x <= k."""
+ return self._k
+
+ @property
+ def H(self):
+ """Returns the H in H x <= k."""
+ return self._H
+
+ @property
+ def ndim(self):
+ """Returns the dimension of the set."""
+ return self._H.shape[1]
+
+ @property
+ def num_constraints(self):
+ """Returns the number of constraints defining the set."""
+ return self._k.shape[0]
+
+ def IsInside(self, point):
+ """Returns true if the point is inside the polytope, edges included."""
+ return (self._H * point <= self._k).all()
+
+ def Vertices(self):
+ """Returns a matrix with the vertices of the set in its rows."""
+ # TODO(aschuh): It would be better to write some small C/C++ function that
+ # does all of this and takes in a numpy array.
+ # The copy is expensive in Python and cheaper in C.
+
+ # Create an empty matrix with the correct size.
+ matrixptr = libcdd.dd_CreateMatrix(self.num_constraints, self.ndim + 1)
+ matrix = matrixptr.contents
+
+ try:
+ # Copy the data into the matrix.
+ for i in xrange(self.num_constraints):
+ libcdd.dd_set_d(matrix.matrix[i][0], self._k[i, 0])
+ for j in xrange(self.ndim):
+ libcdd.dd_set_d(matrix.matrix[i][j + 1], -self._H[i, j])
+
+ # Set enums to the correct values.
+ matrix.representation = libcdd.DD_INEQUALITY
+ matrix.numbtype = libcdd.DD_REAL
+
+ # TODO(aschuh): Set linearity if it is useful.
+ # This would be useful if we had any constraints saying B - A x = 0
+
+ # Build a Polyhedra
+ polyhedraptr = libcdd.dd_DDMatrix2Poly(matrixptr)
+
+ if not polyhedraptr:
+ return None
+
+ try:
+ # Return None on error.
+ # The error values are enums, so they aren't exposed.
+
+ # Magic happens here. Computes the vertices
+ vertex_matrixptr = libcdd.dd_CopyGenerators(
+ polyhedraptr)
+ vertex_matrix = vertex_matrixptr.contents
+
+ try:
+ # Count the number of vertices and rays in the result.
+ num_vertices = 0
+ num_rays = 0
+ for i in xrange(vertex_matrix.rowsize):
+ if libcdd.dd_get_d(vertex_matrix.matrix[i][0]) == 0:
+ num_rays += 1
+ else:
+ num_vertices += 1
+
+ # Build zeroed matricies for the new vertices and rays.
+ vertices = numpy.matrix(numpy.zeros((num_vertices,
+ vertex_matrix.colsize - 1)))
+ rays = numpy.matrix(numpy.zeros((num_rays,
+ vertex_matrix.colsize - 1)))
+
+ ray_index = 0
+ vertex_index = 0
+
+ # Copy the data out of the matrix.
+ for index in xrange(vertex_matrix.rowsize):
+ if libcdd.dd_get_d(vertex_matrix.matrix[index][0]) == 0.0:
+ for j in xrange(vertex_matrix.colsize - 1):
+ rays[ray_index, j] = libcdd.dd_get_d(
+ vertex_matrix.matrix[index][j + 1])
+ ray_index += 1
+ else:
+ for j in xrange(vertex_matrix.colsize - 1):
+ vertices[vertex_index, j] = libcdd.dd_get_d(
+ vertex_matrix.matrix[index][j + 1])
+ vertex_index += 1
+ finally:
+ # Free everything.
+ libcdd.dd_FreeMatrix(vertex_matrixptr)
+
+ finally:
+ libcdd.dd_FreePolyhedra(polyhedraptr)
+
+ finally:
+ libcdd.dd_FreeMatrix(matrixptr)
+
+ # Rays are unsupported right now. This may change in the future.
+ assert(rays.shape[0] == 0)
+
+ return vertices
+
+
+ def __str__(self):
+ """Returns a formatted version of the polytope.
+
+ The dump will look something like the following, which prints out the matrix
+ comparison.
+
+ [[ 1 0] [[12]
+ [-1 0] [[x0] <= [12]
+ [ 0 1] [x1]] [12]
+ [ 0 -1]] [12]]
+ """
+ height = max(self.ndim, self.num_constraints)
+
+ # Split the print up into 4 parts and concatenate them all.
+ H_strings = _PadHeight(_SplitAndPad(str(self.H)), height)
+ v_strings = _PadHeight(_SplitAndPad(str(self.k)), height)
+ x_strings = _PadHeight(self._MakeXStrings(), height)
+ cmp_strings = self._MakeCmpStrings(height)
+
+ return '\n'.join(_PiecewiseConcat(H_strings, x_strings,
+ cmp_strings, v_strings))
+
+ def _MakeXStrings(self):
+ """Builds an array of strings with constraint names in it for printing."""
+ x_strings = []
+ if self.ndim == 1:
+ x_strings = ["[[x0]] "]
+ else:
+ for index in xrange(self.ndim):
+ if index == 0:
+ x = "[[x%d] " % index
+ elif index == self.ndim - 1:
+ x = " [x%d]] " % index
+ else:
+ x = " [x%d] " % index
+ x_strings.append(x)
+ return x_strings
+
+ def _MakeCmpStrings(self, height):
+ """Builds an array of strings with the comparison in it for printing."""
+ cmp_strings = []
+ for index in xrange(height):
+ if index == (height - 1) / 2:
+ cmp_strings.append("<= ")
+ else:
+ cmp_strings.append(" ")
+ return cmp_strings
diff --git a/frc971/control_loops/python/polytope_test.py b/frc971/control_loops/python/polytope_test.py
new file mode 100755
index 0000000..9a35ebe
--- /dev/null
+++ b/frc971/control_loops/python/polytope_test.py
@@ -0,0 +1,192 @@
+#!/usr/bin/python
+
+import numpy
+from numpy.testing import *
+import polytope
+import unittest
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+def MakePoint(*args):
+ """Makes a point from a set of arguments."""
+ return numpy.matrix([[arg] for arg in args])
+
+class TestHPolytope(unittest.TestCase):
+ def setUp(self):
+ """Builds a simple box polytope."""
+ self.H = numpy.matrix([[1, 0],
+ [-1, 0],
+ [0, 1],
+ [0, -1]])
+ self.k = numpy.matrix([[12],
+ [12],
+ [12],
+ [12]])
+ self.p = polytope.HPolytope(self.H, self.k)
+
+ def test_Hk(self):
+ """Tests that H and k are saved correctly."""
+ assert_array_equal(self.p.H, self.H)
+ assert_array_equal(self.p.k, self.k)
+
+ def test_IsInside(self):
+ """Tests IsInside for various points."""
+ inside_points = [
+ MakePoint(0, 0),
+ MakePoint(6, 6),
+ MakePoint(12, 6),
+ MakePoint(-6, 10)]
+ outside_points = [
+ MakePoint(14, 0),
+ MakePoint(-14, 0),
+ MakePoint(0, 14),
+ MakePoint(0, -14),
+ MakePoint(14, -14)]
+
+ for inside_point in inside_points:
+ self.assertTrue(self.p.IsInside(inside_point),
+ msg='Point is' + str(inside_point))
+
+ for outside_point in outside_points:
+ self.assertFalse(self.p.IsInside(outside_point),
+ msg='Point is' + str(outside_point))
+
+ def AreVertices(self, p, vertices):
+ """Checks that all the vertices are on corners of the set."""
+ for i in xrange(vertices.shape[0]):
+ # Check that all the vertices have the correct number of active
+ # constraints.
+ lmda = p.H * vertices[i,:].T - p.k
+ num_active_constraints = 0
+ for j in xrange(lmda.shape[0]):
+ # Verify that the constraints are either active, or not violated.
+ if numpy.abs(lmda[j, 0]) <= 1e-9:
+ num_active_constraints += 1
+ else:
+ self.assertLessEqual(lmda[j, 0], 0.0)
+
+ self.assertEqual(p.ndim, num_active_constraints)
+
+ def HasSamePoints(self, expected, actual):
+ """Verifies that the points in expected are in actual."""
+ found_points = set()
+ self.assertEqual(expected.shape, actual.shape)
+ for index in xrange(expected.shape[0]):
+ expected_point = expected[index, :]
+ for actual_index in xrange(actual.shape[0]):
+ actual_point = actual[actual_index, :]
+ if numpy.abs(expected_point - actual_point).max() <= 1e-4:
+ found_points.add(actual_index)
+ break
+
+ self.assertEqual(len(found_points), actual.shape[0],
+ msg="Expected:\n" + str(expected) + "\nActual:\n" + str(actual))
+
+ def test_Skewed_Nonsym_Vertices(self):
+ """Tests the vertices of a severely skewed space."""
+ self.H = numpy.matrix([[10, -1],
+ [-1, -1],
+ [-1, 10],
+ [10, 10]])
+ self.k = numpy.matrix([[2],
+ [2],
+ [2],
+ [2]])
+ self.p = polytope.HPolytope(self.H, self.k)
+ vertices = self.p.Vertices()
+ self.AreVertices(self.p, vertices)
+
+ self.HasSamePoints(
+ numpy.matrix([[0., 0.2],
+ [0.2, 0.],
+ [-2., 0.],
+ [0., -2.]]),
+ vertices)
+
+ def test_Vertices_Nonsym(self):
+ """Tests the vertices of a nonsymetric space."""
+ self.k = numpy.matrix([[6],
+ [12],
+ [2],
+ [10]])
+ self.p = polytope.HPolytope(self.H, self.k)
+ vertices = self.p.Vertices()
+ self.AreVertices(self.p, vertices)
+
+ self.HasSamePoints(
+ numpy.matrix([[6., 2.],
+ [6., -10.],
+ [-12., -10.],
+ [-12., 2.]]),
+ vertices)
+
+ def test_Vertices(self):
+ """Tests the vertices of a nonsymetric space."""
+ self.HasSamePoints(self.p.Vertices(),
+ numpy.matrix([[12., 12.],
+ [12., -12.],
+ [-12., -12.],
+ [-12., 12.]]))
+
+ def test_concat(self):
+ """Tests that the concat function works for simple inputs."""
+ self.assertEqual(["asd", "qwe"],
+ polytope._PiecewiseConcat(["a", "q"],
+ ["s", "w"],
+ ["d", "e"]))
+
+ def test_str(self):
+ """Verifies that the str method works for the provided p."""
+ self.assertEqual('[[ 1 0] [[12] \n'
+ ' [-1 0] [[x0] <= [12] \n'
+ ' [ 0 1] [x1]] [12] \n'
+ ' [ 0 -1]] [12]] ',
+ str(self.p))
+
+ def MakePWithDims(self, num_constraints, num_dims):
+ """Makes a zeroed out polytope with the correct size."""
+ self.p = polytope.HPolytope(
+ numpy.matrix(numpy.zeros((num_constraints, num_dims))),
+ numpy.matrix(numpy.zeros((num_constraints, 1))))
+
+ def test_few_constraints_odd_constraint_even_dims_str(self):
+ """Tests printing out the set with odd constraints and even dimensions."""
+ self.MakePWithDims(num_constraints=5, num_dims=2)
+ self.assertEqual('[[ 0. 0.] [[ 0.] \n'
+ ' [ 0. 0.] [[x0] [ 0.] \n'
+ ' [ 0. 0.] [x1]] <= [ 0.] \n'
+ ' [ 0. 0.] [ 0.] \n'
+ ' [ 0. 0.]] [ 0.]] ',
+ str(self.p))
+
+ def test_few_constraints_odd_constraint_small_dims_str(self):
+ """Tests printing out the set with odd constraints and odd dimensions."""
+ self.MakePWithDims(num_constraints=5, num_dims=1)
+ self.assertEqual('[[ 0.] [[ 0.] \n'
+ ' [ 0.] [ 0.] \n'
+ ' [ 0.] [[x0]] <= [ 0.] \n'
+ ' [ 0.] [ 0.] \n'
+ ' [ 0.]] [ 0.]] ',
+ str(self.p))
+
+ def test_few_constraints_odd_constraint_odd_dims_str(self):
+ """Tests printing out the set with odd constraints and odd dimensions."""
+ self.MakePWithDims(num_constraints=5, num_dims=3)
+ self.assertEqual('[[ 0. 0. 0.] [[ 0.] \n'
+ ' [ 0. 0. 0.] [[x0] [ 0.] \n'
+ ' [ 0. 0. 0.] [x1] <= [ 0.] \n'
+ ' [ 0. 0. 0.] [x2]] [ 0.] \n'
+ ' [ 0. 0. 0.]] [ 0.]] ',
+ str(self.p))
+
+ def test_many_constraints_even_constraint_odd_dims_str(self):
+ """Tests printing out the set with even constraints and odd dimensions."""
+ self.MakePWithDims(num_constraints=2, num_dims=3)
+ self.assertEqual('[[ 0. 0. 0.] [[x0] [[ 0.] \n'
+ ' [ 0. 0. 0.]] [x1] <= [ 0.]] \n'
+ ' [x2]] ',
+ str(self.p))
+
+
+if __name__ == '__main__':
+ unittest.main()