コード例 #1
0
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
コード例 #2
0
ファイル: feasible.py プロジェクト: johnyf/tulip-control
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
コード例 #3
0
    def region_full_dim_test(self):
        assert not pc.is_fulldim(pc.Region())

        p1 = pc.Polytope(self.A, self.b)
        p2 = pc.Polytope(self.Ab2[:, 0:2], self.Ab2[:, 2])
        reg = pc.Region([p1, p2])
        assert pc.is_fulldim(reg)

        # Adding empty polytopes should not affect the
        # full-dimensional status of this region.
        reg.list_poly.append(pc.Polytope())
        assert pc.is_fulldim(reg)
        reg.list_poly.append(pc.Polytope(self.A, self.b - 1e3))
        assert pc.is_fulldim(reg)
コード例 #4
0
    def region_full_dim_test(self):
        assert not pc.is_fulldim(pc.Region())

        p1 = pc.Polytope(self.A, self.b)
        p2 = pc.Polytope(self.Ab2[:, 0:2], self.Ab2[:, 2])
        reg = pc.Region([p1, p2])
        assert pc.is_fulldim(reg)

        # Adding empty polytopes should not affect the
        # full-dimensional status of this region.
        reg.list_poly.append(pc.Polytope())
        assert pc.is_fulldim(reg)
        reg.list_poly.append(pc.Polytope(self.A, self.b - 1e3))
        assert pc.is_fulldim(reg)
コード例 #5
0
    def polytope_intersect_test(self):
        p1 = pc.Polytope(self.A, self.b)
        p2 = pc.Polytope(self.Ab2[:, 0:2], self.Ab2[:, 2])
        p3 = p1.intersect(p2)
        assert pc.is_fulldim(p1)
        assert pc.is_fulldim(p2)
        assert not pc.is_fulldim(p3)

        # p4 is the unit square with center at the origin.
        p4 = pc.Polytope(np.array([[1., 0.], [0., 1.], [-1., 0.], [0., -1.]]),
                         np.array([0.5, 0.5, 0.5, 0.5]))
        p5 = p2.intersect(p4)
        assert pc.is_fulldim(p4)
        assert pc.is_fulldim(p5)
コード例 #6
0
ファイル: feasible.py プロジェクト: johnyf/tulip-control
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 check_empty(poly, method='polytope-lp'):
    '''
    checks whether a polytope in (A,b) representation is empty 
    variety of methods 
    '''
    A, b = poly
    if method == 'polytope-fulldim':
        poly = pc.Polytope(A=A, b=b, normalize=False)
        empty = not pc.is_fulldim(poly)
    elif method == 'cvxpy':
        # this is 10x slower than polytope
        v = cvx.Variable(A.shape[1])
        prob = cvx.Problem(cvx.Minimize(cvx.norm(v)), [A@v <= b])
        try:
            prob.solve()
            empty = prob.status == "infeasible"
        except cvx.SolverError:
            empty = True
    elif method == 'polytope-lp':
        c = np.ones(A.shape[1])
        res = pc.solvers.lpsolve(c, A, b, solver='glpk') # 'mosek')
        empty = res['status'] != 0
    else:
        raise NotImplementedError('method {} not implemented'.format(method))

    return empty
コード例 #8
0
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
コード例 #9
0
    def polytope_intersect_test(self):
        p1 = pc.Polytope(self.A, self.b)
        p2 = pc.Polytope(self.Ab2[:, 0:2], self.Ab2[:, 2])
        p3 = p1.intersect(p2)
        assert pc.is_fulldim(p1)
        assert pc.is_fulldim(p2)
        assert not pc.is_fulldim(p3)

        # p4 is the unit square with center at the origin.
        p4 = pc.Polytope(np.array([[ 1.,  0.],
                                   [ 0.,  1.],
                                   [-1.,  0.],
                                   [ 0., -1.]]),
                         np.array([0.5, 0.5, 0.5, 0.5]))
        p5 = p2.intersect(p4)
        assert pc.is_fulldim(p4)
        assert pc.is_fulldim(p5)
