Skip to content

Commit 515f3b6

Browse files
AlexYFMAlexYFM
committed
fixing some incorrect fixed_points calls
1 parent 7c0e31b commit 515f3b6

File tree

4 files changed

+174
-15
lines changed

4 files changed

+174
-15
lines changed

‎demo/fixed_points/cardiac_cell_demo.py

+3-3
Original file line numberDiff line numberDiff line change
@@ -32,10 +32,10 @@
3232
'cell', init_cell,(CellMode.On,)
3333
)
3434

35-
trace = scenario.verify(4, 0.01)
35+
trace = scenario.verify(10, 0.01)
3636

37-
pp_fix(reach_at_fix(trace, 0, 4))
38-
print(f'Fixed points exists? {fixed_points_fix(trace)}')
37+
pp_fix(reach_at_fix(trace, 0, 10))
38+
print(f'Fixed points exists? {fixed_points_fix(trace, 15, 0.01)}')
3939

4040
fig = go.Figure()
4141
fig = reachtube_tree(trace, None, fig, 0, 1, [0, 1], "fill", "trace")

‎demo/fixed_points/fixed_points.py

+4-1
Original file line numberDiff line numberDiff line change
@@ -128,6 +128,8 @@ def contain_all_fix(reach1: Dict[int, List[List[float]]], reach2: Dict[int, List
128128
s = Solver()
129129
# print(reach1, reach2[5][3], reach2[5][5], reach2[6][3], reach2[6][5])
130130
for node in reach1: # for each node
131+
if reach1[node] is None:
132+
continue
131133
for i in range(0, len(reach1[node]), 2): # for each vertex in the node
132134
includes = And()
133135
hr = [reach1[node][i], reach1[node][i+1]] # clarifies expressions and matches logic with contain_all
@@ -136,6 +138,8 @@ def contain_all_fix(reach1: Dict[int, List[List[float]]], reach2: Dict[int, List
136138
in_r1 = Or(in_r1, includes)
137139

138140
for node in reach2: #same as above loop except for r2 -- should probably make into subroutine
141+
if reach2[node] is None: ### temp solution, reachset contained a node with nothing in it
142+
continue
139143
for i in range(0, len(reach2[node]), 2): # for each vertex in the node
140144
includes = And()
141145
hr = [reach2[node][i], reach2[node][i+1]] # clarifies expressions and matches logic with contain_all
@@ -179,7 +183,6 @@ def contain_all(reach1: Dict[str, Dict[str, List[List[np.array]]]], reach2: Dict
179183
def fixed_points_fix(tree: AnalysisTree, T: float = 40, t_step: float = 0.01) -> bool:
180184
reach_end = reach_at_fix(tree)
181185
reach_else = reach_at_fix(tree, 0, T-t_step+t_step*.01) # need to add some offset because of some potential decimical diffs
182-
# print(reach_end, reach_else)
183186
return contain_all_fix(reach_end, reach_else)
184187

185188
### assuming user has T and t_step from previous scenario definition

‎demo/fixed_points/helicopter.py

+155
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,155 @@
1+
2+
from typing import Tuple, List
3+
4+
import numpy as np
5+
from scipy.integrate import ode
6+
7+
from verse import BaseAgent, Scenario, ScenarioConfig
8+
from verse.analysis.utils import wrap_to_pi
9+
from verse.analysis.analysis_tree import TraceType, AnalysisTree
10+
from verse.parser import ControllerIR
11+
from verse.analysis import AnalysisTreeNode, AnalysisTree, AnalysisTreeNodeType
12+
import copy
13+
14+
from enum import Enum, auto
15+
16+
from verse.plotter.plotter2D import *
17+
from verse.plotter.plotter3D_new import *
18+
import plotly.graph_objects as go
19+
20+
from fixed_points import fixed_points_fix, pp_fix, reach_at_fix
21+
### full disclosure, structure of file from mp4_p2
22+
23+
# <mode id="0" initial="True" name="Mode0">
24+
# <dai equation="p_dot= -476.851246128715*p**2 + 563.63999719734*p - 65.460328416" />
25+
# <dai equation="lam_dot= -4*lam - 33.3965838336589*p**2 + 99.359272121109*p + 33.8518550719433*pe**2 - 100.713764517065*pe - 4*(-5.05643107459819*p**2 + 15.0435539636327*p - 5.244048)*(-0.240071819537891*pe**2 + 0.714245545738554*pe - 0.248979591836735) + 4*(-4.97822527866553*pe**2 + 14.8108813346529*pe - 5.16294040816327)*(-0.240071819537891*pe**2 + 0.714245545738554*pe - 0.248979591836735) + 56.0441639020408" />
26+
# <dai equation="pe_dot= -478.163885472*p**2 + 567.545273568*p + 1.45848815920571*pe**2 - 4.33919596739959*pe - 65.309067936" />
27+
# <dai equation="ivalue_dot= 0" />
28+
# <dai equation="t_dot= 1" />
29+
# <dai equation="p_out= p" />
30+
# <dai equation="lam_out= lam" />
31+
# <dai equation="pe_out= pe" />
32+
# <dai equation="ivalue_out= ivalue" />
33+
# <dai equation="t_out= t" />
34+
# <invariant equation="t&lt;9.5" />
35+
# </mode>
36+
# <mode id="1" initial="False" name="Mode1">
37+
# <dai equation="p_dot= -476.851246128715*p**2 + 563.63999719734*p - 66.685538304" />
38+
# <dai equation="lam_dot= -4*lam - 33.3965838336589*p**2 + 99.359272121109*p + 82.9456*(0.0680272108843537*ivalue + 0.00272108843537415*lam + 0.0280272108843537)**2*(-3.529055747207*pe**2 + 10.4994095223567*pe - 0.366)**2 - 4*(0.0680272108843537*ivalue + 0.00272108843537415*lam + 0.0280272108843537)*(-5.05643107459819*p**2 + 15.0435539636327*p - 0.5244048)*(-3.529055747207*pe**2 + 10.4994095223567*pe - 0.366) - 141.0072*(0.0680272108843537*ivalue + 0.00272108843537415*lam + 0.0280272108843537)*(-3.529055747207*pe**2 + 10.4994095223567*pe - 0.366) + 52.10842488" />
39+
# <dai equation="pe_dot= -478.163885472*p**2 + 567.545273568*p + 1.45848815920571*pe**2 - 4.33919596739959*pe - 66.670412256" />
40+
# <dai equation="ivalue_dot= 0.14*lam - 2.058" />
41+
# <dai equation="t_dot= 1" />
42+
# <dai equation="p_out= p" />
43+
# <dai equation="lam_out= lam" />
44+
# <dai equation="pe_out= pe" />
45+
# <dai equation="ivalue_out= ivalue" />
46+
# <dai equation="t_out= t" />
47+
# </mode>
48+
# <transition destination="1" id="1" source="0">
49+
# <guard equation="t&gt;=9.5" />
50+
# <action equation="t = 0" />
51+
# </transition>
52+
# </automaton>
53+
# <composition automata="default_automaton" />
54+
# <property initialSet="Mode0:p==0.6353&amp;&amp;lam==14.7&amp;&amp;pe==0.5573&amp;&amp;ivalue==0.017&amp;&amp;t==0" name="Prop1" type="Safety" unsafeSet="lam&gt;=100">
55+
# <parameters kvalue="4000.0" timehorizon="15.0" timestep="0.001" />
56+
57+
class PowerTrainAgent(BaseAgent):
58+
def __init__(
59+
self,
60+
id,
61+
code = None,
62+
file_name = None
63+
):
64+
super().__init__(id, code, file_name)
65+
66+
@staticmethod
67+
def dynamic_mode0(t, state):
68+
p, lam, pe, ivalue, t_int = state
69+
p_dot = -476.851246128715*p**2 + 563.63999719734*p - 65.460328416
70+
lam_dot= -4*lam - 33.3965838336589*p**2 + 99.359272121109*p + 33.8518550719433*pe**2 - 100.713764517065*pe - 4*(-5.05643107459819*p**2 + 15.0435539636327*p - 5.244048)*(-0.240071819537891*pe**2 + 0.714245545738554*pe - 0.248979591836735) + 4*(-4.97822527866553*pe**2 + 14.8108813346529*pe - 5.16294040816327)*(-0.240071819537891*pe**2 + 0.714245545738554*pe - 0.248979591836735) + 56.0441639020408
71+
pe_dot= -478.163885472*p**2 + 567.545273568*p + 1.45848815920571*pe**2 - 4.33919596739959*pe - 65.309067936
72+
ivalue_dot = 0
73+
t_dot = 1
74+
return [p_dot, lam_dot, pe_dot, ivalue_dot, t_dot]
75+
76+
@staticmethod
77+
def dynamic_mode1(t, state):
78+
p, lam, pe, ivalue, t_int = state
79+
p_dot= -476.851246128715*p**2 + 563.63999719734*p - 66.685538304
80+
lam_dot= -4*lam - 33.3965838336589*p**2 + 99.359272121109*p + 82.9456*(0.0680272108843537*ivalue + 0.00272108843537415*lam + 0.0280272108843537)**2*(-3.529055747207*pe**2 + 10.4994095223567*pe - 0.366)**2 - 4*(0.0680272108843537*ivalue + 0.00272108843537415*lam + 0.0280272108843537)*(-5.05643107459819*p**2 + 15.0435539636327*p - 0.5244048)*(-3.529055747207*pe**2 + 10.4994095223567*pe - 0.366) - 141.0072*(0.0680272108843537*ivalue + 0.00272108843537415*lam + 0.0280272108843537)*(-3.529055747207*pe**2 + 10.4994095223567*pe - 0.366) + 52.10842488
81+
pe_dot= -478.163885472*p**2 + 567.545273568*p + 1.45848815920571*pe**2 - 4.33919596739959*pe - 66.670412256
82+
ivalue_dot= 0.14*lam - 2.058
83+
t_dot = 1
84+
return [p_dot, lam_dot, pe_dot, ivalue_dot, t_dot]
85+
86+
87+
def TC_simulate(
88+
self, mode: List[str], init, time_bound, time_step, lane_map = None
89+
) -> TraceType:
90+
time_bound = float(time_bound)
91+
num_points = int(np.ceil(time_bound / time_step))
92+
trace = np.zeros((num_points + 1, 1 + len(init)))
93+
trace[1:, 0] = [round(i * time_step, 10) for i in range(num_points)]
94+
trace[0, 1:] = init
95+
for i in range(num_points):
96+
if mode[0]=="On":
97+
r = ode(self.dynamic_mode0)
98+
elif mode[0]=="Off":
99+
r = ode(self.dynamic_mode1)
100+
else:
101+
raise ValueError
102+
r.set_initial_value(init)
103+
res: np.ndarray = r.integrate(r.t + time_step)
104+
init = res.flatten()
105+
trace[i + 1, 0] = time_step * (i + 1)
106+
trace[i + 1, 1:] = init
107+
return trace
108+
109+
110+
111+
class HelicopterMode(Enum):
112+
Normal=auto()
113+
114+
class State:
115+
x1, x2, x3, x4, x5, x6, x7, x8, x9, x10, x11, x12, x13, x14, x15, x16, x17, x18, x19, x20, x21, x22, x23, x24, x25: float
116+
x26: float
117+
x27: float
118+
x28: float
119+
120+
def __init__(self, p, lam, pe, ivalue, t_int, agent_mode: PTMode):
121+
pass
122+
123+
def decisionLogic(ego: State, other: State):
124+
output = copy.deepcopy(ego)
125+
126+
return output
127+
128+
129+
130+
if __name__ == "__main__":
131+
import os
132+
script_dir = os.path.realpath(os.path.dirname(__file__))
133+
input_code_name = os.path.join(script_dir, "power_train.py")
134+
PT = PowerTrainAgent('PT', file_name=input_code_name)
135+
136+
scenario = Scenario(ScenarioConfig(init_seg_length=1, parallel=False))
137+
138+
scenario.add_agent(PT) ### need to add breakpoint around here to check decision_logic of agents
139+
140+
init_PT = [[0.6353, 14.7, 0.5573, 0.017, 0],[0.6353, 14.7, 0.5573, 0.017, 0]]
141+
# # -----------------------------------------
142+
143+
scenario.set_init_single(
144+
'PT', init_PT,(PTMode.Mode0,)
145+
)
146+
147+
trace = scenario.verify(15, 0.001)
148+
149+
pp_fix(reach_at_fix(trace, 0, 15))
150+
print(f'Fixed points exists? {fixed_points_fix(trace)}')
151+
152+
fig = go.Figure()
153+
fig = reachtube_tree(trace, None, fig, 0, 1, [0, 1], "fill", "trace")
154+
# fig = simulation_tree(trace, None, fig, 1, 2, [1, 2], "fill", "trace")
155+
fig.show()

‎demo/fixed_points/power_train.py

+12-11
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,7 @@
1717
from verse.plotter.plotter3D_new import *
1818
import plotly.graph_objects as go
1919

20-
from fixed_points import fixed_points_fix, pp_fix, reach_at_fix
20+
from fixed_points import fixed_points_fix, pp_fix, reach_at_fix, contain_all_fix
2121
### full disclosure, structure of file from mp4_p2
2222

2323
# <mode id="0" initial="True" name="Mode0">
@@ -93,9 +93,9 @@ def TC_simulate(
9393
trace[1:, 0] = [round(i * time_step, 10) for i in range(num_points)]
9494
trace[0, 1:] = init
9595
for i in range(num_points):
96-
if mode[0]=="On":
96+
if mode[0]=="Mode0":
9797
r = ode(self.dynamic_mode0)
98-
elif mode[0]=="Off":
98+
elif mode[0]=="Mode1":
9999
r = ode(self.dynamic_mode1)
100100
else:
101101
raise ValueError
@@ -144,11 +144,7 @@ def decisionLogic(ego: State, other: State):
144144

145145
scenario.add_agent(PT) ### need to add breakpoint around here to check decision_logic of agents
146146

147-
148-
# <property initialSet="Mode0:p==0.6353&amp;&amp;lam==14.7&amp;&amp;pe==0.5573&amp;&amp;ivalue==0.017&amp;&amp;t==0" name="Prop1" type="Safety" unsafeSet="lam&gt;=100">
149-
# <parameters kvalue="4000.0" timehorizon="15.0" timestep="0.001" />
150-
151-
init_PT = [[0.6353, 14.7, 0.5573, 0, 0],[0.6353, 14.7, 0.5573, 0, 0]]
147+
init_PT = [[0.6353, 14.7, 0.5573, 0.017, 0],[0.6353, 14.7, 0.5573, 0.017, 0]]
152148
# # -----------------------------------------
153149

154150
scenario.set_init_single(
@@ -157,10 +153,15 @@ def decisionLogic(ego: State, other: State):
157153

158154
trace = scenario.verify(15, 0.001)
159155

160-
pp_fix(reach_at_fix(trace, 0, 15))
161-
print(f'Fixed points exists? {fixed_points_fix(trace)}')
156+
# pp_fix(reach_at_fix(trace, 0, 15))
157+
158+
T, t_step = 15, 0.001
159+
# r_final = reach_at_fix(trace)
160+
# r_candid = reach_at_fix(trace, 0, T-t_step+t_step*.01)
161+
# print(f'Does the candidate reachset contain the final reachset: {contain_all_fix(r_final, r_candid)}')
162+
print(f'Fixed points exists? {fixed_points_fix(trace, 15, 0.001)}')
162163

163164
fig = go.Figure()
164-
fig = reachtube_tree(trace, None, fig, 0, 1, [0, 1], "fill", "trace")
165+
fig = reachtube_tree(trace, None, fig, 0, 2, [0, 2], "fill", "trace")
165166
# fig = simulation_tree(trace, None, fig, 1, 2, [1, 2], "fill", "trace")
166167
fig.show()

0 commit comments

Comments
 (0)