Exemple #1
0
def plot_trajectory(ppp, x0, u_seq, ssys,
                    ax=None, color_seed=None):
    """Plots a PropPreservingPartition and the trajectory generated by x0
    input sequence u_seq.

    See Also
    ========
    C{plot_partition}, plot

    @type ppp: L{PropPreservingPartition}
    @param x0: initial state
    @param u_seq: matrix where each row contains an input
    @param ssys: system dynamics
    @param color_seed: see C{plot_partition}
    @return: axis object
    """
    try:
        from tulip.graphics import newax
    except:
        logger.error('failed to import graphics.newax')
        return

    if ax is None:
        ax, fig = newax()

    plot_partition(plot_numbers=False, ax=ax, show=False)

    A = ssys.A
    B = ssys.B

    if ssys.K is not None:
        K = ssys.K
    else:
        K = np.zeros(x0.shape)

    x = x0.flatten()
    x_arr = x0
    for i in range(u_seq.shape[0]):
        x = (
            np.dot(A, x).flatten() +
            np.dot(B, u_seq[i, :] ).flatten() +
            K.flatten())
        x_arr = np.vstack([x_arr, x.flatten()])

    ax.plot(x_arr[:,0], x_arr[:,1], 'o-')

    return ax
Exemple #2
0
def plot_trajectory(ppp, x0, u_seq, ssys,
                    ax=None, color_seed=None):
    """Plots a PropPreservingPartition and the trajectory generated by x0
    input sequence u_seq.

    See Also
    ========
    C{plot_partition}, plot

    @type ppp: L{PropPreservingPartition}
    @param x0: initial state
    @param u_seq: matrix where each row contains an input
    @param ssys: system dynamics
    @param color_seed: see C{plot_partition}
    @return: axis object
    """
    try:
        from tulip.graphics import newax
    except:
        logger.error('failed to import graphics.newax')
        return
    
    if ax is None:
        ax, fig = newax()
    
    plot_partition(plot_numbers=False, ax=ax, show=False)
    
    A = ssys.A
    B = ssys.B
    
    if ssys.K is not None:
        K = ssys.K
    else:
        K = np.zeros(x0.shape)
    
    x = x0.flatten()
    x_arr = x0
    for i in range(u_seq.shape[0]):
        x = np.dot(A, x).flatten() +\
            np.dot(B, u_seq[i, :] ).flatten() +\
            K.flatten()
        x_arr = np.vstack([x_arr, x.flatten()])
    
    ax.plot(x_arr[:,0], x_arr[:,1], 'o-')
    
    return ax
 def plot(
     self, trans=None, ppp2trans=None, only_adjacent=False,
     ax=None, plot_numbers=True, color_seed=None
 ):
     """For details see C{polytope.plot.plot_partition}.
     """
     return plot_partition(
         self, trans, ppp2trans, only_adjacent,
         ax, plot_numbers, color_seed
     )
Exemple #4
0
 def plot(self,
          trans=None,
          ppp2trans=None,
          only_adjacent=False,
          ax=None,
          plot_numbers=True,
          color_seed=None):
     """For details see C{polytope.plot.plot_partition}.
     """
     return plot_partition(self, trans, ppp2trans, only_adjacent, ax,
                           plot_numbers, color_seed)
Exemple #5
0
def plot_strategy(ab, mealy):
    """Plot strategic transitions on PPP.
    
    Assumes that mealy is feasible for ab.
    
    @type ab: L{AbstractPwa} or L{AbstractSwitched}
    
    @type mealy: L{transys.MealyMachine}
    """
    proj_mealy = project_strategy_on_partition(ab.ppp, mealy)
    ax = plot_partition(ab.ppp, proj_mealy, color_seed=0)
    return ax
Exemple #6
0
def plot_strategy(ab, mealy):
    """Plot strategic transitions on PPP.
    
    Assumes that mealy is feasible for ab.
    
    @type ab: L{AbstractPwa} or L{AbstractSwitched}
    
    @type mealy: L{transys.MealyMachine}
    """
    proj_mealy = project_strategy_on_partition(ab.ppp, mealy)
    ax = plot_partition(ab.ppp, proj_mealy, color_seed=0)
    return ax
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
    )