コード例 #10
0
ファイル: hybrid.py プロジェクト: necozay/tulip-control
    def __init__(self,
                 list_subsys=[],
                 domain=None,
                 time_semantics=None,
                 timestep=None,
                 overwrite_time=True):
        """
        @type overwrite_time: bool
        @param overwrite_time: If true, then overwrites any time data in the
                               objects in C{list_subsys} with the data in 
                               C{time_semantics} and C{timestep} variables. 
                               Otherwise checks that the time data of the
                               objects in C{list_subsys} are consistent with 
                               C{time_semantics} and C{timestep}.
        """

        if domain is None:
            warn("Domain not given to PwaSysDyn()")

        if ((domain is not None) and (not (isinstance(domain, pc.Polytope)
                                           or isinstance(domain, pc.Region)))):
            raise Exception(
                "PwaSysDyn: `domain` has to be a Polytope or Region")

        if len(list_subsys) > 0:
            uncovered_dom = domain.copy()
            n = list_subsys[0].A.shape[1]  # State space dimension
            m = list_subsys[0].B.shape[1]  # Input space dimension
            p = list_subsys[0].E.shape[1]  # Disturbance space dimension
            for subsys in list_subsys:
                uncovered_dom = uncovered_dom.diff(subsys.domain)
                if (n != subsys.A.shape[1] or m != subsys.B.shape[1]
                        or p != subsys.E.shape[1]):
                    raise Exception("PwaSysDyn: state, input, disturbance " +
                                    "dimensions have to be the same for all " +
                                    "subsystems")
            if not pc.is_empty(uncovered_dom):
                raise Exception("PwaSysDyn: subdomains must cover the domain")
            for x in itertools.combinations(list_subsys, 2):
                if pc.is_fulldim(x[0].domain.intersect(x[1].domain)):
                    raise Exception(
                        "PwaSysDyn: subdomains have to be mutually" +
                        " exclusive")

        self.list_subsys = list_subsys
        self.domain = domain

        # Input time semantics
        _check_time_data(time_semantics, timestep)
        if overwrite_time:
            _push_time_data(self.list_subsys, time_semantics, timestep)
        else:
            _check_time_consistency(list_subsys, time_semantics, timestep)
        self.timestep = timestep
        self.time_semantics = time_semantics
コード例 #11
0
ファイル: hybrid.py プロジェクト: johnyf/tulip-control
    def __init__(self, list_subsys=[], domain=None, time_semantics=None,
                 timestep=None, overwrite_time=True):
        """
        @type overwrite_time: bool
        @param overwrite_time: If true, then overwrites any time data in the
                               objects in C{list_subsys} with the data in
                               C{time_semantics} and C{timestep} variables.
                               Otherwise checks that the time data of the
                               objects in C{list_subsys} are consistent with
                               C{time_semantics} and C{timestep}.
        """

        if domain is None:
            warn("Domain not given to PwaSysDyn()")

        if ((domain is not None) and
            (not (isinstance(domain, pc.Polytope) or
                isinstance(domain, pc.Region))
            )
        ):
            raise Exception("PwaSysDyn: `domain` has to be a Polytope or Region")

        if len(list_subsys) > 0:
            uncovered_dom = domain.copy()
            n = list_subsys[0].A.shape[1]  # State space dimension
            m = list_subsys[0].B.shape[1]  # Input space dimension
            p = list_subsys[0].E.shape[1]  # Disturbance space dimension
            for subsys in list_subsys:
                uncovered_dom = uncovered_dom.diff(subsys.domain)
                if (n!=subsys.A.shape[1] or m!=subsys.B.shape[1] or
                    p!=subsys.E.shape[1]):
                    raise Exception("PwaSysDyn: state, input, disturbance " +
                                    "dimensions have to be the same for all " +
                                     "subsystems")
            if not pc.is_empty(uncovered_dom):
                raise Exception("PwaSysDyn: subdomains must cover the domain")
            for x in itertools.combinations(list_subsys, 2):
                if pc.is_fulldim(x[0].domain.intersect(x[1].domain) ):
                    raise Exception("PwaSysDyn: subdomains have to be mutually"+
                        " exclusive")

        self.list_subsys = list_subsys
        self.domain = domain

        # Input time semantics
        _check_time_data(time_semantics, timestep)
        if overwrite_time:
            _push_time_data(self.list_subsys, time_semantics, timestep)
        else:
            _check_time_consistency(list_subsys, time_semantics, timestep)
        self.timestep = timestep
        self.time_semantics = time_semantics
