def precursor(Sset, A, Uset=pt.Polytope(), B=np.array([])):
""" The Pre(S) is the set of states which evolve into the target set S in one time step. """
if not B.any():
return pt.Polytope(Sset.A @ A, Sset.b) # (HA, b)
else:
tmp = minkowski_sum( Sset, pt.extreme(Uset) @ -B.T )
return pt.Polytope(tmp.A @ A, tmp.b)
def comparison_test(self):
p = pc.Polytope(self.A, self.b)
p2 = pc.Polytope(self.A, 2*self.b)
assert(p <= p2)
assert(not p2 <= p)
assert(not p2 == p)
r = pc.Region([p])
r2 = pc.Region([p2])
assert(r <= r2)
assert(not r2 <= r)
assert(not r2 == r)
# test H-rep -> V-rep -> H-rep
v = pc.extreme(p)
p3 = pc.qhull(v)
assert(p3 == p)
# test V-rep -> H-rep with d+1 points
p4 = pc.qhull(np.array([[0, 0], [1, 0], [0, 1]]))
assert(p4 == pc.Polytope(
np.array([[1, 1], [0, -1], [0, -1]]),
np.array([1, 0, 0])))
def plot(
self,
show_id=True,
show_edges_id=True,
show_error_bounds=False,
ax=None,
as_goal=False,
):
if show_error_bounds:
A, b = self.as_poly["original"].A, self.as_poly["original"].b
e_l, e_r = self.get_error_bounds()
e = np.array(self.actual_errors)
pc.Polytope(A, b + e_l).plot(ax=ax,
alpha=0.2,
linewidth=2,
color="dimgrey")
pc.Polytope(A, b + e_r).plot(ax=ax,
alpha=0.2,
linewidth=2,
color="dimgrey")
pc.Polytope(A, b + e).plot(ax=ax,
alpha=0.6,
color="cornflowerblue",
linewidth=2,
linestyle="-")
super().plot(show_id=show_id,
show_edges_id=show_edges_id,
as_goal=as_goal,
mask=True)
def region_rotation_test(self):
p = pc.Region([pc.Polytope(self.A, self.b)])
p1 = pc.Region([pc.Polytope(self.A, self.b)])
p2 = pc.Region([pc.Polytope(self.Ab2[:, 0:2], self.Ab2[:, 2])])
p3 = pc.Region([pc.Polytope(self.Ab3[:, 0:2], self.Ab3[:, 2])])
p4 = pc.Region([pc.Polytope(self.Ab4[:, 0:2], self.Ab4[:, 2])])
p = p.rotation(0, 1, np.pi / 2)
print(p.bounding_box)
assert (p == p2)
assert (not p == p3)
assert (not p == p4)
assert (not p == p1)
assert_allclose(p.chebXc, [-0.5, 0.5])
p = p.rotation(0, 1, np.pi / 2)
assert (p == p3)
assert_allclose(p.chebXc, [-0.5, -0.5])
p = p.rotation(0, 1, np.pi / 2)
assert (p == p4)
assert_allclose(p.chebXc, [0.5, -0.5])
p = p.rotation(0, 1, np.pi / 2)
assert (p == p1)
assert_allclose(p.chebXc, [0.5, 0.5])
def precursor(Sset,A,Uset = pt.Polytope(),B = np.array([])):
# see definition of Pre(S) in slides
if not B.any(): # if B is nothing
return pt.Polytope(Sset.A @ A ,Sset.b)
else:
tmp = minkowski_sum(Sset,pt.extreme(Uset) @ -B.T)
return pt.Polytope(tmp.A @ A, tmp.b)
def region_empty_test(self):
# Note that as of commit a037b555758ed9ee736fa7cb324d300b8d622fb4
# Region.__init__ deletes empty polytopes from
# the given list of polytopes at instantiation.
reg = pc.Region()
reg.list_poly = [pc.Polytope(), pc.Polytope()]
assert len(reg) > 0
assert pc.is_empty(reg)
def polytope_translation_test(self):
p = pc.Polytope(self.A, self.b)
p1 = pc.Polytope(self.A, self.b)
p2 = pc.Polytope(self.Ab2[:, 0:2], self.Ab2[:, 2])
p = p.translation([-1, 0])
assert (p == p2)
assert (not p == p1)
p = p.translation([1, 0])
assert (p == p1)
def prop2part_test():
state_space = pc.Polytope.from_box(np.array([[0., 2.],[0., 2.]]))
cont_props = []
A = []
b = []
A.append(np.array([[1., 0.],
[-1., 0.],
[0., 1.],
[0., -1.]]))
b.append(np.array([[.5, 0., .5, 0.]]).T)
cont_props.append(pc.Polytope(A[0], b[0]))
A.append(np.array([[1., 0.],
[-1., 0.],
[0., 1.],
[0., -1.]]))
b.append(np.array([[2., -1.5, 2., -1.5]]).T)
cont_props.append(pc.Polytope(A[1], b[1]))
cont_props_dict = {"C"+str(i) : pc.Polytope(A[i], b[i]) for i in range(2)}
mypartition = prop2part(state_space, cont_props_dict)
print(mypartition)
ref_adjacency = np.array([[1,0,1],[0,1,1],[1,1,1]])
assert np.all(mypartition.adj.todense() == ref_adjacency)
assert len(mypartition.regions) == 3
for reg in mypartition.regions[0:2]:
assert len(reg.props) == 1
assert len(reg) == 1
assert cont_props_dict == mypartition.prop_regions
assert len(mypartition.regions[2].props) == 0
assert len(mypartition.regions[2]) == 3
dum = state_space.copy()
for reg in mypartition.regions[0:2]:
dum = dum.diff(reg)
assert pc.is_empty(dum.diff(mypartition.regions[2]) )
assert pc.is_empty(mypartition.regions[2].diff(dum) )
assert(mypartition.preserves_predicates())
# invalidate it
mypartition.regions += [pc.Region([pc.Polytope(A[0], b[0])], {})]
assert(not mypartition.preserves_predicates())
def region_full_dim_test(self):
assert not pc.is_fulldim(pc.Region())
p1 = pc.Polytope(self.A, self.b)
p2 = pc.Polytope(self.Ab2[:, 0:2], self.Ab2[:, 2])
reg = pc.Region([p1, p2])
assert pc.is_fulldim(reg)
# Adding empty polytopes should not affect the
# full-dimensional status of this region.
reg.list_poly.append(pc.Polytope())
assert pc.is_fulldim(reg)
reg.list_poly.append(pc.Polytope(self.A, self.b - 1e3))
assert pc.is_fulldim(reg)
def polytope_intersect_test(self):
p1 = pc.Polytope(self.A, self.b)
p2 = pc.Polytope(self.Ab2[:, 0:2], self.Ab2[:, 2])
p3 = p1.intersect(p2)
assert pc.is_fulldim(p1)
assert pc.is_fulldim(p2)
assert not pc.is_fulldim(p3)
# p4 is the unit square with center at the origin.
p4 = pc.Polytope(np.array([[1., 0.], [0., 1.], [-1., 0.], [0., -1.]]),
np.array([0.5, 0.5, 0.5, 0.5]))
p5 = p2.intersect(p4)
assert pc.is_fulldim(p4)
assert pc.is_fulldim(p5)
def add_final_constraint(Goal, N, bloat_func, alpha):
Af = Goal[0]
bf = Goal[1]
x_dim = len(Af[0])
poly = pc.Polytope(Af, bf)
pt = poly.chebXc # Find the center of the goal set
fmlas = []
str0 = ''
for dim in range(
x_dim
): # Make sure that ending point is at the center of the goal set
str0 = '(= x_%sref[%s] %s)' % (dim + 1, N, pt[dim])
fmlas.append(str0)
# Additionally, make sure all the error is contained within the goal set
r = bloat_func(N - 1, alpha)
for row in range(len(Af)):
b = bf[row][0] - r
for dim in range(x_dim):
a = Af[row][dim]
str1 = '(* %s x_%sref[%s])' % (a, dim + 1, N)
if dim == 0:
str0 = str1
else:
str0 = '(+ %s %s)' % (str0, str1)
fmla = '(<= %s %s)' % (str0, b)
fmlas.append(fmla)
return fmlas
def check_empty(poly, method='polytope-lp'):
'''
checks whether a polytope in (A,b) representation is empty
variety of methods
'''
A, b = poly
if method == 'polytope-fulldim':
poly = pc.Polytope(A=A, b=b, normalize=False)
empty = not pc.is_fulldim(poly)
elif method == 'cvxpy':
# this is 10x slower than polytope
v = cvx.Variable(A.shape[1])
prob = cvx.Problem(cvx.Minimize(cvx.norm(v)), [A@v <= b])
try:
prob.solve()
empty = prob.status == "infeasible"
except cvx.SolverError:
empty = True
elif method == 'polytope-lp':
c = np.ones(A.shape[1])
res = pc.solvers.lpsolve(c, A, b, solver='glpk') # 'mosek')
empty = res['status'] != 0
else:
raise NotImplementedError('method {} not implemented'.format(method))
return empty
def erode(poly, eps):
"""
Given a polytope compute eps erosion and give polytope under approximation
For a given polytope a polytopic under approximation of the $eps$-eroded set is computed.
An e-eroded Pe set of P is defined as:
Pe = {x |x+n in P forall n in Ball(e)}
where Ball(e) is the epsilon neighborhood with norm |n|<e
The current implementation shifts hyper-planes with eps over there normal,
/ / / / / / / | + |
/ / / / / / /| eps |
/ / / / / / /| + |
/ / / / / / /| eps |
/ / / / / / /| + |
:param poly: original polytope
:param eps: positive scalar value with which the polytope is eroded
:return: polytope
"""
if isinstance(poly, polytope.Region):
er_reg = []
for pol in poly.list_poly:
assert isinstance(pol, polytope.Polytope)
er_reg += [erode(pol, eps)]
return polytope.Region(er_reg)
A = poly.A
b = poly.b
b_e = []
for A_i, b_i in itertools.product(A, b):
b_e += [[b_i - eps * np.linalg.norm(A_i, 2)]]
return polytope.Polytope(A, np.array(b_e))
def _solve_closed_loop_bounded_horizon(
P1, P2, ssys, N, trans_set=None):
"""Under-approximate states in P1 that can reach P2 in <= N steps.
See docstring of function `_solve_closed_loop_fixed_horizon`
for details.
"""
_print_horizon_warning()
p1 = P1.copy() # initial set
p2 = P2.copy() # terminal set
if trans_set is None:
pinit = p1
else:
pinit = trans_set
# backwards in time
s = pc.Region()
for i in xrange(N, 0, -1):
# first step from P1
if i == 1:
pinit = p1
p2 = solve_open_loop(pinit, p2, ssys, 1, trans_set)
p2 = pc.reduce(p2)
# running union
s = s.union(p2, check_convex=True)
s = pc.reduce(s)
# empty target polytope ?
if not pc.is_fulldim(p2):
break
if not pc.is_fulldim(s):
return pc.Polytope()
s = pc.reduce(s)
return s
def solve_open_loop(
P1, P2, ssys, N,
trans_set=None, max_num_poly=5
):
r1 = P1.copy() # Initial set
r2 = P2.copy() # Terminal set
# use the max_num_poly largest volumes for reachability
r1 = volumes_for_reachability(r1, max_num_poly)
r2 = volumes_for_reachability(r2, max_num_poly)
if len(r1) > 0:
start_polys = r1
else:
start_polys = [r1]
if len(r2) > 0:
target_polys = r2
else:
target_polys = [r2]
# union of s0 over all polytope combinations
s0 = pc.Polytope()
for p1 in start_polys:
for p2 in target_polys:
cur_s0 = poly_to_poly(p1, p2, ssys, N, trans_set)
s0 = s0.union(cur_s0, check_convex=True)
return s0
def test_polytope_str():
# 1 constaint (so uniline)
A = np.array([[1]])
b = np.array([1])
p = pc.Polytope(A, b)
s = str(p)
s_ = 'Single polytope \n [[1.]] x <= [[1.]]\n'
assert s == s_, (s, s_)
# > 1 constraints (so multiline)
polys = dict(p1d=[[0, 1]],
p2d=[[0, 1], [0, 2]],
p3d=[[0, 1], [0, 2], [0, 3]])
strings = dict(
p1d='Single polytope \n [[ 1.] x <= [[1.]\n [-1.]]| [0.]]\n',
p2d=('Single polytope \n [[ 1. 0.] | [[1.]\n [ 0. 1.] '
'x <= [2.]\n [-1. -0.] | [0.]\n [-0. -1.]]|'
' [0.]]\n'),
p3d=('Single polytope \n [[ 1. 0. 0.] | [[1.]\n '
'[ 0. 1. 0.] | [2.]\n [ 0. 0. 1.] x <= [3.]\n'
' [-1. -0. -0.] | [0.]\n [-0. -1. -0.] |'
' [0.]\n [-0. -0. -1.]]| [0.]]\n'))
for name, poly in polys.items():
p = pc.Polytope.from_box(poly)
s = str(p)
s_ = strings[name]
assert s == s_, (s, s_)
def load(filename):
data = scipy.io.loadmat(filename)
islti = bool(data['islti'][0][0])
ispwa = bool(data['ispwa'][0][0])
if islti:
sys = load_lti(data['A'], data['B'], data['domainA'], data['domainB'],
data['UsetA'], data['UsetB'])
elif ispwa:
nlti = len(data['A'][0])
lti_systems = []
for i in xrange(nlti):
A = data['A'][0][i]
B = data['B'][0][i]
K = data['K'][0][i]
domainA = data['domainA'][0][i]
domainB = data['domainB'][0][i]
UsetA = data['UsetA'][0][i]
UsetB = data['UsetB'][0][i]
ltisys = load_lti(A, B, K, domainA, domainB, UsetA, UsetB)
lti_systems.append(ltisys)
cts_ss = polytope.Polytope(data['ctsA'], data['ctsB'])
sys = hybrid.PwaSysDyn(list_subsys=lti_systems, domain=cts_ss)
return sys
def _underapproximate_attractor(
P1, P2, ssys, N, trans_set=None):
"""Under-approximate N-step attractor of polytope P2, with N > 0.
See docstring of function `_solve_closed_loop_fixed_horizon`
for details.
"""
assert N > 0, N
_print_horizon_warning()
p1 = P1.copy() # initial set
p2 = P2.copy() # terminal set
if trans_set is None:
pinit = p1
else:
pinit = trans_set
# backwards in time
for i in xrange(N, 0, -1):
# first step from P1
if i == 1:
pinit = p1
r = solve_open_loop(pinit, p2, ssys, 1, trans_set)
p2 = p2.union(r, check_convex=True)
p2 = pc.reduce(p2)
# empty target polytope ?
if not pc.is_fulldim(p2):
return pc.Polytope()
return r
def find_min_set(self, irmp_constraints):
A_rmp = np.empty((len(irmp_constraints), len(self.limits)))
for ix in range(len(irmp_constraints)):
A_rmp[ix, :] = irmp_constraints[ix][0]
b_rmp = np.array([val[1] for val in irmp_constraints])
A_stack, b_stack = np.row_stack((self.rmp.A, A_rmp)), np.concatenate(
(self.rmp.b, b_rmp))
self.rmp = polytope.reduce(polytope.Polytope(A_stack, b_stack))
def shrinkPoly(origPoly, epsilon):
"""Returns a polytope shrunk a distance 'epsilon'
to the edges from the Polytope origPoly.
"""
A = origPoly.A.copy()
b = origPoly.b.copy()
for i in range(A.shape[0]):
b[i] = b[i] - epsilon * np.linalg.norm(A[i][:])
return pc.reduce(pc.Polytope(A, b))
def _import_polytope(node):
# Get the A matrix
A = _import_xml(node.findall('A')[0])
# Get the b matrix
b = _import_xml(node.findall('b')[0])
return polytope.Polytope(A=A, b=b)
def tension_space_polytope(t_min, t_max):
"""
:param t_min: minimum allowable tension value
:param t_max: maxmimum allowable tension value
:return: tension space polytope
"""
A_desired = np.vstack((np.eye(4, 4), -1 * np.eye(4, 4)))
B_desired = np.vstack((t_max * np.ones([4, 1]), -t_min * np.ones([4, 1])))
tension_space_Hrep = polytope.Polytope(A_desired, B_desired)
tension_space_Vrep = polytope.extreme(tension_space_Hrep)
return tension_space_Vrep
def test_plot():
A_desired = np.vstack((np.eye(3, 3), -1 * np.eye(3, 3)))
B_desired = np.ones([6, 1])
print(A_desired)
print(B_desired)
desired_twist = polytope.Polytope(A_desired, B_desired)
print(desired_twist.volume)
V = polytope.extreme(desired_twist)
print("polytope vertices=", polytope.extreme(desired_twist))
print("desired_twist vertices=", V)
polytope_functions.plot_polytope_3d(desired_twist)
def __init__(self,
start,
goal,
obstacle_list,
rand_area,
expand_dis=3.0,
path_resolution=0.01,
goal_sample_rate=5,
max_iter=5000):
"""
Setting Parameter
start:Start Position [x,y]
goal:Goal Position [x,y]
obstacleList:obstacle Positions [[x,y,size],...]
randArea:Random Sampling Area [min,max]
"""
"""
New Parameters
start set: [A_start, b_start] -> Use Chebyshev center as starting point
goal set: [A_goal, b_goal] -> Use Chebyshev center as goal point
"""
start_poly = pc.Polytope(start[0], start[1])
end_poly = pc.Polytope(goal[0], goal[1])
self.start_vtc = ppm.compute_polytope_vertices(start[0], start[1])
self.end_vtc = ppm.compute_polytope_vertices(goal[0], goal[1])
start_pt = start_poly.chebXc
end_pt = end_poly.chebXc
self.start = self.Node(start_pt[0], start_pt[1])
self.end = self.Node(end_pt[0], end_pt[1])
self.min_rand = rand_area[0]
self.max_rand = rand_area[1]
self.expand_dis = expand_dis
self.path_resolution = path_resolution
self.goal_sample_rate = goal_sample_rate
self.max_iter = max_iter
self.obstacle_list = obstacle_list
self.node_list = []
def polytope_contains_test(self):
p = pc.Polytope(self.A, self.b)
# single point
point_i = [0.1, 0.3]
point_o = [2, 0]
assert point_i in p
assert point_o not in p
# multiple points
many_points_i = np.random.random((2, 8))
many_points_0 = np.random.random((2, 8)) - np.array([[0], [1]])
many_points = np.concatenate([many_points_0, many_points_i], axis=1)
truth = np.array([False] * 8 + [True] * 8, dtype=bool)
assert_array_equal(p.contains(many_points), truth)
def tension_space_polytope(t_min_, t_max_, n):
"""
:param n: number of cables
:param t_min_: minimum allowable tension value
:param t_max_: maxmimum allowable tension value
:return: tension space polytope
"""
A_desired = np.vstack((np.eye(n, n), -1 * np.eye(n, n)))
B_desired = np.vstack(
(t_max_ * np.ones([n, 1]), -t_min_ * np.ones([n, 1])))
tension_space_Hrep = polytope.Polytope(A_desired, B_desired)
tension_space_Vrep = polytope.extreme(tension_space_Hrep)
return tension_space_Vrep
def sense_env_bool_dyn(O, O_dyn_sensed_bool, robotx, roboty, sensor_dist = 5):
# This is horrible but not worth solving quadratic minimization problem since in 2d with rectangles
for i in range(len(O)):
ob, dir = O[i][0], O[i][1]
A, b = ob[0], ob[1]
fat_b = np.array([[b[0] + sensor_dist], [b[1] + sensor_dist], [b[2] + sensor_dist], [b[3] + sensor_dist]])
p = pc.Polytope(A, fat_b)
if [robotx, roboty] in p and O_dyn_sensed_bool[i] == False:
O_dyn_sensed_bool[i] = True
# else:
# print('not detected')
return O_dyn_sensed_bool
def get_polytope(self, ball=None):
'''
return a polytope representing the intersections of current region and
regions of all parents.
'''
if self.region is None: return pc.Polytope()
# ensures that polytopes are bounded
A_ball, b_ball = ball if ball is not None else self.ball
region = self.get_region_list()
As = [A for A,_ in region]
bs = [b for _,b in region]
return (np.vstack([A_ball] + As),np.hstack([b_ball] + bs))
def region_contains_test(self):
A = np.array([[1.0], [-1.0]])
b = np.array([1.0, 0.0])
poly = pc.Polytope(A, b)
polys = [poly]
reg = pc.Region(polys)
assert 0.5 in reg
# small positive tolerance (includes boundary)
points = np.array([[-1.0, 0.0, 0.5, 1.0, 2.0]])
c = reg.contains(points)
c_ = np.array([[False, True, True, True, False]], dtype=bool)
# zero tolerance (excludes boundary)
points = np.array([[-1.0, 0.0, 0.5, 1.0, 2.0]])
c = reg.contains(points, abs_tol=0)
c_ = np.array([[False, False, True, False, False]], dtype=bool)
assert np.all(c == c_), c
def add_initial_constraint(Theta): A0 = Theta[0] b0 = Theta[1] x_dim = len(A0[0]) fmlas = [] str0 = '' poly = pc.Polytope(A0, b0) pt = poly.chebXc # Find the center of the initial set for dim in range(x_dim): # Make sure that starting point is at the center of the initial set str0 = '(= x_%sref[0] %s)'%(dim+1, pt[dim]) fmlas.append(str0) return fmlas
