diff --git a/polytope/polytope.py b/polytope/polytope.py index d8785d5..2177d73 100755 --- a/polytope/polytope.py +++ b/polytope/polytope.py @@ -1988,6 +1988,11 @@ def region_diff(poly, reg, abs_tol=ABS_TOL, intersect_tol=ABS_TOL, save=False): """Subtract a region from a polytope + For a discription of the algorithm, see algorithm 2 in: + Mato Baotić "Polytopic computations in constrained optimal control" + Automatika, 2009, Vol. 50, No. 3--4, pp. 119--134, 2009. + https://hrcak.srce.hr/46543 + @param poly: polytope from which to subtract a region @param reg: region which should be subtracted @param abs_tol: absolute tolerance @@ -2056,8 +2061,8 @@ def region_diff(poly, reg, abs_tol=ABS_TOL, intersect_tol=ABS_TOL, return Polytope() # some constraints are active M = np.sum(mi) - if len(mi[0:len(mi) - 1]) > 0: - csum = np.cumsum(np.hstack([0, mi[0:len(mi) - 1]])) + if len(mi) > 1: + csum = np.cumsum(np.hstack([0, mi[0:-1]])) beg_mi = csum + m * np.ones(len(csum), dtype=int) else: beg_mi = np.array([m]) @@ -2076,78 +2081,107 @@ def region_diff(poly, reg, abs_tol=ABS_TOL, intersect_tol=ABS_TOL, ax.figure.savefig('./img/res' + str(res_count) + '.pdf') res_count += 1 if counter[level] == 0: + # This is the first visit to this level if save: logger.debug('counter[level] is 0') + # Go through all remaining polytopes that we want to remove and + # check whether any of them removes anything from the current set + # of hyperplanes (constraints) for j in xrange(level, N): + # Construct a set of constraints with the current hyperplanes + # and the hyperplanes of polytope j. + # This is the intersection of the current hyperplanes and + # polytope j auxINDICES = np.hstack([ INDICES, range(beg_mi[j], beg_mi[j] + mi[j]) ]) Adummy = A[auxINDICES, :] bdummy = B[auxINDICES] - R, xopt = cheby_ball(Polytope(Adummy, bdummy)) + # Will polytope j remove anything from the current set of + # hyperplanes? + R, _ = cheby_ball(Polytope(Adummy, bdummy)) if R > abs_tol: + # Yes, constrain the set of constraints with the negation + # of one of polytope j's hyperplanes level = j counter[level] = 1 + # Offset M negates hyperplanes INDICES = np.hstack([INDICES, beg_mi[level] + M]) break if R < abs_tol: - level = level - 1 + # Since no polytope will remove anything, the current set of + # hyperplanes must be in the result res = union(res, Polytope(A[INDICES, :], B[INDICES]), False) - nzcount = np.nonzero(counter)[0] - for jj in xrange(len(nzcount) - 1, -1, -1): - if counter[level] <= mi[level]: - INDICES[len(INDICES) - - 1] = INDICES[len(INDICES) - 1] - M - INDICES = np.hstack([ - INDICES, - beg_mi[level] + counter[level] + M - ]) - break - else: - counter[level] = 0 - INDICES = INDICES[0:m + sum(counter)] - if level == -1: - logger.debug('returning res from 1st point') - return res + # None of the remaining polytopes removes anything + # Indicate that we are done at this level + counter[level] = mi[level] + # Add the polytope to the indices to get an empty intersection + # This will force the algorithm to pop out of this level and + # move on + INDICES = np.hstack([ + INDICES, + range(beg_mi[level], beg_mi[level] + mi[level]) + ]) else: + # We have been at this level before if save: logger.debug('counter[level] > 0') # counter(level) > 0 nzcount = np.nonzero(counter)[0] + # Start from the deepest level and pop out of it if we are done at + # that level for jj in xrange(len(nzcount) - 1, -1, -1): level = nzcount[jj] - counter[level] = counter[level] + 1 + counter[level] += 1 + # Have we checked all hyperplanes? if counter[level] <= mi[level]: - INDICES[len(INDICES) - 1] = INDICES[len(INDICES) - 1] - M + # No + # We have already handled the negative side of the last + # hyperplane, so negate it to get the + # positive side + INDICES[-1] -= M + # Add the next negative hyperplane of the polytope of the + # current level INDICES = np.hstack([ INDICES, beg_mi[level] + counter[level] + M - 1 ]) + # Stay at this level, since we haven't checked all + # hyperplanes yet break else: + # We have checked all hyperplanes at this level + # Pop out of this level by resetting our states counter[level] = 0 INDICES = INDICES[0:m + np.sum(counter)] - level = level - 1 + level -= 1 if level == -1: + # We're done at all levels, return if save: - if save: - if res: - ax = res.plot() - ax.axis([0.0, 1.0, 0.0, 2.0]) - ax.figure.savefig('./img/res_returned' - + str(res_count) - + '.pdf') + if res: + ax = res.plot() + ax.axis([0.0, 1.0, 0.0, 2.0]) + ax.figure.savefig('./img/res_returned' + + str(res_count) + + '.pdf') logger.debug('returning res from 2nd point') return res test_poly = Polytope(A[INDICES, :], B[INDICES]) - rc, xc = cheby_ball(test_poly) + rc, _ = cheby_ball(test_poly) + # Do we have a non-empty diff at this level? if rc > abs_tol: if level == N - 1: + # The diff is non-empty, and we have no more polytopes to + # remove. + # Add the current set of constraints to the result. res = union(res, reduce(test_poly), False) else: - level = level + 1 + # The diff is non-empty, but we need to check whether the + # remaining polytopes remove anything. + # Go one level deeper to continue the search. + level += 1 logger.debug('returning res from end') return res diff --git a/tests/polytope_test.py b/tests/polytope_test.py index 55c69b0..aa836cb 100644 --- a/tests/polytope_test.py +++ b/tests/polytope_test.py @@ -486,5 +486,176 @@ def is_glpk_present(): return False +def test_simple_region_diff(): + # Test `polytope.polytope.region_diff` with very simple input + # Set up a square and remove half of it + domain = pc.Polytope.from_box([(0.0, 1.0), (0.0, 1.0)]) + region = pc.Region([pc.Polytope.from_box([(0.0, 0.5), (0.0, 1.0)])]) + # Do the diff and compare with exprected result + diff = pc.polytope.region_diff(domain, region) + remaining = pc.Polytope.from_box([(0.5, 1.0), (0.0, 1.0)]) + assert diff == remaining, (diff, remaining) + + +def test_convex_region_diff_1(): + # Test `polytope.polytope.region_diff` when the output should be one + # polytope + # Set up a square and remove 3/4 by subtracting 2 boxes + domain = pc.Polytope.from_box([(0.0, 1.0), (0.0, 1.0)]) + region = pc.Region([ + pc.Polytope.from_box([(0.0, 0.5), (0.0, 1.0)]), + pc.Polytope.from_box([(0.0, 1.0), (0.0, 0.5)]), + ]) + # Do the diff and compare with exprected result + diff = pc.polytope.region_diff(domain, region) + remaining = pc.Polytope.from_box([(0.5, 1.0), (0.5, 1.0)]) + assert diff == remaining, (diff, remaining) + + +def test_convex_region_diff_2(): + # Test `polytope.polytope.region_diff` when the output should be one + # polytope + # Set up a square and remove 3/4 by subtracting 3 boxes + domain = pc.Polytope.from_box([(0.0, 1.0), (0.0, 1.0)]) + region = pc.Region([ + pc.Polytope.from_box([(0.0, 0.5), (0.5, 1.0)]), + pc.Polytope.from_box([(0.5, 1.0), (0.0, 0.5)]), + pc.Polytope.from_box([(0.0, 0.5), (0.0, 0.5)]), + ]) + # Do the diff and compare with expected result + diff = pc.polytope.region_diff(domain, region) + remaining = pc.Polytope.from_box([(0.5, 1.0), (0.5, 1.0)]) + assert diff == remaining, (diff, remaining) + + +def test_non_convex_region_diff(): + # Test `polytope.polytope.region_diff` when the output should be non-convex + # Set up a square and remove 1/4 by subtracting 1 square box + domain = pc.Polytope.from_box([(0.0, 1.0), (0.0, 1.0)]) + region = pc.Region([ + pc.Polytope.from_box([(0.0, 0.5), (0.0, 0.5)]) + ]) + # Do the diff and compare with expected result + diff = pc.polytope.region_diff(domain, region) + remaining1 = pc.Polytope.from_box([(0.5, 1.0), (0.0, 1.0)]) + remaining2 = pc.Polytope.from_box([(0.0, 0.5), (0.5, 1.0)]) + assert pc.is_fulldim(diff.diff(remaining1)), diff + assert pc.is_fulldim(diff.diff(remaining2)), diff + assert not pc.is_fulldim(diff.diff(remaining1).diff(remaining2)), diff + + +def test_checker_region_diff(): + # Test `polytope.polytope.region_diff` when the output should be non-convex + # Set up a square and remove 1/2 by subtracting 2 square boxes on the + # diagonal + domain = pc.Polytope.from_box([(0.0, 1.0), (0.0, 1.0)]) + region = pc.Region([ + pc.Polytope.from_box([(0.0, 0.5), (0.0, 0.5)]), + pc.Polytope.from_box([(0.5, 1.0), (0.5, 1.0)]), + ]) + # Do the diff and compare with expected result + diff = pc.polytope.region_diff(domain, region) + remaining1 = pc.Polytope.from_box([(0.5, 1.0), (0.0, 0.5)]) + remaining2 = pc.Polytope.from_box([(0.0, 0.5), (0.5, 1.0)]) + assert pc.is_fulldim(diff.diff(remaining1)), diff + assert pc.is_fulldim(diff.diff(remaining2)), diff + assert not pc.is_fulldim(diff.diff(remaining1).diff(remaining2)), diff + + +def test_triangles_region_diff(): + # Test `polytope.polytope.region_diff` when the output should be convex + # Test with something else than boxes + poly_region = pc.Region() + # First polytope to subtract is + # (0, 0) -- (3, 0) -- (3, 1) -- (1, 1) -- (0, 0) + a = np.array([ + [-1.0, 1.0], + [0.0, -1.0], + [1.0, 0.0], + [0.0, 1.0] + ]) + b = np.array([ + 0.0, + 0.0, + 3.0, + 1.0, + ]) + poly_region = poly_region.union(pc.Polytope(a, b), check_convex=False) + # + # Second polytope to subtract is + # (3, 0) -- (4, 0) -- (3, 1) -- (3, 0) + a = np.array([ + [-1.0, 0.0], + [0.0, -1.0], + [1.0, 1.0] + ]) + b = np.array([ + -3.0, + 0.0, + 4.0, + ]) + poly_region = poly_region.union(pc.Polytope(a, b), check_convex=False) + # + # Third polytope to subtract is + # (4, 0) -- (4, 1) -- (3, 1) -- (4, 0) + a = np.array([ + [1.0, 0.0], + [0.0, 1.0], + [-1.0, -1.0] + ]) + b = np.array([ + 4.0, + 1.0, + -4.0, + ]) + poly_region = poly_region.union(pc.Polytope(a, b), check_convex=False) + # + # Fourth polytope to subtract is + # (3, 1) -- (4, 1) -- (4, 4) -- (3, 3) -- (3, 1) + a = np.array([ + [1.0, 0.0], + [-1.0, 0.0], + [0.0, -1.0], + [-1.0, 1.0] + ]) + b = np.array([ + 4.0, + -3.0, + -1.0, + 0.0, + ]) + poly_region = poly_region.union(pc.Polytope(a, b), check_convex=False) + # + # The polytope to subtract from is + # (0, 0) -- (4, 0) -- (4, 4) -- (0, 0) + a = np.array([ + [1.0, 0.0], + [0.0, -1.0], + [-1.0, 1.0], + ]) + b = np.array([ + 4.0, + 0.0, + 0.0, + ]) + poly_domain = pc.Polytope(a, b) + # + # Do the diff + poly_diff = poly_domain.diff(poly_region) + # Set up the expected result + a = np.array([ + [-1.0, 1.0], + [1.0, 0.0], + [0.0, -1.0], + ]) + b = np.array([ + 0.0, + 3.0, + -1.0, + ]) + remaining_poly = pc.Polytope(a, b) + assert poly_diff == remaining_poly, (poly_diff, remaining_poly) + + if __name__ == '__main__': pass