def polyplot(poly, ax, color=None,
         hatch=None, alpha=1.0):
    if poly.dim != 2:
        raise Exception("Cannot plot polytopes of dimension larger than 2")
    if not pc.is_fulldim(poly):
        return None
    if color is None:
        color = np.random.rand(3)
    poly = _get_patch(
        poly, facecolor=color, hatch=hatch,
        alpha=alpha)
    ax.add_patch(poly)
    return ax
コード例 #13
0
 def plot(self, ax=None, color=None,
          hatch=None, alpha=1.0, linestyle='dashed', linewidth=3):
     if self.dim != 2:
         raise Exception("Cannot plot polytopes of dimension larger than 2")
     ax = _newax(ax)
     if not ptope.is_fulldim(self):
         print("Cannot plot empty polytope")
         return None
     if color is None:
         color = np.random.rand(3)
     poly = _get_patch(
         self, facecolor=color, hatch=hatch,
         alpha=alpha, linestyle=linestyle, linewidth=linewidth,
         edgecolor='black')
     ax.add_patch(poly)
     return ax
コード例 #14
0
ファイル: feasible.py プロジェクト: johnyf/tulip-control
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
コード例 #15
0
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
コード例 #16
0
ファイル: feasible.py プロジェクト: necozay/tulip-control
def createLM(ssys, N, list_P, Pk=None, PN=None, disturbance_ind=None):
    """Compute the components of the polytope::
    
        L [x(0)' u(0)' ... u(N-1)']' <= M
    
    which stacks the following constraints:
    
      - x(t+1) = A x(t) + B u(t) + E d(t)
      - [u(k); x(k)] \in ssys.Uset for all k
    
    If list_P is a C{Polytope}:

      - x(0) \in list_P if list_P
      - x(k) \in Pk for k= 1,2, .. N-1
      - x(N) \in PN
    
    If list_P is a list of polytopes:

      - x(k) \in list_P[k] for k= 0, 1 ... N
    
    The returned polytope describes the intersection of the polytopes
    for all possible inputs.

    @param ssys: system dynamics
    @type ssys: L{LtiSysDyn}
    
    @param N: horizon length
    
    @type list_P: list of Polytopes or C{Polytope}
    @type Pk: C{Polytope}
    @type PN: C{Polytope}
    
    @param disturbance_ind: list indicating which k's
        that disturbance should be taken into account.
        Default is [1,2, ... N]
    """
    if not isinstance(list_P, Iterable):
        list_P = [list_P] +(N-1) *[Pk] +[PN]
        
    if disturbance_ind is None:
        disturbance_ind = range(1,N+1)
    
    A = ssys.A
    B = ssys.B
    E = ssys.E
    K = ssys.K
    
    D = ssys.Wset
    PU = ssys.Uset

    n = A.shape[1]  # State space dimension
    m = B.shape[1]  # Input space dimension
    p = E.shape[1]  # Disturbance space dimension
    
    # non-zero disturbance matrix E ?
    if not np.all(E==0):
        if not pc.is_fulldim(D):
            E = np.zeros(K.shape)
    
    list_len = np.array([P.A.shape[0] for P in list_P])
    sumlen = np.sum(list_len)

    LUn = np.shape(PU.A)[0]
    
    Lk = np.zeros([sumlen, n+N*m])
    LU = np.zeros([LUn*N, n+N*m])
    
    Mk = np.zeros([sumlen, 1])
    MU = np.tile(PU.b.reshape(PU.b.size, 1), (N, 1))
  
    Gk = np.zeros([sumlen, p*N])
    GU = np.zeros([LUn*N, p*N])
    
    K_hat = np.tile(K, (N, 1))
    
    B_diag = B
    E_diag = E
    for i in xrange(N-1):
        B_diag = _block_diag2(B_diag, B)
        E_diag = _block_diag2(E_diag, E)
    
    A_n = np.eye(n)
    A_k = np.zeros([n, n*N])
    
    sum_vert = 0
    for i in xrange(N+1):
        Li = list_P[i]
        
        if not isinstance(Li, pc.Polytope):
            logger.warn('createLM: Li of type: ' +str(type(Li) ) )
        
        ######### FOR M #########
        idx = range(sum_vert, sum_vert + Li.A.shape[0])
        Mk[idx, :] = Li.b.reshape(Li.b.size,1) - \
                     Li.A.dot(A_k).dot(K_hat)
        
        ######### FOR G #########
        if i in disturbance_ind:
            idx = np.ix_(
                range(sum_vert, sum_vert + Li.A.shape[0]),
                range(Gk.shape[1])
            )
            Gk[idx] = Li.A.dot(A_k).dot(E_diag)
            
            if (PU.A.shape[1] == m+n) and (i < N):
                A_k_E_diag = A_k.dot(E_diag)
                d_mult = np.vstack([np.zeros([m, p*N]), A_k_E_diag])
                
                idx = np.ix_(range(LUn*i, LUn*(i+1)), range(p*N))
                GU[idx] = PU.A.dot(d_mult)
        
        ######### FOR L #########
        AB_line = np.hstack([A_n, A_k.dot(B_diag)])
        
        idx = np.ix_(
            range(sum_vert, sum_vert + Li.A.shape[0]),
            range(0,Lk.shape[1])
        )
        Lk[idx] = Li.A.dot(AB_line)
        
        if i >= N:
            continue
        
        if PU.A.shape[1] == m:
            idx = np.ix_(
                range(i*LUn, (i+1)*LUn),
                range(n + m*i, n + m*(i+1))
            )
            LU[idx] = PU.A
        elif PU.A.shape[1] == m+n:
            uk_line = np.zeros([m, n + m*N])
            
            idx = np.ix_(range(m), range(n+m*i, n+m*(i+1)))
            uk_line[idx] = np.eye(m)
            
            A_mult = np.vstack([uk_line, AB_line])
            
            b_mult = np.zeros([m+n, 1])
            b_mult[range(m, m+n), :] = A_k.dot(K_hat)
            
            idx = np.ix_(
                range(i*LUn, (i+1)*LUn),
                range(n+m*N)
            )
            LU[idx] = PU.A.dot(A_mult)
            
            MU[range(i*LUn, (i+1)*LUn), :] -= PU.A.dot(b_mult)
        
        ####### Iterate #########
        sum_vert += Li.A.shape[0]
        A_n = A.dot(A_n)
        A_k = A.dot(A_k)
        
        idx = np.ix_(range(n), range(i*n, (i+1)*n))
        A_k[idx] = np.eye(n)
                
    # Get disturbance sets
    if not np.all(Gk==0):  
        G = np.vstack([Gk, GU])
        D_hat = get_max_extreme(G, D, N)
    else:
        D_hat = np.zeros([sumlen + LUn*N, 1])

    # Put together matrices L, M
    L = np.vstack([Lk, LU])
    M = np.vstack([Mk, MU]) - D_hat
    
    msg = 'Computed S0 polytope: L x <= M, where:\n\t L = \n'
    msg += str(L) +'\n\t M = \n' + str(M) +'\n'
    logger.debug(msg)
    
    return L,M
