dryvrcore.py 10.2 KB
Newer Older
navidmokh's avatar
navidmokh committed
1
2
3
"""
This file contains core functions used by DryVR
"""
4
5
from __future__ import print_function
import random
navidmokh's avatar
navidmokh committed
6
7

import networkx as nx
8
import numpy as np
9
import igraph
navidmokh's avatar
navidmokh committed
10

11

navidmokh's avatar
navidmokh committed
12
13
from src.common.constant import *
from src.common.io import writeReachTubeFile
14
from src.common.utils import randomPoint, calcDelta, calcCenterPoint, trimTraces
15
from src.discrepancy.Global_Disc import get_reachtube_segment
navidmokh's avatar
navidmokh committed
16

17
18

def build_graph(vertex, edge, guards, resets):
navidmokh's avatar
navidmokh committed
19
20
21
22
23
24
25
26
27
28
29
30
31
    """
    Build graph object using given parameters
    
    Args:
        vertex (list): list of vertex with mode name
        edge (list): list of edge that connects vertex
        guards (list): list of guard corresponding to each edge
        resets (list): list of reset corresponding to each edge

    Returns:
        graph object

    """
32
    g = igraph.Graph(directed=True)
navidmokh's avatar
navidmokh committed
33
34
35
36
37
38
39
    g.add_vertices(len(vertex))
    g.add_edges(edge)

    g.vs['label'] = vertex
    g.vs['name'] = vertex
    labels = []
    for i in range(len(guards)):
40
41
42
43
        cur_guard = guards[i]
        cur_reset = resets[i]
        if not cur_reset:
            labels.append(cur_guard)
navidmokh's avatar
navidmokh committed
44
        else:
45
            labels.append(cur_guard + '|' + cur_reset)
navidmokh's avatar
navidmokh committed
46
47
48
49
50

    g.es['label'] = labels
    g.es['guards'] = guards
    g.es['resets'] = resets

51
52
53
    # if PLOTGRAPH:
    #     graph = igraph.plot(g, GRAPHOUTPUT, margin=40)
    #     graph.save()
navidmokh's avatar
navidmokh committed
54
55
    return g

56
57

def build_rrt_graph(modes, traces, is_ipynb):
navidmokh's avatar
navidmokh committed
58
59
60
61
62
63
64
65
66
    """
    Build controller synthesis graph object using given modes and traces.
    Note this function is very different from buildGraph function.
    This is white-box transition graph learned from controller synthesis algorithm
    The reason to build it is to output the transition graph to file
    
    Args:
        modes (list): list of mode name
        traces (list): list of trace corresponding to each mode
67
        is_ipynb (bool): check if it's in Ipython notebook environment
navidmokh's avatar
navidmokh committed
68
69
70
71
72

    Returns:
        None

    """
73
    if is_ipynb:
navidmokh's avatar
navidmokh committed
74
75
        vertex = []
        # Build unique identifier for a vertex and mode name
76
77
        for idx, v in enumerate(modes):
            vertex.append(v + "," + str(idx))
navidmokh's avatar
navidmokh committed
78

79
80
        edge_list = []
        edge_label = {}
navidmokh's avatar
navidmokh committed
81
        for i in range(1, len(modes)):
82
83
84
85
            edge_list.append((vertex[i - 1], vertex[i]))
            lower = traces[i - 1][-2][0]
            upper = traces[i - 1][-1][0]
            edge_label[(vertex[i - 1], vertex[i])] = "[" + str(lower) + "," + str(upper) + "]"
navidmokh's avatar
navidmokh committed
86
87
88

        fig = plt.figure()
        ax = fig.add_subplot(111)
89
90
91
92
        nx_graph = nx.DiGraph()
        nx_graph.add_edges_from(edge_list)
        pos = nx.spring_layout(nx_graph)
        colors = ['green'] * len(nx_graph.nodes())
navidmokh's avatar
navidmokh committed
93
        fig.suptitle('transition graph', fontsize=10)
94
        nx.draw_networkx_labels(nx_graph, pos)
navidmokh's avatar
navidmokh committed
95
96
97
98
99
100
101
        options = {
            'node_color': colors,
            'node_size': 1000,
            'cmap': plt.get_cmap('jet'),
            'arrowstyle': '-|>',
            'arrowsize': 50,
        }
102
103
        nx.draw_networkx(nx_graph, pos, arrows=True, **options)
        nx.draw_networkx_edge_labels(nx_graph, pos, edge_labels=edge_label)
