Exemple #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 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 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)
Exemple #3
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 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 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)
Exemple #5
0
    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
Exemple #6
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 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
Exemple #7
0
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
Exemple #8
0
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_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 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))
Exemple #12
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
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)