コード例 #17
0
ファイル: feasible.py プロジェクト: necozay/tulip-control
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 plot_latent_space(H, leaves, nonnegative=False, ax=None, projection=[0,1], 
    color_by_top=0, title='Items in Latent Space', colorbar=True, figsize=None, arrowax=True):
    n_latent_features, n_items = H.shape
    cmap = discrete_cmap(n_items)

    if ax is None:
        fig = plt.figure(figsize=figsize)
        ax = plt.subplot(111)

    # plotting polytope regions
    for leaf in leaves:
        A, b = leaf.get_polytope()
        A = A[:,projection] if n_latent_features > 2 else A
        plot_poly = pc.Polytope(A=A, b=b, normalize=False)
        corresponding_rec = leaf.get_rec_list()
        if pc.is_fulldim(plot_poly): 
            polyplot(plot_poly, ax=ax, color=cmap(corresponding_rec[color_by_top]), alpha=0.5)

    # plotting items
    im = ax.scatter(H[projection[0],:], H[projection[1],:], c=np.arange(n_items), cmap=cmap, 
                edgecolors='black', marker='o', s=50)
    im.set_clim([0,n_items])

    

    ax.axis('equal')
    xlim, ylim = np.amax(np.abs(H[projection]), axis=1)
    xmax = 1.3*xlim; ymax = 1.3*ylim
    if nonnegative:
        xmin = -0.1*xlim; ymin = -0.1*xlim
        ax.set_xlim([xmin,xmax])
        ax.set_ylim([ymin,ymax])
    else:
        xmin = -xmax; ymin = -ymax
        ax.set_xlim([-xmax,xmax])
        ax.set_ylim([-ymax,ymax])
    ax.margins(x=0,y=0)
    ax.set_title(title)

    if arrowax:
        # removing the default axis on all sides:
        for side in ['bottom','right','top','left']:
            ax.spines[side].set_visible(False)
        # removing the axis ticks
        plt.xticks([]) # labels 
        plt.yticks([])
        ax.xaxis.set_ticks_position('none') # tick markers
        ax.yaxis.set_ticks_position('none')

        # get width and height of axes object to compute 
        # matching arrowhead length and width
        dps = fig.dpi_scale_trans.inverted()
        bbox = ax.get_window_extent().transformed(dps)
        width, height = bbox.width, bbox.height
         
        # manual arrowhead width and length
        hw = 1./20.*(ymax-ymin) 
        hl = 1./20.*(xmax-xmin)
        lw = 2. # axis line width
        ohg = 0.3 # arrow overhang
         
        # compute matching arrowhead length and width
        yhw = hw/(ymax-ymin)*(xmax-xmin)* height/width 
        yhl = hl/(xmax-xmin)*(ymax-ymin)* width/height
         
        # draw x and y axis
        ax.arrow(xmin, 0, xmax-xmin, 0., fc='k', ec='k', lw = lw, 
                 head_width=hw, head_length=hl, overhang = ohg, 
                 length_includes_head=False, clip_on = False) 
         
        ax.arrow(0, ymin, 0., ymax-ymin, fc='k', ec='k', lw = lw, 
                 head_width=yhw, head_length=yhl, overhang = ohg, 
                 length_includes_head= False, clip_on = False) 
         

    if colorbar: plt.colorbar(im, ax=ax)
    return ax
