diff --git a/verse/agents/example_agent/car_agent.py b/verse/agents/example_agent/car_agent.py
index abbaf36a92dad967e04e095db7e50d200336991e..c9fa3d9ef875e68ec2b075005efda4229c43acd7 100644
--- a/verse/agents/example_agent/car_agent.py
+++ b/verse/agents/example_agent/car_agent.py
@@ -7,6 +7,7 @@ from scipy.integrate import ode
from verse import BaseAgent
from verse import LaneMap
from verse.analysis.utils import wrap_to_pi
+from verse.analysis.analysis_tree import TraceType
from verse.parser import ControllerIR
class NPCAgent(BaseAgent):
@@ -42,20 +43,19 @@ class NPCAgent(BaseAgent):
a = 0
return steering, a
- def TC_simulate(self, mode: List[str], initialCondition, time_bound, time_step, lane_map:LaneMap=None)->np.ndarray:
+ def TC_simulate(self, mode: List[str], initialCondition, time_bound, time_step, lane_map:LaneMap=None)->TraceType:
time_bound = float(time_bound)
number_points = int(np.ceil(time_bound/time_step))
- t = [i*time_step for i in range(0,number_points)]
init = initialCondition
- trace = [[0]+init]
- for i in range(len(t)):
+ trace = np.array([0, *init])
+ for i in range(number_points):
steering, a = self.action_handler(mode, init, lane_map)
r = ode(self.dynamic)
r.set_initial_value(init).set_f_params([steering, a])
res:np.ndarray = r.integrate(r.t + time_step)
- init = res.flatten().tolist()
- trace.append([t[i] + time_step] + init)
+ init = res.flatten()
+ trace = np.vstack((trace, np.insert(init, 0, time_step * (i + 1))))
return np.array(trace)
@@ -101,24 +101,22 @@ class CarAgent(BaseAgent):
steering = np.clip(steering, -0.61, 0.61)
return steering, a
- def TC_simulate(self, mode: List[str], initialCondition, time_bound, time_step, lane_map:LaneMap=None)->np.ndarray:
+ def TC_simulate(self, mode: List[str], initialCondition, time_bound, time_step, lane_map:LaneMap=None)->TraceType:
time_bound = float(time_bound)
number_points = int(np.ceil(time_bound/time_step))
- t = [round(i*time_step,10) for i in range(0,number_points)]
init = initialCondition
- trace = [[0]+init]
- for i in range(len(t)):
+ trace = np.array([0, *init])
+ for i in range(number_points):
steering, a = self.action_handler(mode, init, lane_map)
r = ode(self.dynamic)
r.set_initial_value(init).set_f_params([steering, a])
res:np.ndarray = r.integrate(r.t + time_step)
- init = res.flatten().tolist()
+ init = res.flatten()
if init[3] < 0:
init[3] = 0
- trace.append([t[i] + time_step] + init)
-
- return np.array(trace)
+ trace = np.vstack((trace, np.insert(init, 0, time_step * (i + 1))))
+ return trace
class WeirdCarAgent(CarAgent):
def __init__(self, id, code = None, file_name = None):
diff --git a/verse/analysis/analysis_tree.py b/verse/analysis/analysis_tree.py
index ddb7532f9adf9b192cbbae126c238d1ea3d49e90..8e4420b43ae2b7ade1cc1b17b24171dce15e4e97 100644
--- a/verse/analysis/analysis_tree.py
+++ b/verse/analysis/analysis_tree.py
@@ -1,6 +1,13 @@
-from typing import List, Dict, Any
+from functools import reduce
+import pickle
+from typing import List, Dict, Any, Optional
import json
from treelib import Tree
+import numpy.typing as nptyp, numpy as np, portion
+
+from verse.analysis.dryvr import _EPSILON
+
+TraceType = nptyp.NDArray[np.float_]
class AnalysisTreeNode:
"""AnalysisTreeNode class
@@ -26,7 +33,7 @@ class AnalysisTreeNode:
type = 'simtrace',
id = 0
):
- self.trace:Dict = trace
+ self.trace: Dict[str, TraceType] = trace
self.init: Dict[str, List[float]] = init
self.mode: Dict[str, List[str]] = mode
self.agent: Dict = agent
@@ -45,12 +52,12 @@ class AnalysisTreeNode:
'parent': None,
'child': [],
'agent': {},
- 'init': self.init,
- 'mode': self.mode,
+ 'init': {aid: list(init) for aid, init in self.init.items()},
+ 'mode': self.mode,
'height': self.height,
'static': self.static,
'start_time': self.start_time,
- 'trace': self.trace,
+ 'trace': {aid: t.tolist() for aid, t in self.trace.items()},
'type': self.type,
'assert_hits': self.assert_hits
}
@@ -86,7 +93,7 @@ class AnalysisTreeNode:
@staticmethod
def from_dict(data) -> "AnalysisTreeNode":
return AnalysisTreeNode(
- trace = data['trace'],
+ trace = {aid: np.array(data['trace'][aid]) for aid in data["agent"].keys()},
init = data['init'],
mode = data['mode'],
height = data['height'],
diff --git a/verse/analysis/incremental.py b/verse/analysis/incremental.py
index 82756bbb692a05ccc833637811a90d72f75dc4c8..5228d9019dab1b20d1a74c191f42c30c3b615146 100644
--- a/verse/analysis/incremental.py
+++ b/verse/analysis/incremental.py
@@ -5,7 +5,7 @@ from typing import Any, DefaultDict, List, Tuple, Optional, Dict
from verse.agents.base_agent import BaseAgent
from verse.analysis import AnalysisTreeNode
from intervaltree import IntervalTree
-import itertools, copy, numpy as np
+import itertools, copy, numpy.typing as nptyp, numpy as np
from verse.analysis.dryvr import _EPSILON
# from verse.analysis.simulator import PathDiffs
@@ -21,7 +21,7 @@ class CachedTransition:
@dataclass
class CachedSegment:
- trace: List[List[float]]
+ trace: nptyp.NDArray[np.float_]
asserts: List[str]
transitions: List[CachedTransition]
run_num: int
@@ -97,7 +97,7 @@ def to_simulate(old_agents: Dict[str, BaseAgent], new_agents: Dict[str, BaseAgen
def convert_sim_trans(agent_id, transit_agents, inits, transition, trans_ind):
if agent_id in transit_agents:
- return [CachedTransition(inits, trans_ind, mode, init, paths) for _id, mode, init, paths in transition]
+ return [CachedTransition({k: list(v) for k, v in inits.items()}, trans_ind, mode, list(init), paths) for _id, mode, init, paths in transition]
else:
return []
@@ -138,7 +138,7 @@ class SimTraceCache:
def __init__(self):
self.cache: DefaultDict[tuple, IntervalTree] = defaultdict(IntervalTree)
- def add_segment(self, agent_id: str, node: AnalysisTreeNode, transit_agents: List[str], trace: List[List[float]], transition, trans_ind: int, run_num: int):
+ def add_segment(self, agent_id: str, node: AnalysisTreeNode, transit_agents: List[str], trace: nptyp.NDArray[np.float_], transition, trans_ind: int, run_num: int):
key = (agent_id,) + tuple(node.mode[agent_id])
init = node.init[agent_id]
tree = self.cache[key]
diff --git a/verse/analysis/simulator.py b/verse/analysis/simulator.py
index a0a5a75c4b9455db3c3811530fa2625d2be49565..651a529e1e18d7cf67041e41bfbde294749f2027 100644
--- a/verse/analysis/simulator.py
+++ b/verse/analysis/simulator.py
@@ -13,7 +13,7 @@ from verse.parser.parser import ModePath, find
pp = functools.partial(pprint.pprint, compact=True, width=130)
# from verse.agents.base_agent import BaseAgent
-from verse.analysis.analysis_tree import AnalysisTreeNode, AnalysisTree
+from verse.analysis.analysis_tree import AnalysisTreeNode, AnalysisTree, TraceType
PathDiffs = List[Tuple[BaseAgent, ModePath]]
@@ -29,7 +29,7 @@ class Simulator:
self.cache_hits = (0, 0)
@ray.remote
- def simulate_one(config: "ScenarioConfig", cache, node: AnalysisTreeNode, later: int, remain_time: float, time_step: float, lane_map: LaneMap, run_num: int, past_runs: List[AnalysisTree], transition_graph: "Scenario") -> Tuple[int, int, List[AnalysisTreeNode], Dict[str, list], list]:
+ def simulate_one(config: "ScenarioConfig", cache, node: AnalysisTreeNode, later: int, remain_time: float, time_step: float, lane_map: LaneMap, run_num: int, past_runs: List[AnalysisTree], transition_graph: "Scenario") -> Tuple[int, int, List[AnalysisTreeNode], Dict[str, TraceType], list]:
print(f"node id: {node.id}")
cached_segments = {}
cache_updates = []
@@ -58,10 +58,8 @@ class Simulator:
else:
# pp(("sim", agent_id, *mode, *init))
# Simulate the trace starting from initial condition
- trace = node.agent[agent_id].TC_simulate(
- mode, init, remain_time, time_step, lane_map)
+ trace = node.agent[agent_id].TC_simulate(mode, init, remain_time, time_step, lane_map)
trace[:, 0] += node.start_time
- trace = trace.tolist()
node.trace[agent_id] = trace
# pp(("cached_segments", cached_segments.keys()))
# TODO: for now, make sure all the segments comes from the same node; maybe we can do
@@ -86,7 +84,8 @@ class Simulator:
# pp(("next init:", {a: trace[transition_idx] for a, trace in node.trace.items()}))
# truncate the computed trajectories from idx and store the content after truncate
- truncated_trace, full_traces = {}, {}
+ truncated_trace: Dict[str, TraceType] = {}
+ full_traces: Dict[str, TraceType] = {}
for agent_idx in node.agent:
full_traces[agent_idx] = node.trace[agent_idx]
if transitions:
@@ -151,7 +150,7 @@ class Simulator:
for agent_idx in next_node_agent:
if agent_idx not in next_node_init:
next_node_trace[agent_idx] = truncated_trace[agent_idx]
- next_node_init[agent_idx] = truncated_trace[agent_idx][0][1:]
+ next_node_init[agent_idx] = truncated_trace[agent_idx][0][1:].tolist()
all_transition_paths.append(transition_paths)
tmp = AnalysisTreeNode(
@@ -293,7 +292,6 @@ class Simulator:
trace = node.agent[agent_id].TC_simulate(
mode, init, remain_time, time_step, lane_map)
trace[:, 0] += node.start_time
- trace = trace.tolist()
node.trace[agent_id] = trace
# pp(("cached_segments", cached_segments.keys()))
# TODO: for now, make sure all the segments comes from the same node; maybe we can do