frc971/control_loops/python/polytope_test.py

#!/usr/bin/python3

import numpy
from numpy.testing import *
import unittest

import frc971.control_loops.python.polytope as polytope

__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 range(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 range(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 range(expected.shape[0]):
            expected_point = expected[index, :]
            for actual_index in range(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"],
                         list(
                             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()