コード例 #19
0
 def polytope_full_dim_test(self):
     assert pc.is_fulldim(pc.Polytope(self.A, self.b))
     assert pc.is_fulldim(pc.Polytope(self.Ab2[:, 0:2], self.Ab2[:, 2]))
     assert not pc.is_fulldim(pc.Polytope())
     assert not pc.is_fulldim(pc.Polytope(self.A, self.b - 1e3))
コード例 #20
0
 def polytope_full_dim_test(self):
     assert pc.is_fulldim(pc.Polytope(self.A, self.b))
     assert pc.is_fulldim(pc.Polytope(self.Ab2[:, 0:2], self.Ab2[:, 2]))
     assert not pc.is_fulldim(pc.Polytope())
     assert not pc.is_fulldim(pc.Polytope(self.A, self.b - 1e3))
コード例 #21
0
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
コード例 #22
0
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)
コード例 #23
0
    def are_disjoint(self, check_all=False, fname=None):
        """Return True if all Regions are disjoint.

        Print:

            - the offending Regions and their
            - their intersection (mean) volume ratio
            - their difference (mean) volume ratio

        Optionally save numbered figures of:

            - offending Regions
            - their intersection
            - their difference

        @param check_all: don't return when first offending regions found,
            continue and check all pairs
        @type check_all: bool

        @param fname: path prefix where to save the debugging figures
            By default no figures are saved.
        @type fname: str
        """
        logger.info('checking if PPP is a partition.')

        l, u = self.set.bounding_box
        ok = True
        for i, region in enumerate(self.regions):
            for j, other in enumerate(self.regions[0:i]):
                if pc.is_fulldim(region.intersect(other)):
                    msg = 'PPP is not a partition, regions: '
                    msg += str(i) + ' and: ' + str(j)
                    msg += ' intersect each other.\n'
                    msg += 'Offending regions are:\n' + 10 * '-' + '\n'
                    msg += str(region) + 10 * '-' + '\n'
                    msg += str(other) + 10 * '-' + '\n'

                    isect = region.intersect(other)
                    diff = region.diff(other)

                    mean_volume = (region.volume + other.volume) / 2.0

                    overlap = 100 * isect.volume / mean_volume
                    non_overlap = 100 * diff.volume / mean_volume

                    msg += '|cap| = ' + str(overlap) + ' %\n'
                    msg += '|diff| = ' + str(non_overlap) + '\n'

                    logger.error(msg)

                    if fname:
                        print('saving')
                        fname1 = fname + 'region' + str(i) + '.pdf'
                        fname2 = fname + 'region' + str(j) + '.pdf'
                        fname3 = (
                            fname + 'isect_' + str(i) +
                            '_' + str(j) + '.pdf')
                        fname4 = (
                            fname + 'diff_' + str(i) +
                            '_' + str(j) + '.pdf')

                        _save_region_plot(region, fname1, l, u)
                        _save_region_plot(other, fname2, l, u)
                        _save_region_plot(isect, fname3, l, u)
                        _save_region_plot(diff, fname4, l, u)

                    ok = False
                    if not check_all:
                        break
        return ok
