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