navidmokh's avatar
navidmokh committed
104
105
106
        fig.canvas.draw()

    else:
107
        g = igraph.Graph(directed=True)
navidmokh's avatar
navidmokh committed
108
109
110
        g.add_vertices(len(modes))
        edges = []
        for i in range(1, len(modes)):
111
            edges.append([i - 1, i])
navidmokh's avatar
navidmokh committed
112
113
114
115
116
117
118
        g.add_edges(edges)

        g.vs['label'] = modes
        g.vs['name'] = modes

        # Build guard
        guard = []
119
        for i in range(len(traces) - 1):
navidmokh's avatar
navidmokh committed
120
121
            lower = traces[i][-2][0]
            upper = traces[i][-1][0]
122
            guard.append("And(t>" + str(lower) + ", t<=" + str(upper) + ")")
navidmokh's avatar
navidmokh committed
123
        g.es['label'] = guard
124
        graph = igraph.plot(g, RRTGRAPHPOUTPUT, margin=40)
navidmokh's avatar
navidmokh committed
125
126
127
        graph.save()


128
def simulate(g, init_condition, time_horizon, guard, sim_func, reset, init_vertex, deterministic):
navidmokh's avatar
navidmokh committed
129
130
131
132
133
    """
    This function does a full hybrid simulation

    Args:
        g (obj): graph object
134
135
136
137
138
139
        init_condition (list): initial point
        time_horizon (float): time horizon to simulate
        guard (src.core.guard.Guard): list of guard string corresponding to each transition
        sim_func (function): simulation function
        reset (src.core.reset.Reset): list of reset corresponding to each transition
        init_vertex (int): initial vertex that simulation starts
navidmokh's avatar
navidmokh committed
140
141
142
143
144
145
146
147
        deterministic (bool) : enable or disable must transition

    Returns:
        A dictionary obj contains simulation result.
        Key is mode name and value is the simulation trace.

    """

148
149
150
151
152
    ret_val = igraph.defaultdict(list)
    # If you do not declare initialMode, then we will just use topological sort to find starting point
    if init_vertex == -1:
        computer_order = g.topological_sorting(mode=igraph.OUT)
        cur_vertex = computer_order[0]
navidmokh's avatar
navidmokh committed
153
    else:
154
155
156
        cur_vertex = init_vertex
    remain_time = time_horizon
    cur_time = 0
navidmokh's avatar
navidmokh committed
157

158
159
    # Plus 1 because we need to consider about time
    dimensions = len(init_condition) + 1
navidmokh's avatar
navidmokh committed
160

161
    sim_result = []
navidmokh's avatar
navidmokh committed
162
    # Avoid numeric error
163
    while remain_time > 0.01:
navidmokh's avatar
navidmokh committed
164
165

        if DEBUG:
166
167
168
            print(NEWLINE)
            print((cur_vertex, remain_time))
            print('Current State', g.vs[cur_vertex]['label'], remain_time)
navidmokh's avatar
navidmokh committed
169

170
        if init_condition is None:
navidmokh's avatar
navidmokh committed
171
172
173
            # Ideally this should not happen
            break

174
175
176
        cur_successors = g.successors(cur_vertex)
        transit_time = remain_time
        cur_label = g.vs[cur_vertex]['label']
navidmokh's avatar
navidmokh committed
177

178
        cur_sim_result = sim_func(cur_label, init_condition, transit_time)
179
        if isinstance(cur_sim_result, np.ndarray):
180
            cur_sim_result = cur_sim_result.tolist()
navidmokh's avatar
navidmokh committed
181

182
        if len(cur_successors) == 0:
navidmokh's avatar
navidmokh committed
183
            # Some model return numpy array, convert to list
184
185
186
            init_condition, truncated_result = guard.guard_sim_trace(
                cur_sim_result,
                ""
navidmokh's avatar
navidmokh committed
187
            )
188
            cur_successor = None
navidmokh's avatar
navidmokh committed
189
190
191
192

        else:
            # First find all possible transition
            # Second randomly pick a path and time to transit
193
194
195
196
197
198
199
200
201
            next_modes = []
            for cur_successor in cur_successors:
                edge_id = g.get_eid(cur_vertex, cur_successor)
                cur_guard_str = g.es[edge_id]['guards']
                cur_reset_str = g.es[edge_id]['resets']

                next_init, truncated_result = guard.guard_sim_trace(
                    cur_sim_result,
                    cur_guard_str
navidmokh's avatar
navidmokh committed
202
203
                )

204
                next_init = reset.reset_point(cur_reset_str, next_init)
