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