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_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 range(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 _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 range(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 _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 Oinf(A, Xset):
Omega = Xset
k = 0
Omegap = precursor(Omega, A).intersect(Omega)
while not Omegap == Omega:
k += 1
Omega = Omegap
Omegap = pt.reduce(precursor(Omega, A).intersect(Omega))
return Omegap
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 poly_to_poly(p1, p2, ssys, N, trans_set=None):
"""Compute s0 for open-loop polytope to polytope N-reachability.
"""
p1 = p1.copy()
p2 = p2.copy()
if trans_set is None:
trans_set = p1
# stack polytope constraints
L, M = createLM(ssys, N, p1, trans_set, p2)
s0 = pc.Polytope(L, M)
s0 = pc.reduce(s0)
# Project polytope s0 onto lower dim
n = np.shape(ssys.A)[1]
dims = range(1, n+1)
s0 = s0.project(dims)
return pc.reduce(s0)
def max_cntr_inv(A,B,X,U):
maxIterations = 500
# initialization
Omega0 = X
for i in range(maxIterations):
# compute backward reachable set
P = precursor(Omega0, A, U, B)
# intersect with the state constraints
P = pt.reduce(P).intersect(Omega0)
if P == Omega0:
Cinf = Omega0
break
else:
Omega0 = P
if i == maxIterations:
converged = 0
else:
converged = 1
return Cinf, converged
def max_pos_inv(A, S):
maxIterations = 500
# initialization
Omega_i = S
for i in range(maxIterations):
# compute backward reachable set
P = precursor(Omega_i, A)
# intersect with the state constraints
P = pt.reduce(P).intersect(Omega_i)
if P == Omega_i:
Oinf = Omega_i
break
else:
Omega_i = P
if i == maxIterations:
converged = 0
else:
converged = 1
return Oinf, converged
def _solve_closed_loop_fixed_horizon(
P1, P2, ssys, N, trans_set=None):
"""Under-approximate states in P1 that can reach P2 in N > 0 steps.
If intermediate polytopes are convex,
then the result is exact and not an under-approximation.
@type P1: C{Polytope} or C{Region}
@type P2: C{Polytope} or C{Region}
@param ssys: system dynamics
@param N: horizon length
@type N: int > 0
@param trans_set: If provided,
then intermediate steps are allowed
to be in trans_set.
Otherwise, P1 is used.
"""
assert N > 0, N
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 range(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)
if not pc.is_fulldim(p2):
return pc.Polytope()
return p2
def _solve_closed_loop_fixed_horizon(
P1, P2, ssys, N, trans_set=None):
"""Under-approximate states in P1 that can reach P2 in N > 0 steps.
If intermediate polytopes are convex,
then the result is exact and not an under-approximation.
@type P1: C{Polytope} or C{Region}
@type P2: C{Polytope} or C{Region}
@param ssys: system dynamics
@param N: horizon length
@type N: int > 0
@param trans_set: If provided,
then intermediate steps are allowed
to be in trans_set.
Otherwise, P1 is used.
"""
assert N > 0, N
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
p2 = solve_open_loop(pinit, p2, ssys, 1, trans_set)
p2 = pc.reduce(p2)
if not pc.is_fulldim(p2):
return pc.Polytope()
return p2
b = np.array([[10],
[10],
[10],
[10]])
P = pt.Polytope(A,b)
if False:
fig, ax = plt.subplots(1,1)
plt.rcParams['figure.figsize'] = [20, 20]
P.plot(ax, color='r')
ax.autoscale_view()
ax.axis('equal')
plt.show()
# reduce
P = pt.reduce(P)
print(P)
# HV conversion
V=np.array([[10,10],[-10,10],[10,-10],[-10,-10]])
P = pt.qhull(V)
print(P)
V1 = pt.extreme(P)
print(V1)
# Minkwoski sum of two Polytopes
def minkowski_sum(X,Y):
v_sum = []
if isinstance(X,pt.Polytope):
def solve_closed_loop(
P1, P2, ssys, N,
use_all_horizon=False, trans_set=None
):
"""Compute S0 \subseteq P1 from which P2 is closed-loop N-reachable.
@type P1: C{Polytope} or C{Region}
@type P2: C{Polytope} or C{Region}
@param ssys: system dynamics
@param N: horizon length
@type N: int > 0
@param use_all_horizon:
- if True, then take union of S0 sets
- Otherwise, chain S0 sets (funnel-like)
@type use_all_horizon: bool
@param trans_set: If provided,
then intermediate steps are allowed
to be in trans_set.
Otherwise, P1 is used.
"""
if use_all_horizon:
raise ValueError('solve_closed_loop() with use_all_horizon=True '
'is still under development\nand currently '
'unavailable.')
p1 = P1.copy() # Initial set
p2 = P2.copy() # Terminal set
if trans_set is not None:
Pinit = trans_set
else:
Pinit = p1
# backwards in time
s0 = pc.Region()
reached = False
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)
s0 = s0.union(p2, check_convex=True)
s0 = pc.reduce(s0)
# empty target polytope ?
if not pc.is_fulldim(p2):
break
old_reached = reached
# overlaps initial set ?
if p1.intersect(p2):
s0 = s0.union(p2, check_convex=True)
s0 = pc.reduce(s0)
# we went past it -> don't continue
if old_reached is True and reached is False:
logger.info('stopped intersecting si')
#break
if reached is True:
break
if not pc.is_fulldim(s0):
return pc.Polytope()
s0 = pc.reduce(s0)
return s0
def prop2part(state_space, cont_props_dict):
"""Main function that takes a domain (state_space) and a list of
propositions (cont_props), and returns a proposition preserving
partition of the state space.
See Also
========
L{PropPreservingPartition},
C{polytope.Polytope}
@param state_space: problem domain
@type state_space: C{polytope.Polytope}
@param cont_props_dict: propositions
@type cont_props_dict: dict of C{polytope.Polytope}
@return: state space quotient partition induced by propositions
@rtype: L{PropPreservingPartition}
"""
first_poly = [] #Initial Region's polytopes
first_poly.append(state_space)
regions = [pc.Region(first_poly)]
for cur_prop in cont_props_dict:
cur_prop_poly = cont_props_dict[cur_prop]
num_reg = len(regions)
prop_holds_reg = []
for i in xrange(num_reg): #i region counter
region_now = regions[i].copy()
#loop for prop holds
prop_holds_reg.append(0)
prop_now = regions[i].props.copy()
dummy = region_now.intersect(cur_prop_poly)
# does cur_prop hold in dummy ?
if pc.is_fulldim(dummy):
dum_prop = prop_now.copy()
dum_prop.add(cur_prop)
# is dummy a Polytope ?
if len(dummy) == 0:
regions[i] = pc.Region([dummy], dum_prop)
else:
# dummy is a Region
dummy.props = dum_prop.copy()
regions[i] = dummy.copy()
prop_holds_reg[-1] = 1
else:
#does not hold in the whole region
# (-> no need for the 2nd loop)
regions.append(region_now)
continue
#loop for prop does not hold
regions.append(pc.Region([], props=prop_now))
dummy = region_now.diff(cur_prop_poly)
if pc.is_fulldim(dummy):
dum_prop = prop_now.copy()
# is dummy a Polytope ?
if len(dummy) == 0:
regions[-1] = pc.Region([pc.reduce(dummy)], dum_prop)
else:
# dummy is a Region
dummy.props = dum_prop.copy()
regions[-1] = dummy.copy()
else:
regions.pop()
count = 0
for hold_count in xrange(len(prop_holds_reg)):
if prop_holds_reg[hold_count] == 0:
regions.pop(hold_count - count)
count += 1
mypartition = PropPreservingPartition(
domain=copy.deepcopy(state_space),
regions=regions,
prop_regions=copy.deepcopy(cont_props_dict))
mypartition.adj = pc.find_adjacent_regions(mypartition).copy()
return mypartition
def pwa_shrunk_partition(pwa_sys, ppp, eps, abs_tol=1e-5):
"""This function takes:
- a piecewise affine system C{pwa_sys} and
- a proposition-preserving partition C{ppp}
whose domain is a subset of the domain of C{pwa_sys}
- a shrinkage factor C{eps}
and returns a *refined* proposition preserving partition
where in each region a unique subsystem of pwa_sys is active
and pwa_sys domains are shrunk to account for estimation
errors.
See Also
========
L{pwa_partition}
@type pwa_sys: L{hybrid.PwaSysDyn}
@type ppp: L{PropPreservingPartition}
@return: new partition and associated maps:
- new partition C{new_ppp}
- map of C{new_ppp.regions} to C{pwa_sys.list_subsys}
- map of C{new_ppp.regions} to C{ppp.regions}
@rtype: C{(L{PropPreservingPartition}, list, list)}
"""
new_list = []
subsys_list = []
parents = []
for i, subsys in enumerate(pwa_sys.list_subsys):
dom = shrinkPoly(subsys.domain, eps)
for j, region in enumerate(ppp.regions):
isect = pc.reduce(region.intersect(dom))
if pc.is_fulldim(isect):
rc, xc = pc.cheby_ball(isect)
if rc < abs_tol:
msg = 'One of the regions in the refined PPP is '
msg += 'too small, this may cause numerical problems'
warnings.warn(msg)
# not Region yet, but Polytope ?
if len(isect) == 0:
isect = pc.Region([isect])
# label with AP
isect.props = region.props.copy()
# store new Region
new_list.append(isect)
# keep track of original Region in ppp.regions
parents.append(j)
# index of subsystem active within isect
subsys_list.append(i)
# compute spatial adjacency matrix
n = len(new_list)
adj = sp.lil_matrix((n, n), dtype=np.int8)
for i, ri in enumerate(new_list):
pi = parents[i]
for j, rj in enumerate(new_list[0:i]):
pj = parents[j]
if (ppp.adj[pi, pj] == 1) or (pi == pj):
# account for shrinkage in adjacency check
if pc.is_adjacent(ri, rj, 2*eps):
adj[i, j] = 1
adj[j, i] = 1
adj[i, i] = 1
new_ppp = PropPreservingPartition(
domain = ppp.domain,
regions = new_list,
adj = adj,
prop_regions = ppp.prop_regions
)
return (new_ppp, subsys_list, parents)
def prop2part(state_space, cont_props_dict):
"""Main function that takes a domain (state_space) and a list of
propositions (cont_props), and returns a proposition preserving
partition of the state space.
See Also
========
L{PropPreservingPartition},
C{polytope.Polytope}
@param state_space: problem domain
@type state_space: C{polytope.Polytope}
@param cont_props_dict: propositions
@type cont_props_dict: dict of C{polytope.Polytope}
@return: state space quotient partition induced by propositions
@rtype: L{PropPreservingPartition}
"""
first_poly = [] #Initial Region's polytopes
first_poly.append(state_space)
regions = [pc.Region(first_poly)]
for cur_prop in cont_props_dict:
cur_prop_poly = cont_props_dict[cur_prop]
num_reg = len(regions)
prop_holds_reg = []
for i in xrange(num_reg): #i region counter
region_now = regions[i].copy()
#loop for prop holds
prop_holds_reg.append(0)
prop_now = regions[i].props.copy()
dummy = region_now.intersect(cur_prop_poly)
# does cur_prop hold in dummy ?
if pc.is_fulldim(dummy):
dum_prop = prop_now.copy()
dum_prop.add(cur_prop)
# is dummy a Polytope ?
if len(dummy) == 0:
regions[i] = pc.Region([dummy], dum_prop)
else:
# dummy is a Region
dummy.props = dum_prop.copy()
regions[i] = dummy.copy()
prop_holds_reg[-1] = 1
else:
#does not hold in the whole region
# (-> no need for the 2nd loop)
regions.append(region_now)
continue
#loop for prop does not hold
regions.append(pc.Region([], props=prop_now) )
dummy = region_now.diff(cur_prop_poly)
if pc.is_fulldim(dummy):
dum_prop = prop_now.copy()
# is dummy a Polytope ?
if len(dummy) == 0:
regions[-1] = pc.Region([pc.reduce(dummy)], dum_prop)
else:
# dummy is a Region
dummy.props = dum_prop.copy()
regions[-1] = dummy.copy()
else:
regions.pop()
count = 0
for hold_count in xrange(len(prop_holds_reg)):
if prop_holds_reg[hold_count]==0:
regions.pop(hold_count-count)
count+=1
mypartition = PropPreservingPartition(
domain = copy.deepcopy(state_space),
regions = regions,
prop_regions = copy.deepcopy(cont_props_dict)
)
mypartition.adj = pc.find_adjacent_regions(mypartition).copy()
return mypartition
def pwa_shrunk_partition(pwa_sys, ppp, eps, abs_tol=1e-5):
"""This function takes:
- a piecewise affine system C{pwa_sys} and
- a proposition-preserving partition C{ppp}
whose domain is a subset of the domain of C{pwa_sys}
- a shrinkage factor C{eps}
and returns a *refined* proposition preserving partition
where in each region a unique subsystem of pwa_sys is active
and pwa_sys domains are shrunk to account for estimation
errors.
See Also
========
L{pwa_partition}
@type pwa_sys: L{hybrid.PwaSysDyn}
@type ppp: L{PropPreservingPartition}
@return: new partition and associated maps:
- new partition C{new_ppp}
- map of C{new_ppp.regions} to C{pwa_sys.list_subsys}
- map of C{new_ppp.regions} to C{ppp.regions}
@rtype: C{(L{PropPreservingPartition}, list, list)}
"""
new_list = []
subsys_list = []
parents = []
for i, subsys in enumerate(pwa_sys.list_subsys):
dom = shrinkPoly(subsys.domain, eps)
for j, region in enumerate(ppp.regions):
isect = pc.reduce(region.intersect(dom))
if pc.is_fulldim(isect):
rc, xc = pc.cheby_ball(isect)
if rc < abs_tol:
msg = 'One of the regions in the refined PPP is '
msg += 'too small, this may cause numerical problems'
warnings.warn(msg)
# not Region yet, but Polytope ?
if len(isect) == 0:
isect = pc.Region([isect])
# label with AP
isect.props = region.props.copy()
# store new Region
new_list.append(isect)
# keep track of original Region in ppp.regions
parents.append(j)
# index of subsystem active within isect
subsys_list.append(i)
# compute spatial adjacency matrix
n = len(new_list)
adj = sp.lil_matrix((n, n), dtype=np.int8)
for i, ri in enumerate(new_list):
pi = parents[i]
for j, rj in enumerate(new_list[0:i]):
pj = parents[j]
if (ppp.adj[pi, pj] == 1) or (pi == pj):
# account for shrinkage in adjacency check
if pc.is_adjacent(ri, rj, 2 * eps):
adj[i, j] = 1
adj[j, i] = 1
adj[i, i] = 1
new_ppp = PropPreservingPartition(domain=ppp.domain,
regions=new_list,
adj=adj,
prop_regions=ppp.prop_regions)
return (new_ppp, subsys_list, parents)
