def plot_transition_arrow(polyreg0, polyreg1, ax, arr_size=None):
"""Plot arrow starting from polyreg0 and ending at polyreg1.
@type polyreg0: L{Polytope} or L{Region}
@type polyreg1: L{Polytope} or L{Region}
@param ax: axes where to plot
@return: arrow object
"""
# brevity
p0 = polyreg0
p1 = polyreg1
rc0, xc0 = cheby_ball(p0)
rc1, xc1 = cheby_ball(p1)
if np.sum(np.abs(xc1-xc0)) < 1e-7:
return None
if arr_size is None:
l,u = polyreg1.bounding_box
arr_size = (u[0,0]-l[0,0])/25.0
#TODO: 3d
x = xc0[0]
y = xc0[1]
dx = xc1[0] - xc0[0]
dy = xc1[1] - xc0[1]
arrow = mpl.patches.Arrow(
float(x), float(y), float(dx), float(dy),
width=arr_size, color='black'
)
ax.add_patch(arrow)
return arrow
def plot_transition_arrow(polyreg0, polyreg1, ax, arr_size=None):
"""Plot arrow starting from polyreg0 and ending at polyreg1.
@type polyreg0: L{Polytope} or L{Region}
@type polyreg1: L{Polytope} or L{Region}
@param ax: axes where to plot
@return: arrow object
"""
try:
from matplotlib import patches
except:
logger.error('failed to import matplotlib')
return
# brevity
p0 = polyreg0
p1 = polyreg1
rc0, xc0 = cheby_ball(p0)
rc1, xc1 = cheby_ball(p1)
if np.sum(np.abs(xc1-xc0)) < 1e-7:
return None
if arr_size is None:
l,u = polyreg1.bounding_box
arr_size = (u[0,0]-l[0,0])/25.0
#TODO: 3d
x = xc0[0]
y = xc0[1]
dx = xc1[0] - xc0[0]
dy = xc1[1] - xc0[1]
arrow = patches.Arrow(
float(x), float(y), float(dx), float(dy),
width=arr_size, color='black'
)
ax.add_patch(arrow)
return arrow
def plot_region(ax,
poly,
name,
prob,
color='red',
alpha=0.5,
hatch=False,
fill=True):
ax.add_patch(
patches.Polygon(pc.extreme(poly),
color=color,
alpha=alpha,
hatch=hatch,
fill=fill))
_, xc = pc.cheby_ball(poly)
ax.text(xc[0] - 0.4, xc[1] - 0.43,
'${}_{}$\n$p={}$'.format(name[0].upper(), name[1], prob))
def _get_patch(poly1, **kwargs):
"""Return matplotlib patch for given Polytope.
Example::
> # Plot Polytope objects poly1 and poly2 in the same plot
> import matplotlib.pyplot as plt
> fig = plt.figure()
> ax = fig.add_subplot(111)
> p1 = _get_patch(poly1, color="blue")
> p2 = _get_patch(poly2, color="yellow")
> ax.add_patch(p1)
> ax.add_patch(p2)
> ax.set_xlim(xl, xu) # Optional: set axis max/min
> ax.set_ylim(yl, yu)
> plt.show()
@type poly1: L{Polytope}
@param kwargs: any keyword arguments valid for
matplotlib.patches.Polygon
"""
import matplotlib as mpl
V = ptope.extreme(poly1)
if (V is not None):
rc, xc = ptope.cheby_ball(poly1)
x = V[:, 1] - xc[1]
y = V[:, 0] - xc[0]
mult = np.sqrt(x**2 + y**2)
x = x / mult
angle = np.arccos(x)
corr = np.ones(y.size) - 2 * (y < 0)
angle = angle * corr
ind = np.argsort(angle)
# create patch
patch = mpl.patches.Polygon(V[ind, :], True, **kwargs)
patch.set_zorder(0)
else:
patch = mpl.patches.Polygon([], True, **kwargs)
patch.set_zorder(0)
return patch
def add_grid(ppp, grid_size=None, num_grid_pnts=None, abs_tol=1e-10):
""" This function takes a proposition preserving partition ppp and the size
of the grid or the number of grids, and returns a refined proposition
preserving partition with grids.
Input:
- `ppp`: a L{PropPreservingPartition} object
- `grid_size`: the size of the grid,
type: float or list of float
- `num_grid_pnts`: the number of grids for each dimension,
type: integer or list of integer
Output:
- A L{PropPreservingPartition} object with grids
Note: There could be numerical instabilities when the continuous
propositions in ppp do not align well with the grid resulting in very small
regions. Performace significantly degrades without glpk.
"""
if (grid_size != None) & (num_grid_pnts != None):
raise Exception("add_grid: Only one of the grid size or number of \
grid points parameters is allowed to be given.")
if (grid_size == None) & (num_grid_pnts == None):
raise Exception("add_grid: At least one of the grid size or number of \
grid points parameters must be given.")
dim = len(ppp.domain.A[0])
domain_bb = ppp.domain.bounding_box
size_list = list()
if grid_size != None:
if isinstance(grid_size, list):
if len(grid_size) == dim:
size_list = grid_size
else:
raise Exception(
"add_grid: grid_size isn't given in a correct format.")
elif isinstance(grid_size, float):
for i in xrange(dim):
size_list.append(grid_size)
else:
raise Exception("add_grid: "
"grid_size isn't given in a correct format.")
else:
if isinstance(num_grid_pnts, list):
if len(num_grid_pnts) == dim:
for i in xrange(dim):
if isinstance(num_grid_pnts[i], int):
grid_size = (float(domain_bb[1][i]) -
float(domain_bb[0][i])) / num_grid_pnts[i]
size_list.append(grid_size)
else:
raise Exception(
"add_grid: "
"num_grid_pnts isn't given in a correct format.")
else:
raise Exception(
"add_grid: "
"num_grid_pnts isn't given in a correct format.")
elif isinstance(num_grid_pnts, int):
for i in xrange(dim):
grid_size = (float(domain_bb[1][i]) -
float(domain_bb[0][i])) / num_grid_pnts
size_list.append(grid_size)
else:
raise Exception("add_grid: "
"num_grid_pnts isn't given in a correct format.")
j = 0
list_grid = dict()
while j < dim:
list_grid[j] = compute_interval(float(domain_bb[0][j]),
float(domain_bb[1][j]), size_list[j],
abs_tol)
if j > 0:
if j == 1:
re_list = list_grid[j - 1]
re_list = product_interval(re_list, list_grid[j])
else:
re_list = product_interval(re_list, list_grid[j])
j += 1
new_list = []
parent = []
for i in xrange(len(re_list)):
temp_list = list()
j = 0
while j < dim * 2:
temp_list.append([re_list[i][j], re_list[i][j + 1]])
j = j + 2
for j in xrange(len(ppp.regions)):
tmp = pc.box2poly(temp_list)
isect = tmp.intersect(ppp.regions[j], abs_tol)
#if pc.is_fulldim(isect):
rc, xc = pc.cheby_ball(isect)
if rc > abs_tol / 2:
if rc < abs_tol:
print("Warning: "
"One of the regions in the refined PPP is too small"
", this may cause numerical problems")
if len(isect) == 0:
isect = pc.Region([isect], [])
isect.props = ppp.regions[j].props.copy()
new_list.append(isect)
parent.append(j)
adj = sp.lil_matrix((len(new_list), len(new_list)), dtype=np.int8)
for i in xrange(len(new_list)):
adj[i, i] = 1
for j in xrange(i + 1, len(new_list)):
if (ppp.adj[parent[i], parent[j]] == 1) or \
(parent[i] == parent[j]):
if pc.is_adjacent(new_list[i], new_list[j]):
adj[i, j] = 1
adj[j, i] = 1
return PropPreservingPartition(domain=ppp.domain,
regions=new_list,
adj=adj,
prop_regions=ppp.prop_regions)
def pwa_partition(pwa_sys, ppp, 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}
and returns a *refined* proposition preserving partition
where in each region a unique subsystem of pwa_sys is active.
Reference
=========
Modified from Petter Nilsson's code
implementing merge algorithm in:
Nilsson et al.
`Temporal Logic Control of Switched Affine Systems with an
Application in Fuel Balancing`, ACC 2012.
See Also
========
L{discretize}
@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)}
"""
if pc.is_fulldim(ppp.domain.diff(pwa_sys.domain)):
raise Exception('pwa system is not defined everywhere ' +
'in state space')
# for each subsystem's domain, cut it into pieces
# each piece is the intersection with
# a unique Region in ppp.regions
new_list = []
subsys_list = []
parents = []
for i, subsys in enumerate(pwa_sys.list_subsys):
for j, region in enumerate(ppp.regions):
isect = region.intersect(subsys.domain)
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):
if pc.is_adjacent(ri, rj):
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 get_input(x0,
ssys,
abstraction,
start,
end,
R=None,
r=None,
Q=None,
ord=1,
mid_weight=0.0,
solver=None):
"""Compute continuous control input for discrete transition.
Computes a continuous control input sequence
which takes the plant:
- from state C{start}
- to state C{end}
These are states of the partition C{abstraction}.
The computed control input is such that::
f(x, u) = |Rx|_{ord} + |Qu|_{ord} + r'x +
mid_weight * |xc - x(N)|_{ord}
be minimal.
C{xc} is the chebyshev center of the final cell.
If no cost parameters are given, then the defaults are:
- Q = I
- mid_weight = 3
Notes
=====
1. The same horizon length as in reachability analysis
should be used in order to guarantee feasibility.
2. If the closed loop algorithm has been used
to compute reachability the input needs to be
recalculated for each time step
(with decreasing horizon length).
In this case only u(0) should be used as
a control signal and u(1) ... u(N-1) discarded.
3. The "conservative" calculation makes sure that
the plant remains inside the convex hull of the
starting region during execution, i.e.::
x(1), x(2) ... x(N-1) are
\in conv_hull(starting region).
If the original proposition preserving partition
is not convex, then safety cannot be guaranteed.
@param x0: initial continuous state
@type x0: numpy 1darray
@param ssys: system dynamics
@type ssys: L{LtiSysDyn}
@param abstraction: abstract system dynamics
@type abstraction: L{AbstractPwa}
@param start: index of the initial state in C{abstraction.ts}
@type start: int >= 0
@param end: index of the end state in C{abstraction.ts}
@type end: int >= 0
@param R: state cost matrix for::
x = [x(1)' x(2)' .. x(N)']'
If empty, zero matrix is used.
@type R: size (N*xdim x N*xdim)
@param r: cost vector for state trajectory:
x = [x(1)' x(2)' .. x(N)']'
@type r: size (N*xdim x 1)
@param Q: input cost matrix for control input::
u = [u(0)' u(1)' .. u(N-1)']'
If empty, identity matrix is used.
@type Q: size (N*udim x N*udim)
@param mid_weight: cost weight for |x(N)-xc|_{ord}
@param ord: norm used for cost function::
f(x, u) = |Rx|_{ord} + |Qu|_{ord} + r'x +
mid_weight *|xc - x(N)|_{ord}
@type ord: ord \in {1, 2, np.inf}
@return: array A where row k contains the
control input: u(k)
for k = 0, 1 ... N-1
@rtype: (N x m) numpy 2darray
"""
part = abstraction.ppp
regions = part.regions
ofts = abstraction.ts
original_regions = abstraction.orig_ppp
orig = abstraction._ppp2orig
params = abstraction.disc_params
N = params['N'] # horizon length
conservative = params['conservative']
closed_loop = params['closed_loop']
if closed_loop:
logger.warning('`closed_loop = True` for controller computation. '
'This option is under development: use with caution.')
if (R is None and Q is None and r is None and mid_weight == 0):
# Default behavior
Q = np.eye(N * ssys.B.shape[1])
R = np.zeros([N * x0.size, N * x0.size])
r = np.zeros([N * x0.size, 1])
mid_weight = 3
if R is None:
R = np.zeros([N * x0.size, N * x0.size])
if Q is None:
Q = np.eye(N * ssys.B.shape[1])
if r is None:
r = np.zeros([N * x0.size, 1])
if (R.shape[0] != R.shape[1]) or (R.shape[0] != N * x0.size):
raise Exception("get_input: "
"R must be square and have side N * dim(state space)")
if (Q.shape[0] != Q.shape[1]) or (Q.shape[0] != N * ssys.B.shape[1]):
raise Exception("get_input: "
"Q must be square and have side N * dim(input space)")
if ofts is not None:
start_state = start
end_state = end
if end_state not in ofts.states.post(start_state):
raise Exception('get_input: '
'no transition from state s' + str(start) +
' to state s' + str(end))
else:
print("get_input: "
"Warning, no transition matrix found, assuming feasible")
if (not conservative) & (orig is None):
print("List of original proposition preserving "
"partitions not given, reverting to conservative mode")
conservative = True
P_start = regions[start]
P_end = regions[end]
n = ssys.A.shape[1]
m = ssys.B.shape[1]
idx = range((N - 1) * n, N * n)
if conservative:
# Take convex hull or P_start as constraint
if len(P_start) > 0:
if len(P_start) > 1:
# Take convex hull
vert = pc.extreme(P_start[0])
for i in range(1, len(P_start)):
vert = np.vstack([vert, pc.extreme(P_start[i])])
P1 = pc.qhull(vert)
else:
P1 = P_start[0]
else:
P1 = P_start
else:
# Take original proposition preserving cell as constraint
P1 = original_regions[orig[start]]
# must be single polytope (ensuring convex)
assert len(P1) > 0, P1
if len(P1) == 1:
P1 = P1[0]
else:
print(P1)
raise Exception('`conservative = False` arg requires '
'that original regions be convex')
if len(P_end) > 0:
low_cost = np.inf
low_u = np.zeros([N, m])
# for each polytope in target region
for P3 in P_end:
cost = np.inf
if mid_weight > 0:
rc, xc = pc.cheby_ball(P3)
R[np.ix_(range(n * (N - 1), n * N),
range(n * (N - 1), n * N))] += mid_weight * np.eye(n)
r[idx, 0] += -mid_weight * xc
try:
u, cost = get_input_helper(x0,
ssys,
P1,
P3,
N,
R,
r,
Q,
ord,
closed_loop=closed_loop,
solver=solver)
except _InputHelperLPException as ex:
# The end state might consist of several polytopes.
# For some of them there might not be a control action that
# brings the system there. In that case the matrix
# constructed by get_input_helper will be singular and the
# LP solver cannot return a solution.
# This is not a problem unless all polytopes in the end
# region are unreachable, in which case it seems likely that
# there is something wrong with the abstraction routine.
logger.info(repr(ex))
logger.info(
("Failed to find control action from continuous "
"state {x0} in discrete state {start} "
"to a target polytope in the discrete state {end}.\n"
"Target polytope:\n{P3}").format(x0=x0,
start=start,
end=end,
P3=P3))
r[idx, 0] += mid_weight * xc
if cost < low_cost:
low_u = u
low_cost = cost
if low_cost == np.inf:
raise Exception("get_input: Did not find any trajectory")
else:
P3 = P_end
if mid_weight > 0:
rc, xc = pc.cheby_ball(P3)
R[np.ix_(range(n * (N - 1), n * N),
range(n * (N - 1), n * N))] += mid_weight * np.eye(n)
r[idx, 0] += -mid_weight * xc
low_u, cost = get_input_helper(x0,
ssys,
P1,
P3,
N,
R,
r,
Q,
ord,
closed_loop=closed_loop,
solver=solver)
return low_u
def add_grid(ppp, grid_size=None, num_grid_pnts=None, abs_tol=1e-10):
""" This function takes a proposition preserving partition ppp and the size
of the grid or the number of grids, and returns a refined proposition
preserving partition with grids.
Input:
- `ppp`: a L{PropPreservingPartition} object
- `grid_size`: the size of the grid,
type: float or list of float
- `num_grid_pnts`: the number of grids for each dimension,
type: integer or list of integer
Output:
- A L{PropPreservingPartition} object with grids
Note: There could be numerical instabilities when the continuous
propositions in ppp do not align well with the grid resulting in very small
regions. Performace significantly degrades without glpk.
"""
if (grid_size!=None)&(num_grid_pnts!=None):
raise Exception("add_grid: Only one of the grid size or number of \
grid points parameters is allowed to be given.")
if (grid_size==None)&(num_grid_pnts==None):
raise Exception("add_grid: At least one of the grid size or number of \
grid points parameters must be given.")
dim=len(ppp.domain.A[0])
domain_bb = ppp.domain.bounding_box
size_list=list()
if grid_size!=None:
if isinstance( grid_size, list ):
if len(grid_size) == dim:
size_list=grid_size
else:
raise Exception(
"add_grid: grid_size isn't given in a correct format."
)
elif isinstance( grid_size, float ):
for i in xrange(dim):
size_list.append(grid_size)
else:
raise Exception("add_grid: "
"grid_size isn't given in a correct format.")
else:
if isinstance( num_grid_pnts, list ):
if len(num_grid_pnts) == dim:
for i in xrange(dim):
if isinstance( num_grid_pnts[i], int ):
grid_size=(
float(domain_bb[1][i]) -float(domain_bb[0][i])
) /num_grid_pnts[i]
size_list.append(grid_size)
else:
raise Exception("add_grid: "
"num_grid_pnts isn't given in a correct format.")
else:
raise Exception("add_grid: "
"num_grid_pnts isn't given in a correct format.")
elif isinstance( num_grid_pnts, int ):
for i in xrange(dim):
grid_size=(
float(domain_bb[1][i])-float(domain_bb[0][i])
) /num_grid_pnts
size_list.append(grid_size)
else:
raise Exception("add_grid: "
"num_grid_pnts isn't given in a correct format.")
j=0
list_grid=dict()
while j<dim:
list_grid[j] = compute_interval(
float(domain_bb[0][j]),
float(domain_bb[1][j]),
size_list[j],
abs_tol
)
if j>0:
if j==1:
re_list=list_grid[j-1]
re_list=product_interval(re_list, list_grid[j])
else:
re_list=product_interval(re_list, list_grid[j])
j+=1
new_list = []
parent = []
for i in xrange(len(re_list)):
temp_list=list()
j=0
while j<dim*2:
temp_list.append([re_list[i][j],re_list[i][j+1]])
j=j+2
for j in xrange(len(ppp.regions)):
tmp = pc.box2poly(temp_list)
isect = tmp.intersect(ppp.regions[j], abs_tol)
#if pc.is_fulldim(isect):
rc, xc = pc.cheby_ball(isect)
if rc > abs_tol/2:
if rc < abs_tol:
print("Warning: "
"One of the regions in the refined PPP is too small"
", this may cause numerical problems")
if len(isect) == 0:
isect = pc.Region([isect], [])
isect.props = ppp.regions[j].props.copy()
new_list.append(isect)
parent.append(j)
adj = sp.lil_matrix((len(new_list), len(new_list)), dtype=np.int8)
for i in xrange(len(new_list)):
adj[i,i] = 1
for j in xrange(i+1, len(new_list)):
if (ppp.adj[parent[i], parent[j]] == 1) or \
(parent[i] == parent[j]):
if pc.is_adjacent(new_list[i], new_list[j]):
adj[i,j] = 1
adj[j,i] = 1
return PropPreservingPartition(
domain = ppp.domain,
regions = new_list,
adj = adj,
prop_regions = ppp.prop_regions
)
def pwa_partition(pwa_sys, ppp, 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}
and returns a *refined* proposition preserving partition
where in each region a unique subsystem of pwa_sys is active.
Reference
=========
Modified from Petter Nilsson's code
implementing merge algorithm in:
Nilsson et al.
`Temporal Logic Control of Switched Affine Systems with an
Application in Fuel Balancing`, ACC 2012.
See Also
========
L{discretize}
@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)}
"""
if pc.is_fulldim(ppp.domain.diff(pwa_sys.domain) ):
raise Exception('pwa system is not defined everywhere ' +
'in state space')
# for each subsystem's domain, cut it into pieces
# each piece is the intersection with
# a unique Region in ppp.regions
new_list = []
subsys_list = []
parents = []
for i, subsys in enumerate(pwa_sys.list_subsys):
for j, region in enumerate(ppp.regions):
isect = region.intersect(subsys.domain)
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):
if pc.is_adjacent(ri, rj):
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 get_input(
x0, ssys, abstraction,
start, end,
R=None, r=None, Q=None,
ord=1, mid_weight=0.0
):
"""Compute continuous control input for discrete transition.
Computes a continuous control input sequence
which takes the plant:
- from state C{start}
- to state C{end}
These are states of the partition C{abstraction}.
The computed control input is such that::
f(x, u) = |Rx|_{ord} + |Qu|_{ord} + r'x +
mid_weight * |xc - x(N)|_{ord}
be minimal.
C{xc} is the chebyshev center of the final cell.
If no cost parameters are given, then the defaults are:
- Q = I
- mid_weight = 3
Notes
=====
1. The same horizon length as in reachability analysis
should be used in order to guarantee feasibility.
2. If the closed loop algorithm has been used
to compute reachability the input needs to be
recalculated for each time step
(with decreasing horizon length).
In this case only u(0) should be used as
a control signal and u(1) ... u(N-1) discarded.
3. The "conservative" calculation makes sure that
the plant remains inside the convex hull of the
starting region during execution, i.e.::
x(1), x(2) ... x(N-1) are
\in conv_hull(starting region).
If the original proposition preserving partition
is not convex, then safety cannot be guaranteed.
@param x0: initial continuous state
@type x0: numpy 1darray
@param ssys: system dynamics
@type ssys: L{LtiSysDyn}
@param abstraction: abstract system dynamics
@type abstraction: L{AbstractPwa}
@param start: index of the initial state in C{abstraction.ts}
@type start: int >= 0
@param end: index of the end state in C{abstraction.ts}
@type end: int >= 0
@param R: state cost matrix for::
x = [x(1)' x(2)' .. x(N)']'
If empty, zero matrix is used.
@type R: size (N*xdim x N*xdim)
@param r: cost vector for state trajectory:
x = [x(1)' x(2)' .. x(N)']'
@type r: size (N*xdim x 1)
@param Q: input cost matrix for control input::
u = [u(0)' u(1)' .. u(N-1)']'
If empty, identity matrix is used.
@type Q: size (N*udim x N*udim)
@param mid_weight: cost weight for |x(N)-xc|_{ord}
@param ord: norm used for cost function::
f(x, u) = |Rx|_{ord} + |Qu|_{ord} + r'x +
mid_weight *|xc - x(N)|_{ord}
@type ord: ord \in {1, 2, np.inf}
@return: array A where row k contains the
control input: u(k)
for k = 0, 1 ... N-1
@rtype: (N x m) numpy 2darray
"""
part = abstraction.ppp
regions = part.regions
ofts = abstraction.ts
original_regions = abstraction.orig_ppp
orig = abstraction._ppp2orig
params = abstraction.disc_params
N = params['N'] # horizon length
conservative = params['conservative']
closed_loop = params['closed_loop']
if closed_loop:
logger.warning(
'`closed_loop = True` for controller computation. '
'This option is under development: use with caution.')
if (
R is None and
Q is None and
r is None and
mid_weight == 0):
# Default behavior
Q = np.eye(N * ssys.B.shape[1])
R = np.zeros([N * x0.size, N * x0.size])
r = np.zeros([N * x0.size, 1])
mid_weight = 3
if R is None:
R = np.zeros([N * x0.size, N * x0.size])
if Q is None:
Q = np.eye(N * ssys.B.shape[1])
if r is None:
r = np.zeros([N * x0.size, 1])
if (R.shape[0] != R.shape[1]) or (R.shape[0] != N * x0.size):
raise Exception("get_input: "
"R must be square and have side N * dim(state space)")
if (Q.shape[0] != Q.shape[1]) or (Q.shape[0] != N * ssys.B.shape[1]):
raise Exception("get_input: "
"Q must be square and have side N * dim(input space)")
if ofts is not None:
start_state = start
end_state = end
if end_state not in ofts.states.post(start_state):
raise Exception('get_input: '
'no transition from state s' + str(start) +
' to state s' + str(end)
)
else:
print("get_input: "
"Warning, no transition matrix found, assuming feasible")
if (not conservative) & (orig is None):
print("List of original proposition preserving "
"partitions not given, reverting to conservative mode")
conservative = True
P_start = regions[start]
P_end = regions[end]
n = ssys.A.shape[1]
m = ssys.B.shape[1]
idx = range((N - 1) * n, N * n)
if conservative:
# Take convex hull or P_start as constraint
if len(P_start) > 0:
if len(P_start) > 1:
# Take convex hull
vert = pc.extreme(P_start[0])
for i in range(1, len(P_start)):
vert = np.vstack([
vert,
pc.extreme(P_start[i])
])
P1 = pc.qhull(vert)
else:
P1 = P_start[0]
else:
P1 = P_start
else:
# Take original proposition preserving cell as constraint
P1 = original_regions[orig[start]]
# must be single polytope (ensuring convex)
assert len(P1) > 0, P1
if len(P1) == 1:
P1 = P1[0]
else:
print(P1)
raise Exception(
'`conservative = False` arg requires '
'that original regions be convex')
if len(P_end) > 0:
low_cost = np.inf
low_u = np.zeros([N, m])
# for each polytope in target region
for P3 in P_end:
if mid_weight > 0:
rc, xc = pc.cheby_ball(P3)
R[
np.ix_(
range(n * (N - 1), n * N),
range(n * (N - 1), n * N)
)
] += mid_weight * np.eye(n)
r[idx, 0] += -mid_weight * xc
u, cost = get_input_helper(
x0, ssys, P1, P3, N, R, r, Q, ord,
closed_loop=closed_loop
)
r[idx, 0] += mid_weight * xc
if cost < low_cost:
low_u = u
low_cost = cost
if low_cost == np.inf:
raise Exception("get_input: Did not find any trajectory")
else:
P3 = P_end
if mid_weight > 0:
rc, xc = pc.cheby_ball(P3)
R[
np.ix_(
range(n * (N - 1), n * N),
range(n * (N - 1), n * N)
)
] += mid_weight * np.eye(n)
r[idx, 0] += -mid_weight * xc
low_u, cost = get_input_helper(
x0, ssys, P1, P3, N, R, r, Q, ord,
closed_loop=closed_loop
)
return low_u
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 get_input(
x0, ssys, abstraction,
start, end,
R=[], r=[], Q=[], mid_weight=0.0,
test_result=False
):
"""Compute continuous control input for discrete transition.
Computes a continuous control input sequence
which takes the plant:
- from state C{start}
- to state C{end}
These are states of the partition C{abstraction}.
The computed control input is such that::
f(x, u) = x'Rx +r'x +u'Qu +mid_weight *|xc-x(0)|_2
be minimal.
C{xc} is the chebyshev center of the final cell.
If no cost parameters are given, then the defaults are:
- Q = I
- mid_weight = 3
Notes
=====
1. The same horizon length as in reachability analysis
should be used in order to guarantee feasibility.
2. If the closed loop algorithm has been used
to compute reachability the input needs to be
recalculated for each time step
(with decreasing horizon length).
In this case only u(0) should be used as
a control signal and u(1) ... u(N-1) discarded.
3. The "conservative" calculation makes sure that
the plant remains inside the convex hull of the
starting region during execution, i.e.::
x(1), x(2) ... x(N-1) are
\in conv_hull(starting region).
If the original proposition preserving partition
is not convex, then safety cannot be guaranteed.
@param x0: initial continuous state
@type x0: numpy 1darray
@param ssys: system dynamics
@type ssys: L{LtiSysDyn}
@param abstraction: abstract system dynamics
@type abstraction: L{AbstractPwa}
@param start: index of the initial state in C{abstraction.ts}
@type start: int >= 0
@param end: index of the end state in C{abstraction.ts}
@type end: int >= 0
@param R: state cost matrix for::
x = [x(1)' x(2)' .. x(N)']'
If empty, zero matrix is used.
@type R: size (N*xdim x N*xdim)
@param r: cost vector for state trajectory:
x = [x(1)' x(2)' .. x(N)']'
@type r: size (N*xdim x 1)
@param Q: input cost matrix for control input::
u = [u(0)' u(1)' .. u(N-1)']'
If empty, identity matrix is used.
@type Q: size (N*udim x N*udim)
@param mid_weight: cost weight for |x(N)-xc|_2
@param test_result: performs a simulation
(without disturbance) to make sure that
the calculated input sequence is safe.
@type test_result: bool
@return: array A where row k contains the
control input: u(k)
for k = 0,1 ... N-1
@rtype: (N x m) numpy 2darray
"""
#@param N: horizon length
#@type N: int >= 1
#@param conservative:
# if True,
# then force plant to stay inside initial
# state during execution.
#
# Otherwise, plant is forced to stay inside
# the original proposition preserving cell.
#@type conservative: bool
#@param closed_loop: should be True
# if closed loop discretization has been used.
#@type closed_loop: bool
part = abstraction.ppp
regions = part.regions
ofts = abstraction.ts
original_regions = abstraction.orig_ppp
orig = abstraction._ppp2orig
params = abstraction.disc_params
N = params['N']
conservative = params['conservative']
closed_loop = params['closed_loop']
if (len(R) == 0) and (len(Q) == 0) and \
(len(r) == 0) and (mid_weight == 0):
# Default behavior
Q = np.eye(N*ssys.B.shape[1])
R = np.zeros([N*x0.size, N*x0.size])
r = np.zeros([N*x0.size,1])
mid_weight = 3
if len(R) == 0:
R = np.zeros([N*x0.size, N*x0.size])
if len(Q) == 0:
Q = np.eye(N*ssys.B.shape[1])
if len(r) == 0:
r = np.zeros([N*x0.size,1])
if (R.shape[0] != R.shape[1]) or (R.shape[0] != N*x0.size):
raise Exception("get_input: "
"R must be square and have side N * dim(state space)")
if (Q.shape[0] != Q.shape[1]) or (Q.shape[0] != N*ssys.B.shape[1]):
raise Exception("get_input: "
"Q must be square and have side N * dim(input space)")
if ofts is not None:
start_state = start
end_state = end
if end_state not in ofts.states.post(start_state):
raise Exception('get_input: '
'no transition from state s' +str(start) +
' to state s' +str(end)
)
else:
print("get_input: "
"Warning, no transition matrix found, assuming feasible")
if (not conservative) & (orig is None):
print("List of original proposition preserving "
"partitions not given, reverting to conservative mode")
conservative = True
P_start = regions[start]
P_end = regions[end]
n = ssys.A.shape[1]
m = ssys.B.shape[1]
idx = range((N-1)*n, N*n)
if conservative:
# Take convex hull or P_start as constraint
if len(P_start) > 0:
if len(P_start) > 1:
# Take convex hull
vert = pc.extreme(P_start[0])
for i in range(1, len(P_start)):
vert = np.vstack([
vert,
pc.extreme(P_start[i])
])
P1 = pc.qhull(vert)
else:
P1 = P_start[0]
else:
P1 = P_start
else:
# Take original proposition preserving cell as constraint
P1 = original_regions[orig[start]]
if len(P_end) > 0:
low_cost = np.inf
low_u = np.zeros([N,m])
# for each polytope in target region
for P3 in P_end:
if mid_weight > 0:
rc, xc = pc.cheby_ball(P3)
R[
np.ix_(
range(n*(N-1), n*N),
range(n*(N-1), n*N)
)
] += mid_weight*np.eye(n)
r[idx, :] += -mid_weight*xc
try:
u, cost = get_input_helper(
x0, ssys, P1, P3, N, R, r, Q,
closed_loop=closed_loop
)
r[idx, :] += mid_weight*xc
except:
r[idx, :] += mid_weight*xc
continue
if cost < low_cost:
low_u = u
low_cost = cost
if low_cost == np.inf:
raise Exception("get_input: Did not find any trajectory")
else:
P3 = P_end
if mid_weight > 0:
rc, xc = pc.cheby_ball(P3)
R[
np.ix_(
range(n*(N-1), n*N),
range(n*(N-1), n*N)
)
] += mid_weight*np.eye(n)
r[idx, :] += -mid_weight*xc
low_u, cost = get_input_helper(
x0, ssys, P1, P3, N, R, r, Q,
closed_loop=closed_loop
)
if test_result:
good = is_seq_inside(x0, low_u, ssys, P1, P3)
if not good:
print("Calculated sequence not good")
return low_u
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 discretize(
part, ssys, N=10, min_cell_volume=0.1,
closed_loop=True, conservative=False,
max_num_poly=5, use_all_horizon=False,
trans_length=1, remove_trans=False,
abs_tol=1e-7,
plotit=False, save_img=False, cont_props=None,
plot_every=1
):
"""Refine the partition and establish transitions
based on reachability analysis.
Reference
=========
U{[NOTM12]
<https://tulip-control.sourceforge.io/doc/bibliography.html#notm12>}
See Also
========
L{prop2partition.pwa_partition}, L{prop2partition.part2convex}
@param part: L{PropPreservingPartition} object
@param ssys: L{LtiSysDyn} or L{PwaSysDyn} object
@param N: horizon length
@param min_cell_volume: the minimum volume of cells in the resulting
partition.
@param closed_loop: boolean indicating whether the `closed loop`
algorithm should be used. default True.
@param conservative: if true, force sequence in reachability analysis
to stay inside starting cell. If false, safety
is ensured by keeping the sequence inside a convexified
version of the original proposition preserving cell.
@param max_num_poly: maximum number of polytopes in a region to use in
reachability analysis.
@param use_all_horizon: in closed loop algorithm: if we should look
for reachability also in less than N steps.
@param trans_length: the number of polytopes allowed to cross in a
transition. a value of 1 checks transitions
only between neighbors, a value of 2 checks
neighbors of neighbors and so on.
@param remove_trans: if True, remove found transitions between
non-neighbors.
@param abs_tol: maximum volume for an "empty" polytope
@param plotit: plot partitioning as it evolves
@type plotit: boolean,
default = False
@param save_img: save snapshots of partitioning to PDF files,
requires plotit=True
@type save_img: boolean,
default = False
@param cont_props: continuous propositions to plot
@type cont_props: list of C{Polytope}
@rtype: L{AbstractPwa}
"""
start_time = os.times()[0]
orig_ppp = part
min_cell_volume = (min_cell_volume /np.finfo(np.double).eps
*np.finfo(np.double).eps)
ispwa = isinstance(ssys, PwaSysDyn)
islti = isinstance(ssys, LtiSysDyn)
if ispwa:
(part, ppp2pwa, part2orig) = pwa_partition(ssys, part)
else:
part2orig = range(len(part))
# Save original polytopes, require them to be convex
if conservative:
orig_list = None
orig = [0]
else:
(part, new2old) = part2convex(part) # convexify
part2orig = [part2orig[i] for i in new2old]
# map new regions to pwa subsystems
if ispwa:
ppp2pwa = [ppp2pwa[i] for i in new2old]
remove_trans = False # already allowed in nonconservative
orig_list = []
for poly in part:
if len(poly) == 0:
orig_list.append(poly.copy())
elif len(poly) == 1:
orig_list.append(poly[0].copy())
else:
raise Exception("discretize: "
"problem in convexification")
orig = list(range(len(orig_list)))
# Cheby radius of disturbance set
# (defined within the loop for pwa systems)
if islti:
if len(ssys.E) > 0:
rd = ssys.Wset.chebR
else:
rd = 0.
# Initialize matrix for pairs to check
IJ = part.adj.copy()
IJ = IJ.todense()
IJ = np.array(IJ)
logger.debug("\n Starting IJ: \n" + str(IJ) )
# next line omitted in discretize_overlap
IJ = reachable_within(trans_length, IJ,
np.array(part.adj.todense()) )
# Initialize output
num_regions = len(part)
transitions = np.zeros(
[num_regions, num_regions],
dtype = int
)
sol = deepcopy(part.regions)
adj = part.adj.copy()
adj = adj.todense()
adj = np.array(adj)
# next 2 lines omitted in discretize_overlap
if ispwa:
subsys_list = list(ppp2pwa)
else:
subsys_list = None
ss = ssys
# init graphics
if plotit:
try:
import matplotlib.pyplot as plt
plt.ion()
fig, (ax1, ax2) = plt.subplots(1, 2)
ax1.axis('scaled')
ax2.axis('scaled')
file_extension = 'pdf'
except:
logger.error('failed to import matplotlib')
plt = None
else:
plt = None
iter_count = 0
# List of how many "new" regions
# have been created for each region
# and a list of original number of neighbors
#num_new_reg = np.zeros(len(orig_list))
#num_orig_neigh = np.sum(adj, axis=1).flatten() - 1
progress = list()
# Do the abstraction
while np.sum(IJ) > 0:
ind = np.nonzero(IJ)
# i,j swapped in discretize_overlap
i = ind[1][0]
j = ind[0][0]
IJ[j, i] = 0
si = sol[i]
sj = sol[j]
si_tmp = deepcopy(si)
sj_tmp = deepcopy(sj)
#num_new_reg[i] += 1
#print(num_new_reg)
if ispwa:
ss = ssys.list_subsys[subsys_list[i]]
if len(ss.E) > 0:
rd, xd = pc.cheby_ball(ss.Wset)
else:
rd = 0.
if conservative:
# Don't use trans_set
trans_set = None
else:
# Use original cell as trans_set
trans_set = orig_list[orig[i]]
S0 = solve_feasible(
si, sj, ss, N, closed_loop,
use_all_horizon, trans_set, max_num_poly
)
msg = '\n Working with partition cells: ' + str(i) + ', ' + str(j)
logger.info(msg)
msg = '\t' + str(i) +' (#polytopes = ' +str(len(si) ) +'), and:\n'
msg += '\t' + str(j) +' (#polytopes = ' +str(len(sj) ) +')\n'
if ispwa:
msg += '\t with active subsystem: '
msg += str(subsys_list[i]) + '\n'
msg += '\t Computed reachable set S0 with volume: '
msg += str(S0.volume) + '\n'
logger.debug(msg)
#logger.debug('si \cap s0')
isect = si.intersect(S0)
vol1 = isect.volume
risect, xi = pc.cheby_ball(isect)
#logger.debug('si \ s0')
diff = si.diff(S0)
vol2 = diff.volume
rdiff, xd = pc.cheby_ball(diff)
# if pc.is_fulldim(pc.Region([isect]).intersect(diff)):
# logging.getLogger('tulip.polytope').setLevel(logging.DEBUG)
# diff = pc.mldivide(si, S0, save=True)
#
# ax = S0.plot()
# ax.axis([0.0, 1.0, 0.0, 2.0])
# ax.figure.savefig('./img/s0.pdf')
#
# ax = si.plot()
# ax.axis([0.0, 1.0, 0.0, 2.0])
# ax.figure.savefig('./img/si.pdf')
#
# ax = isect.plot()
# ax.axis([0.0, 1.0, 0.0, 2.0])
# ax.figure.savefig('./img/isect.pdf')
#
# ax = diff.plot()
# ax.axis([0.0, 1.0, 0.0, 2.0])
# ax.figure.savefig('./img/diff.pdf')
#
# ax = isect.intersect(diff).plot()
# ax.axis([0.0, 1.0, 0.0, 2.0])
# ax.figure.savefig('./img/diff_cap_isect.pdf')
#
# logger.error('Intersection \cap Difference != \emptyset')
#
# assert(False)
if vol1 <= min_cell_volume:
logger.warning('\t too small: si \cap Pre(sj), '
'so discard intersection')
if vol1 <= min_cell_volume and isect:
logger.warning('\t discarded non-empty intersection: '
'consider reducing min_cell_volume')
if vol2 <= min_cell_volume:
logger.warning('\t too small: si \ Pre(sj), so not reached it')
# We don't want our partitions to be smaller than the disturbance set
# Could be a problem since cheby radius is calculated for smallest
# convex polytope, so if we have a region we might throw away a good
# cell.
if (vol1 > min_cell_volume) and (risect > rd) and \
(vol2 > min_cell_volume) and (rdiff > rd):
# Make sure new areas are Regions and add proposition lists
if len(isect) == 0:
isect = pc.Region([isect], si.props)
else:
isect.props = si.props.copy()
if len(diff) == 0:
diff = pc.Region([diff], si.props)
else:
diff.props = si.props.copy()
# replace si by intersection (single state)
isect_list = pc.separate(isect)
sol[i] = isect_list[0]
# cut difference into connected pieces
difflist = pc.separate(diff)
difflist += isect_list[1:]
n_isect = len(isect_list) -1
num_new = len(difflist)
# add each piece, as a new state
for region in difflist:
sol.append(region)
# keep track of PWA subsystems map to new states
if ispwa:
subsys_list.append(subsys_list[i])
n_cells = len(sol)
new_idx = range(n_cells-1, n_cells-num_new-1, -1)
"""Update transition matrix"""
transitions = np.pad(transitions, (0,num_new), 'constant')
transitions[i, :] = np.zeros(n_cells)
for r in new_idx:
#transitions[:, r] = transitions[:, i]
# All sets reachable from start are reachable from both part's
# except possibly the new part
transitions[i, r] = 0
transitions[j, r] = 0
# sol[j] is reachable from intersection of sol[i] and S0
if i != j:
transitions[j, i] = 1
# sol[j] is reachable from each piece os S0 \cap sol[i]
#for k in range(n_cells-n_isect-2, n_cells):
# transitions[j, k] = 1
"""Update adjacency matrix"""
old_adj = np.nonzero(adj[i, :])[0]
# reset new adjacencies
adj[i, :] = np.zeros([n_cells -num_new])
adj[:, i] = np.zeros([n_cells -num_new])
adj[i, i] = 1
adj = np.pad(adj, (0, num_new), 'constant')
for r in new_idx:
adj[i, r] = 1
adj[r, i] = 1
adj[r, r] = 1
if not conservative:
orig = np.hstack([orig, orig[i]])
# adjacencies between pieces of isect and diff
for r in new_idx:
for k in new_idx:
if r is k:
continue
if pc.is_adjacent(sol[r], sol[k]):
adj[r, k] = 1
adj[k, r] = 1
msg = ''
if logger.getEffectiveLevel() <= logging.DEBUG:
msg += '\t\n Adding states ' + str(i) + ' and '
for r in new_idx:
msg += str(r) + ' and '
msg += '\n'
logger.debug(msg)
for k in np.setdiff1d(old_adj, [i,n_cells-1]):
# Every "old" neighbor must be the neighbor
# of at least one of the new
if pc.is_adjacent(sol[i], sol[k]):
adj[i, k] = 1
adj[k, i] = 1
elif remove_trans and (trans_length == 1):
# Actively remove transitions between non-neighbors
transitions[i, k] = 0
transitions[k, i] = 0
for r in new_idx:
if pc.is_adjacent(sol[r], sol[k]):
adj[r, k] = 1
adj[k, r] = 1
elif remove_trans and (trans_length == 1):
# Actively remove transitions between non-neighbors
transitions[r, k] = 0
transitions[k, r] = 0
"""Update IJ matrix"""
IJ = np.pad(IJ, (0,num_new), 'constant')
adj_k = reachable_within(trans_length, adj, adj)
sym_adj_change(IJ, adj_k, transitions, i)
for r in new_idx:
sym_adj_change(IJ, adj_k, transitions, r)
if logger.getEffectiveLevel() <= logging.DEBUG:
msg = '\n\n Updated adj: \n' + str(adj)
msg += '\n\n Updated trans: \n' + str(transitions)
msg += '\n\n Updated IJ: \n' + str(IJ)
logger.debug(msg)
logger.info('Divided region: ' + str(i) + '\n')
elif vol2 < abs_tol:
logger.info('Found: ' + str(i) + ' ---> ' + str(j) + '\n')
transitions[j,i] = 1
else:
if logger.level <= logging.DEBUG:
msg = '\t Unreachable: ' + str(i) + ' --X--> ' + str(j) + '\n'
msg += '\t\t diff vol: ' + str(vol2) + '\n'
msg += '\t\t intersect vol: ' + str(vol1) + '\n'
logger.debug(msg)
else:
logger.info('\t unreachable\n')
transitions[j,i] = 0
# check to avoid overlapping Regions
if debug:
tmp_part = PropPreservingPartition(
domain=part.domain,
regions=sol, adj=sp.lil_matrix(adj),
prop_regions=part.prop_regions
)
assert(tmp_part.is_partition() )
n_cells = len(sol)
progress_ratio = 1 - float(np.sum(IJ) ) /n_cells**2
progress += [progress_ratio]
msg = '\t total # polytopes: ' + str(n_cells) + '\n'
msg += '\t progress ratio: ' + str(progress_ratio) + '\n'
logger.info(msg)
iter_count += 1
# no plotting ?
if not plotit:
continue
if plt is None or plot_partition is None:
continue
if iter_count % plot_every != 0:
continue
tmp_part = PropPreservingPartition(
domain=part.domain,
regions=sol, adj=sp.lil_matrix(adj),
prop_regions=part.prop_regions
)
# plot pair under reachability check
ax2.clear()
si_tmp.plot(ax=ax2, color='green')
sj_tmp.plot(ax2, color='red', hatch='o', alpha=0.5)
plot_transition_arrow(si_tmp, sj_tmp, ax2)
S0.plot(ax2, color='none', hatch='/', alpha=0.3)
fig.canvas.draw()
# plot partition
ax1.clear()
plot_partition(tmp_part, transitions.T, ax=ax1, color_seed=23)
# plot dynamics
ssys.plot(ax1, show_domain=False)
# plot hatched continuous propositions
part.plot_props(ax1)
fig.canvas.draw()
# scale view based on domain,
# not only the current polytopes si, sj
l,u = part.domain.bounding_box
ax2.set_xlim(l[0,0], u[0,0])
ax2.set_ylim(l[1,0], u[1,0])
if save_img:
fname = 'movie' +str(iter_count).zfill(3)
fname += '.' + file_extension
fig.savefig(fname, dpi=250)
plt.pause(1)
new_part = PropPreservingPartition(
domain=part.domain,
regions=sol, adj=sp.lil_matrix(adj),
prop_regions=part.prop_regions
)
# check completeness of adjacency matrix
if debug:
tmp_part = deepcopy(new_part)
tmp_part.compute_adj()
# Generate transition system and add transitions
ofts = trs.FTS()
adj = sp.lil_matrix(transitions.T)
n = adj.shape[0]
ofts_states = range(n)
ofts.states.add_from(ofts_states)
ofts.transitions.add_adj(adj, ofts_states)
# Decorate TS with state labels
atomic_propositions = set(part.prop_regions)
ofts.atomic_propositions.add_from(atomic_propositions)
for state, region in zip(ofts_states, sol):
state_prop = region.props.copy()
ofts.states.add(state, ap=state_prop)
param = {
'N':N,
'trans_length':trans_length,
'closed_loop':closed_loop,
'conservative':conservative,
'use_all_horizon':use_all_horizon,
'min_cell_volume':min_cell_volume,
'max_num_poly':max_num_poly
}
ppp2orig = [part2orig[x] for x in orig]
end_time = os.times()[0]
msg = 'Total abstraction time: ' +\
str(end_time - start_time) + '[sec]'
print(msg)
logger.info(msg)
if save_img and plt is not None:
fig, ax = plt.subplots(1, 1)
plt.plot(progress)
ax.set_xlabel('iteration')
ax.set_ylabel('progress ratio')
ax.figure.savefig('progress.pdf')
return AbstractPwa(
ppp=new_part,
ts=ofts,
ppp2ts=ofts_states,
pwa=ssys,
pwa_ppp=part,
ppp2pwa=orig,
ppp2sys=subsys_list,
orig_ppp=orig_ppp,
ppp2orig=ppp2orig,
disc_params=param
)
def merge_partition_pair(
old_regions, ab2,
cur_mode, prev_modes,
old_parents, old_ap_labeling
):
"""Merge an Abstraction with the current partition iterate.
@param old_regions: A list of C{Region} that is from either:
1. The ppp of the first (initial) L{AbstractPwa} to be merged.
2. A list of already-merged regions
@type old_regions: list of C{Region}
@param ab2: Abstracted piecewise affine dynamics to be merged into the
@type ab2: L{AbstractPwa}
@param cur_mode: mode to be merged
@type cur_mode: tuple
@param prev_modes: list of modes that have already been merged together
@type prev_modes: list of tuple
@param old_parents: dict of modes that have already been merged to dict of
indices of new regions to indices of regions
@type old_parents: dict of modes to list of region indices in list
C{old_regions} or dict of region indices to regions in original ppp for
that mode
@param old_ap_labeling: dict of states of already-merged modes to sets of
propositions for each state
@type old_ap_labeling: dict of tuples to sets
@return: the following:
- C{new_list}, list of new regions
- C{parents}, same as input param C{old_parents}, except that it
includes the mode that was just merged and for list of regions in
return value C{new_list}
- C{ap_labeling}, same as input param C{old_ap_labeling}, except that it
includes the mode that was just merged.
"""
logger.info('merging partitions')
part2 = ab2.ppp
modes = prev_modes + [cur_mode]
new_list = []
parents = {mode:dict() for mode in modes}
ap_labeling = dict()
for i in range(len(old_regions)):
for j in range(len(part2)):
isect = pc.intersect(old_regions[i],
part2[j])
rc, xc = pc.cheby_ball(isect)
# no intersection ?
if rc < 1e-5:
continue
logger.info('merging region: A' + str(i) +
', with: B' + str(j))
# if Polytope, make it Region
if len(isect) == 0:
isect = pc.Region([isect])
# label the Region with propositions
isect.props = old_regions[i].props.copy()
new_list.append(isect)
idx = new_list.index(isect)
# keep track of parents
for mode in prev_modes:
parents[mode][idx] = old_parents[mode][i]
parents[cur_mode][idx] = j
# union of AP labels from parent states
ap_label_1 = old_ap_labeling[i]
ap_label_2 = ab2.ts.states[j]['ap']
logger.debug('AP label 1: ' + str(ap_label_1))
logger.debug('AP label 2: ' + str(ap_label_2))
# original partitions may be different if pwa_partition used
# but must originate from same initial partition,
# i.e., have same continuous propositions, checked above
#
# so no two intersecting regions can have different AP labels,
# checked here
if ap_label_1 != ap_label_2:
msg = 'Inconsistent AP labels between intersecting regions\n'
msg += 'of partitions of switched system.'
raise Exception(msg)
ap_labeling[idx] = ap_label_1
return new_list, parents, ap_labeling
