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