navidmokh's avatar
navidmokh committed
205
                # If there is a transition
206
207
208
                if next_init:
                    next_modes.append((cur_successor, next_init, truncated_result))
            if next_modes:
navidmokh's avatar
navidmokh committed
209
                # It is a non-deterministic system, randomly choose next state to transit
210
211
                if not deterministic:
                    cur_successor, init_condition, truncated_result = random.choice(next_modes)
navidmokh's avatar
navidmokh committed
212
213
                # This is deterministic system, choose earliest transition
                else:
214
215
216
217
218
219
220
221
                    shortest_time = float('inf')
                    for s, i, t in next_modes:
                        cur_tube_time = t[-1][0]
                        if cur_tube_time < shortest_time:
                            cur_successor = s
                            init_condition = i
                            truncated_result = t
                            shortest_time = cur_tube_time
navidmokh's avatar
navidmokh committed
222
            else:
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
                cur_successor = None
                init_condition = None

        # Get real transit time from truncated result
        transit_time = truncated_result[-1][0]
        ret_val[cur_label] += truncated_result
        sim_result.append(cur_label)
        for simRow in truncated_result:
            simRow[0] += cur_time
            sim_result.append(simRow)

        remain_time -= transit_time
        print("transit time", transit_time, "remain time", remain_time)
        cur_time += transit_time
        cur_vertex = cur_successor

    writeReachTubeFile(sim_result, SIMRESULTOUTPUT)
    return ret_val


def calc_bloated_tube(
        mode_label,
        initial_set,
        time_horizon,
        sim_func,
        bloating_method,
        kvalue,
        sim_trace_num,
        guard_checker=None,
        guard_str=""):
navidmokh's avatar
navidmokh committed
253
254
255
256
    """
    This function calculate the reach tube for single given mode

    Args:
257
258
259
260
261
262
        mode_label (str): mode name
        initial_set (list): a list contains upper and lower bound of the initial set
        time_horizon (float): time horizon to simulate
        sim_func (function): simulation function
        bloating_method (str): determine the bloating method for reach tube, either GLOBAL or PW
        sim_trace_num (int): number of simulations used to calculate the discrepancy
navidmokh's avatar
navidmokh committed
263
        kvalue (list): list of float used when bloating method set to PW
264
265
        guard_checker (src.core.guard.Guard or None): guard check object
        guard_str (str): guard string
navidmokh's avatar
navidmokh committed
266
267
268
269
270
       
    Returns:
        Bloated reach tube

    """
li213's avatar
li213 committed
271
    random.seed(4)
272
273
274
    cur_center = calcCenterPoint(initial_set[0], initial_set[1])
    cur_delta = calcDelta(initial_set[0], initial_set[1])
    traces = [sim_func(mode_label, cur_center, time_horizon)]
navidmokh's avatar
navidmokh committed
275
    # Simulate SIMTRACENUM times to learn the sensitivity
276
277
278
    for _ in range(sim_trace_num):
        new_init_point = randomPoint(initial_set[0], initial_set[1])
        traces.append(sim_func(mode_label, new_init_point, time_horizon))
navidmokh's avatar
navidmokh committed
279
280
281

    # Trim the trace to the same length
    traces = trimTraces(traces)
282
283
284
    if guard_checker is not None:
        # pre truncated traces to get better bloat result
        max_idx = -1
navidmokh's avatar
navidmokh committed
285
        for trace in traces:
286
287
            ret_idx = guard_checker.guard_sim_trace_time(trace, guard_str)
            max_idx = max(max_idx, ret_idx + 1)
navidmokh's avatar
navidmokh committed
288
        for i in range(len(traces)):
289
            traces[i] = traces[i][:max_idx]
navidmokh's avatar
navidmokh committed
290

291
    if bloating_method == GLOBAL:
292
        cur_reach_tube: np.ndarray = get_reachtube_segment(np.array(traces), np.array(cur_delta), "PWGlobal")
293
    elif bloating_method == PW:
294
        cur_reach_tube: np.ndarray = get_reachtube_segment(np.array(traces), np.array(cur_delta), "PW")
295
296
    else:
        raise ValueError("Unsupported bloating method '" + bloating_method + "'")
297
298
299
300
    final_tube = np.zeros((cur_reach_tube.shape[0]*2, cur_reach_tube.shape[2]))
    final_tube[0::2, :] = cur_reach_tube[:, 0, :]
    final_tube[1::2, :] = cur_reach_tube[:, 1, :]
    return final_tube.tolist()