コード例 #24
0
ファイル: prop2partition.py プロジェクト: pettni/polytope
    def are_disjoint(self, check_all=False, fname=None):
        """Return True if all Regions are disjoint.

        Print:

            - the offending Regions and their
            - their intersection (mean) volume ratio
            - their difference (mean) volume ratio

        Optionally save numbered figures of:

            - offending Regions
            - their intersection
            - their difference

        @param check_all: don't return when first offending regions found,
            continue and check all pairs
        @type check_all: bool

        @param fname: path prefix where to save the debugging figures
            By default no figures are saved.
        @type fname: str
        """
        logger.info('checking if PPP is a partition.')

        l,u = self.set.bounding_box
        ok = True
        for i, region in enumerate(self.regions):
            for j, other in enumerate(self.regions[0:i]):
                if pc.is_fulldim(region.intersect(other) ):
                    msg = 'PPP is not a partition, regions: '
                    msg += str(i) + ' and: ' + str(j)
                    msg += ' intersect each other.\n'
                    msg += 'Offending regions are:\n' + 10*'-' + '\n'
                    msg += str(region) + 10*'-' + '\n'
                    msg += str(other) + 10*'-' + '\n'

                    isect = region.intersect(other)
                    diff = region.diff(other)

                    mean_volume = (region.volume + other.volume) /2.0

                    overlap = 100 * isect.volume / mean_volume
                    non_overlap = 100 * diff.volume / mean_volume

                    msg += '|cap| = ' + str(overlap) + ' %\n'
                    msg += '|diff| = ' + str(non_overlap) + '\n'

                    logger.error(msg)

                    if fname:
                        print('saving')
                        fname1 = fname + 'region' + str(i) + '.pdf'
                        fname2 = fname + 'region' + str(j) + '.pdf'
                        fname3 = fname + 'isect_' + str(i) + '_' + str(j) + '.pdf'
                        fname4 = fname + 'diff_' + str(i) + '_' + str(j) + '.pdf'

                        _save_region_plot(region, fname1, l, u)
                        _save_region_plot(other, fname2, l, u)
                        _save_region_plot(isect, fname3, l, u)
                        _save_region_plot(diff, fname4, l, u)

                    ok = False
                    if not check_all:
                        break
        return ok
コード例 #25
0
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)
コード例 #26
0
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
コード例 #27
0
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)
コード例 #28
0
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)