diff --git a/animation.gif b/animation.gif new file mode 100644 index 00000000..4de748c2 Binary files /dev/null and b/animation.gif differ diff --git a/tutorial/dl_sec6.py b/tutorial/dl_sec6.py new file mode 100644 index 00000000..9c3ba353 --- /dev/null +++ b/tutorial/dl_sec6.py @@ -0,0 +1,79 @@ +from enum import Enum, auto + + +class AgentMode(Enum): + Normal = auto() + SwitchLeft = auto() + SwitchRight = auto() + + +class TrackMode(Enum): + T0 = auto() + T1 = auto() + M01 = auto() + M10 = auto() + + +class State: + x: float + y: float + theta: float + v: float + agent_mode: AgentMode + track_mode: TrackMode + + def __init__(self, x, y, theta, v, agent_mode: AgentMode, track_mode: TrackMode): + pass + + +from typing import List +import copy + +#DL 1 +# def decisionLogic(ego: State, other: State, track_map): +# output = copy.deepcopy(ego) +# if ego.agent_mode == AgentMode.Normal: +# if ego.track_mode == other.track_mode and 6 < (other.x - ego.x) and (other.x - ego.x) < 8: +# if track_map.h_exist(ego.track_mode, ego.agent_mode, AgentMode.SwitchLeft): +# output.agent_mode = AgentMode.SwitchLeft +# output.track_mode = track_map.h(ego.track_mode, ego.agent_mode, AgentMode.SwitchLeft) + +# if ego.track_mode == other.track_mode and 6 < (other.x - ego.x) and (other.x - ego.x) < 8: +# if track_map.h_exist(ego.track_mode, ego.agent_mode, AgentMode.SwitchRight): +# output.agent_mode = AgentMode.SwitchRight +# output.track_mode = track_map.h(ego.track_mode, ego.agent_mode, AgentMode.SwitchRight) + +# if ego.agent_mode == AgentMode.SwitchLeft: +# if ego.y >= 2.5: +# output.agent_mode = AgentMode.Normal +# output.track_mode = track_map.h(ego.track_mode, ego.agent_mode, AgentMode.Normal) +# if ego.agent_mode == AgentMode.SwitchRight: +# if ego.y <= -2.5: +# output.agent_mode = AgentMode.Normal +# output.track_mode = track_map.h(ego.track_mode, ego.agent_mode, AgentMode.Normal) +# return output + +#DL 2 and 3 +def decisionLogic(ego: State, other: State, track_map): + output = copy.deepcopy(ego) + if ego.agent_mode == AgentMode.Normal: + if ego.track_mode == other.track_mode and 6 < other.dist and other.dist < 8: + if track_map.h_exist(ego.track_mode, ego.agent_mode, AgentMode.SwitchLeft): + output.agent_mode = AgentMode.SwitchLeft + output.track_mode = track_map.h(ego.track_mode, ego.agent_mode, AgentMode.SwitchLeft) + + #drop + #if ego.track_mode == other.track_mode and 6 < other.dist and other.dist < 8: + if track_map.h_exist(ego.track_mode, ego.agent_mode, AgentMode.SwitchRight): + output.agent_mode = AgentMode.SwitchRight + output.track_mode = track_map.h(ego.track_mode, ego.agent_mode, AgentMode.SwitchRight) + + if ego.agent_mode == AgentMode.SwitchLeft: + if ego.y >= 2.5: + output.agent_mode = AgentMode.Normal + output.track_mode = track_map.h(ego.track_mode, ego.agent_mode, AgentMode.Normal) + if ego.agent_mode == AgentMode.SwitchRight: + if ego.y <= -2.5: + output.agent_mode = AgentMode.Normal + output.track_mode = track_map.h(ego.track_mode, ego.agent_mode, AgentMode.Normal) + return output \ No newline at end of file diff --git a/tutorial/test_sense.py b/tutorial/test_sense.py new file mode 100644 index 00000000..ddf0cb00 --- /dev/null +++ b/tutorial/test_sense.py @@ -0,0 +1,322 @@ +from verse.map.lane_segment import StraightLane +from verse.map.lane import Lane + +segment0 = StraightLane("seg0", [0, 0], [500, 0], 3) +lane0 = Lane("T0", [segment0]) +segment1 = StraightLane("seg1", [0, 3], [500, 3], 3) +lane1 = Lane("T1", [segment1]) + +h_dict = { + ("T0", "Normal", "SwitchLeft"): "M01", + ("T1", "Normal", "SwitchRight"): "M10", + ("M01", "SwitchLeft", "Normal"): "T1", + ("M10", "SwitchRight", "Normal"): "T0", +} + + +def h(lane_idx, agent_mode_src, agent_mode_dest): + return h_dict[(lane_idx, agent_mode_src, agent_mode_dest)] + + +def h_exist(lane_idx, agent_mode_src, agent_mode_dest): + return (lane_idx, agent_mode_src, agent_mode_dest) in h_dict + + +from verse.map import LaneMap + + +class Map2Lanes(LaneMap): + def __init__(self): + super().__init__() + self.add_lanes([lane0, lane1]) + self.h = h + self.h_exist = h_exist + + +from enum import Enum, auto + + +class AgentMode(Enum): + Normal = auto() + SwitchLeft = auto() + SwitchRight = auto() + + +class TrackMode(Enum): + T0 = auto() + T1 = auto() + M01 = auto() + M10 = auto() + +#Sensor 1 +class VechicleBaseSensor: + def __init__(self): + self.sensor = 1 + + def sense(self, agent, state_dict, lane_map, simulate = True): + len_dict = {} + cont = {} + disc = {} + len_dict = {"others": len(state_dict) - 1} + if simulate: + if agent.id == 'car1': + cont['ego.x'] = state_dict['car1'][0][1] + cont['ego.y'] = state_dict['car1'][0][2] + cont['ego.theta'] = state_dict['car1'][0][3] + cont['ego.v'] = state_dict['car1'][0][4] + disc['ego.agent_mode'] = state_dict['car1'][1][0] + disc['ego.track_mode'] = state_dict['car1'][1][1] + + cont['other.x'] = state_dict['car2'][0][1] + cont['other.y'] = state_dict['car2'][0][2] + disc['other.track_mode'] = state_dict['car2'][1][1] + + if agent.id == 'car2': + cont['ego.x'] = state_dict['car2'][0][1] + cont['ego.y'] = state_dict['car2'][0][2] + cont['ego.theta'] = state_dict['car2'][0][3] + cont['ego.v'] = state_dict['car2'][0][4] + disc['ego.agent_mode'] = state_dict['car2'][1][0] + disc['ego.track_mode'] = state_dict['car2'][1][1] + + cont['other.x'] = state_dict['car1'][0][1] + cont['other.y'] = state_dict['car1'][0][2] + disc['other.track_mode'] = state_dict['car1'][1][1] + + else: + if agent.id == 'car1': + cont['ego.x'] = [state_dict['car1'][0][0][1], state_dict['car1'][0][1][1]] + cont['ego.y'] = [state_dict['car1'][0][0][2], state_dict['car1'][0][1][2]] + cont['ego.theta'] = [state_dict['car1'][0][0][3], state_dict['car1'][0][1][3]] + cont['ego.v'] = [state_dict['car1'][0][0][4], state_dict['car1'][0][1][4]] + disc['ego.agent_mode'] = state_dict['car1'][1][0] + disc['ego.track_mode'] = state_dict['car1'][1][1] + + #Sensor 1 Difference + cont['other.x'] = [state_dict['car2'][0][0][1], state_dict['car2'][0][1][1]] + cont['other.y'] = [state_dict['car2'][0][0][2], state_dict['car2'][0][1][2]] + disc['other.track_mode'] = state_dict['car2'][1][1] + + if agent.id == 'car2': + cont['ego.x'] = [state_dict['car2'][0][0][1], state_dict['car2'][0][1][1]] + cont['ego.y'] = [state_dict['car2'][0][0][2], state_dict['car2'][0][1][2]] + cont['ego.theta'] = [state_dict['car2'][0][0][3], state_dict['car2'][0][1][3]] + cont['ego.v'] = [state_dict['car2'][0][0][4], state_dict['car2'][0][1][4]] + disc['ego.agent_mode'] = state_dict['car2'][1][0] + disc['ego.track_mode'] = state_dict['car2'][1][1] + + #Sensor 1 Difference + cont['other.x'] = [state_dict['car1'][0][0][1], state_dict['car1'][0][1][1]] + cont['other.y'] = [state_dict['car1'][0][0][2], state_dict['car1'][0][1][2]] + disc['other.track_mode'] = state_dict['car1'][1][1] + return cont, disc, len_dict + +#Sensor 2 +class VechicleDistSensor: + def __init__(self): + self.sensor = 1 + + def sense(self, agent, state_dict, lane_map, simulate = True): + len_dict = {} + cont = {} + disc = {} + len_dict = {"others": len(state_dict) - 1} + if simulate: + if agent.id == 'car1': + cont['ego.x'] = state_dict['car1'][0][1] + cont['ego.y'] = state_dict['car1'][0][2] + cont['ego.theta'] = state_dict['car1'][0][3] + cont['ego.v'] = state_dict['car1'][0][4] + disc['ego.agent_mode'] = state_dict['car1'][1][0] + disc['ego.track_mode'] = state_dict['car1'][1][1] + + cont['other.dist'] = state_dict['car2'][0][1] - state_dict['car1'][0][1] + disc['other.track_mode'] = state_dict['car2'][1][1] + + if agent.id == 'car2': + cont['ego.x'] = state_dict['car2'][0][1] + cont['ego.y'] = state_dict['car2'][0][2] + cont['ego.theta'] = state_dict['car2'][0][3] + cont['ego.v'] = state_dict['car2'][0][4] + disc['ego.agent_mode'] = state_dict['car2'][1][0] + disc['ego.track_mode'] = state_dict['car2'][1][1] + + cont['other.dist'] = state_dict['car1'][0][1] - state_dict['car2'][0][1] + disc['other.track_mode'] = state_dict['car1'][1][1] + + else: + if agent.id == 'car1': + cont['ego.x'] = [state_dict['car1'][0][0][1], state_dict['car1'][0][1][1]] + cont['ego.y'] = [state_dict['car1'][0][0][2], state_dict['car1'][0][1][2]] + cont['ego.theta'] = [state_dict['car1'][0][0][3], state_dict['car1'][0][1][3]] + cont['ego.v'] = [state_dict['car1'][0][0][4], state_dict['car1'][0][1][4]] + disc['ego.agent_mode'] = state_dict['car1'][1][0] + disc['ego.track_mode'] = state_dict['car1'][1][1] + + #Sensor 2 Difference + disc['other.track_mode'] = state_dict['car2'][1][1] + + other_x = [state_dict['car2'][0][0][1], state_dict['car2'][0][1][1]] + other_y = [state_dict['car2'][0][0][2], state_dict['car2'][0][1][2]] + + ego_long = [] + other_long = [] + for x in cont['ego.x']: + for y in cont['ego.y']: + ego_long.append(lane_map.get_longitudinal_position(disc['ego.track_mode'], [x,y])) + + for x in other_x: + for y in other_y: + other_long.append(lane_map.get_longitudinal_position(disc['other.track_mode'], [x,y])) + + cont['other.dist'] = [min(other_long) - max(ego_long), max(other_long) - min(ego_long)] + + + + if agent.id == 'car2': + cont['ego.x'] = [state_dict['car2'][0][0][1], state_dict['car2'][0][1][1]] + cont['ego.y'] = [state_dict['car2'][0][0][2], state_dict['car2'][0][1][2]] + cont['ego.theta'] = [state_dict['car2'][0][0][3], state_dict['car2'][0][1][3]] + cont['ego.v'] = [state_dict['car2'][0][0][4], state_dict['car2'][0][1][4]] + disc['ego.agent_mode'] = state_dict['car2'][1][0] + disc['ego.track_mode'] = state_dict['car2'][1][1] + + #Sensor 2 Difference + disc['other.track_mode'] = state_dict['car1'][1][1] + + other_x = [state_dict['car1'][0][0][1], state_dict['car1'][0][1][1]] + other_y = [state_dict['car1'][0][0][2], state_dict['car1'][0][1][2]] + + ego_long = [] + other_long = [] + for x in cont['ego.x']: + for y in cont['ego.y']: + ego_long.append(lane_map.get_longitudinal_position(disc['ego.track_mode'], [x,y])) + + for x in other_x: + for y in other_y: + other_long.append(lane_map.get_longitudinal_position(disc['other.track_mode'], [x,y])) + + cont['other.dist'] = [min(other_long) - max(ego_long), max(other_long) - min(ego_long)] + return cont, disc, len_dict + +#Sensor 3 +class VechicleNoisySensor: + def __init__(self, noise): + self.sensor = 1 + self.noise = noise + + def sense(self, agent, state_dict, lane_map, simulate = True): + len_dict = {} + cont = {} + disc = {} + len_dict = {"others": len(state_dict) - 1} + if simulate: + if agent.id == 'car1': + cont['ego.x'] = state_dict['car1'][0][1] + cont['ego.y'] = state_dict['car1'][0][2] + cont['ego.theta'] = state_dict['car1'][0][3] + cont['ego.v'] = state_dict['car1'][0][4] + disc['ego.agent_mode'] = state_dict['car1'][1][0] + disc['ego.track_mode'] = state_dict['car1'][1][1] + + cont['other.dist'] = (state_dict['car2'][0][1] - state_dict['car1'][0][1]) + self.noise + disc['other.track_mode'] = state_dict['car2'][1][1] + + if agent.id == 'car2': + cont['ego.x'] = state_dict['car2'][0][1] + cont['ego.y'] = state_dict['car2'][0][2] + cont['ego.theta'] = state_dict['car2'][0][3] + cont['ego.v'] = state_dict['car2'][0][4] + disc['ego.agent_mode'] = state_dict['car2'][1][0] + disc['ego.track_mode'] = state_dict['car2'][1][1] + + cont['other.dist'] = (state_dict['car1'][0][1] - state_dict['car2'][0][1]) + self.noise + disc['other.track_mode'] = state_dict['car1'][1][1] + + else: + if agent.id == 'car1': + cont['ego.x'] = [state_dict['car1'][0][0][1], state_dict['car1'][0][1][1]] + cont['ego.y'] = [state_dict['car1'][0][0][2], state_dict['car1'][0][1][2]] + cont['ego.theta'] = [state_dict['car1'][0][0][3], state_dict['car1'][0][1][3]] + cont['ego.v'] = [state_dict['car1'][0][0][4], state_dict['car1'][0][1][4]] + disc['ego.agent_mode'] = state_dict['car1'][1][0] + disc['ego.track_mode'] = state_dict['car1'][1][1] + + #Sensor 3 Difference + disc['other.track_mode'] = state_dict['car2'][1][1] + + other_x = [state_dict['car2'][0][0][1], state_dict['car2'][0][1][1]] + other_y = [state_dict['car2'][0][0][2], state_dict['car2'][0][1][2]] + + ego_long = [] + other_long = [] + for x in cont['ego.x']: + for y in cont['ego.y']: + ego_long.append(lane_map.get_longitudinal_position(disc['ego.track_mode'], [x,y])) + + for x in other_x: + for y in other_y: + other_long.append(lane_map.get_longitudinal_position(disc['other.track_mode'], [x,y])) + + cont['other.dist'] = [min(other_long) - max(ego_long), max(other_long) - min(ego_long)] + cont['other.dist'] = [cont['other.dist'][0] - self.noise, cont['other.dist'][1] + self.noise] + + if agent.id == 'car2': + cont['ego.x'] = [state_dict['car2'][0][0][1], state_dict['car2'][0][1][1]] + cont['ego.y'] = [state_dict['car2'][0][0][2], state_dict['car2'][0][1][2]] + cont['ego.theta'] = [state_dict['car2'][0][0][3], state_dict['car2'][0][1][3]] + cont['ego.v'] = [state_dict['car2'][0][0][4], state_dict['car2'][0][1][4]] + disc['ego.agent_mode'] = state_dict['car2'][1][0] + disc['ego.track_mode'] = state_dict['car2'][1][1] + + #Sensor 3 Difference + disc['other.track_mode'] = state_dict['car1'][1][1] + + other_x = [state_dict['car1'][0][0][1], state_dict['car1'][0][1][1]] + other_y = [state_dict['car1'][0][0][2], state_dict['car1'][0][1][2]] + + ego_long = [] + other_long = [] + for x in cont['ego.x']: + for y in cont['ego.y']: + ego_long.append(lane_map.get_longitudinal_position(disc['ego.track_mode'], [x,y])) + + for x in other_x: + for y in other_y: + other_long.append(lane_map.get_longitudinal_position(disc['other.track_mode'], [x,y])) + + cont['other.dist'] = [min(other_long) - max(ego_long), max(other_long) - min(ego_long)] + cont['other.dist'] = [cont['other.dist'][0] - self.noise, cont['other.dist'][1] + self.noise] + return cont, disc, len_dict + + +from verse.scenario import Scenario, ScenarioConfig + +scenario = Scenario(ScenarioConfig(parallel=False)) +scenario.set_map(Map2Lanes()) + +from tutorial_agent import CarAgent + +car1 = CarAgent("car1", file_name="tutorial/dl_sec6.py") +car1.set_initial([[0, -0.5, 0, 2], [0.5, 0.5, 0, 2]], (AgentMode.Normal, TrackMode.T0)) +car2 = CarAgent("car2", file_name="tutorial/dl_sec6.py") +car2.set_initial([[20, -0.5, 0, 1], [20.5, 0.5, 0, 1]], (AgentMode.Normal, TrackMode.T0)) +scenario.add_agent(car1) +scenario.add_agent(car2) + +#scenario.set_sensor(VechicleBaseSensor()) +scenario.set_sensor(VechicleDistSensor()) +#scenario.set_sensor(VechicleNoisySensor(1)) + +traces_veri = scenario.verify(20, 0.01) + +import plotly.graph_objects as go +from verse.plotter.plotter2D import * + +fig = go.Figure() +fig = reachtube_tree(traces_veri, Map2Lanes(), fig, 1, 2, [1, 2], "lines", "trace") +#fig.show() +fig.write_html('figure.html', auto_open=True) + + diff --git a/verse/plotter/plotter3D_new.py b/verse/plotter/plotter3D_new.py index ba89f0d0..7698c1dc 100644 --- a/verse/plotter/plotter3D_new.py +++ b/verse/plotter/plotter3D_new.py @@ -5,6 +5,13 @@ from plotly.subplots import make_subplots from enum import Enum, auto +import matplotlib.pyplot as plt +from scipy.spatial import ConvexHull + +from mpl_toolkits.mplot3d.art3d import Poly3DCollection +from matplotlib.animation import FuncAnimation +import math + # import time from verse.analysis.analysis_tree import AnalysisTree, AnalysisTreeNode from verse.map.lane_map_3d import LaneMap_3d @@ -538,4 +545,173 @@ def update_style(fig: go.Figure() = go.Figure(), x_title=None, y_title=None, z_t # gridwidth=gridwidth, # gridcolor="LightGrey", # ) + return fig + +colors = [ + ["#CC0000", "#FF0000", "#FF3333", "#FF6666", "#FF9999", "#FFCCCC"], # red + ["#0000CC", "#0000FF", "#3333FF", "#6666FF", "#9999FF", "#CCCCFF"], # blue + ["#00CC00", "#00FF00", "#33FF33", "#66FF66", "#99FF99", "#CCFFCC"], # green + ["#CCCC00", "#FFFF00", "#FFFF33", "#FFFF66", "#FFFF99", "#FFE5CC"], # yellow + # ['#66CC00', '#80FF00', '#99FF33', '#B2FF66', '#CCFF99', '#FFFFCC'], # yellowgreen + ["#CC00CC", "#FF00FF", "#FF33FF", "#FF66FF", "#FF99FF", "#FFCCFF"], # magenta + # ['#00CC66', '#00FF80', '#33FF99', '#66FFB2', '#99FFCC', '#CCFFCC'], # springgreen + ["#00CCCC", "#00FFFF", "#33FFFF", "#66FFFF", "#99FFFF", "#CCFFE5"], # cyan + # ['#0066CC', '#0080FF', '#3399FF', '#66B2FF', '#99CCFF', '#CCE5FF'], # cyanblue + ["#CC6600", "#FF8000", "#FF9933", "#FFB266", "#FFCC99", "#FFE5CC"], # orange + # ['#6600CC', '#7F00FF', '#9933FF', '#B266FF', '#CC99FF', '#E5CCFF'], # purple + ["#00CC00", "#00FF00", "#33FF33", "#66FF66", "#99FF99", "#E5FFCC"], # lime + ["#CC0066", "#FF007F", "#FF3399", "#FF66B2", "#FF99CC", "#FFCCE5"], # pink +] + +scheme_dict = { + "red": 0, + "blue": 1, + "green": 2, + "yellow": 3, + "yellowgreen": 4, + "lime": 5, + "springgreen": 6, + "cyan": 7, + "cyanblue": 8, + "orange": 9, + "purple": 10, + "magenta": 11, + "pink": 12, +} +scheme_list = list(scheme_dict.keys()) +num_theme = len(colors) +color_cnt = 0 +text_size = 8 +marker_size = 4 +scale_factor = 0.25 +mode_point_color = "rgba(0,0,0,0.5)" +mode_text_color = "black" +duration = 1 + +def reachtube_anime_3d( + root: Union[AnalysisTree, AnalysisTreeNode], + fig=go.Figure(), + x_dim: int = 1, + x_title: str = None, + y_dim: int = 2, + y_title: str = None, + z_dim: int = 3, + z_title: str = None, + print_dim_list=None, + color_array=scheme_list, + sample_rate=1, + combine_rect=None, + save = False, + name = "animation.gif" +): + """It gives the animation of the verification traces in 3D.""" + if isinstance(root, AnalysisTree): + root = root.root + + + num_digit = 3 + root = sample_trace(root, sample_rate) + agent_list = list(root.agent.keys()) + timed_point_dict = {} + queue = [root] + + + num_dim = np.array(root.trace[agent_list[0]]).shape[1] + check_dim(num_dim, x_dim, y_dim, z_dim, print_dim_list) + if print_dim_list is None: + print_dim_list = range(0, num_dim) + + num_points = 0 + while queue: + node = queue.pop() + traces = node.trace + for agent_id in traces: + trace = np.array(traces[agent_id]) + if trace[0][0] > 0: + trace = trace[8:] + for i in range(0, len(trace) - 1, 2): + time_point = round(trace[i][0], num_digit) + + rect = [trace[i][0:].tolist(), trace[i + 1][0:].tolist()] + if time_point not in timed_point_dict: + num_points += 1 + timed_point_dict[time_point] = {agent_id: [rect]} + else: + if agent_id in timed_point_dict[time_point]: + timed_point_dict[time_point][agent_id].append(rect) + else: + timed_point_dict[time_point][agent_id] = [rect] + + queue += node.child + + fig = plt.figure(figsize=(14, 10)) + ax = fig.add_subplot(111, projection='3d') + + time_steps = sorted(timed_point_dict.keys()) + + #init agent colors + time_step = time_steps[0] + step_info = timed_point_dict[time_step] + agents = list(step_info.keys()) + agent_colors = {} + for i,agent in enumerate(agents): + agent_colors[agent] = color_array[i] + + # init plot + def init(): + ax.set_xlabel(x_title) + ax.set_ylabel(y_title) + ax.set_zlabel(z_title) + ax.set_xlim([0, 55]) + ax.set_ylim([-10, 260]) + ax.set_zlim([-50, 100]) + ax.set_title('3D Rectangle Animation') + + def update(frame): + time_step = time_steps[frame] + step_info = timed_point_dict[time_step] + agents = list(step_info.keys()) + + for agent in agents: + agent_rectangles = step_info[agent] + for rect in agent_rectangles: + x_min = rect[0][x_dim] + y_min = rect[0][y_dim] + z_min = rect[0][z_dim] + + x_max = rect[1][x_dim] + y_max = rect[1][y_dim] + z_max = rect[1][z_dim] + + vertices = np.array([ + [x_min, y_min, z_min], # Bottom-left-front + [x_max, y_min, z_min], # Bottom-right-front + [x_max, y_max, z_min], # Bottom-right-back + [x_min, y_max, z_min], # Bottom-left-back + [x_min, y_min, z_max], # Top-left-front + [x_max, y_min, z_max], # Top-right-front + [x_max, y_max, z_max], # Top-right-back + [x_min, y_max, z_max] # Top-left-back + ]) + + faces = [ + [vertices[0], vertices[1], vertices[2], vertices[3]], # Bottom face + [vertices[4], vertices[5], vertices[6], vertices[7]], # Top face + [vertices[0], vertices[1], vertices[5], vertices[4]], # Front face + [vertices[2], vertices[3], vertices[7], vertices[6]], # Back face + [vertices[1], vertices[2], vertices[6], vertices[5]], # Right face + [vertices[0], vertices[3], vertices[7], vertices[4]], # Left face + ] + + # Plot the rectangle + cuboid = Poly3DCollection(faces, facecolors=agent_colors[agent], linewidths=1, edgecolors=agent_colors[agent], alpha=.125) + ax.add_collection3d(cuboid) + ax.view_init(elev=15, azim=frame*0.75) + + + # Create animation + ani = FuncAnimation(fig, update, frames=len(time_steps), init_func=init, interval = 25, repeat=True) + if save: + ani.save(name) + # plt.show() return fig \ No newline at end of file diff --git a/verse/stars/starset.py b/verse/stars/starset.py index 3f2d3ffc..f81010ff 100644 --- a/verse/stars/starset.py +++ b/verse/stars/starset.py @@ -176,22 +176,31 @@ def calc_reach_tube(self, mode_label,time_horizon,time_step,sim_func,bloating_me #odd entries: bloat_tube[::2, 1:] #even entries: bloat_tube[1::2, 1:] #data only bloat_tube[ 1:] - for entry in bloat_tube: - time = entry[0] - data = entry[1:] - if len(star_tube) > 0: - if star_tube[-1][0] == time: - star_tube[-1][1] = StarSet.rect_to_star(data,star_tube[-1][1]) - else: - if not isinstance(star_tube[-1][1], StarSet): - star_tube[-1][1] = StarSet.rect_to_star(star_tube[-1][1], star_tube[-1][1]) - star_tube.append([time,data]) - else: - star_tube.append([time,data]) + for i in range(0,len(bloat_tube),2): + # time = entry[0] + # data = entry[1:] + # if len(star_tube) > 0: + # if star_tube[-1][0] == time: + # star_tube[-1][1] = StarSet.rect_to_star(data,star_tube[-1][1]) + # else: + # if not isinstance(star_tube[-1][1], StarSet): + # star_tube[-1][1] = StarSet.rect_to_star(star_tube[-1][1], star_tube[-1][1]) + # star_tube.append([time,data]) + # else: + # star_tube.append([time,data]) + entry1 = bloat_tube[i] + entry2 = bloat_tube[i+1] + + time = entry1[0] + data1 = entry1[1:] + data2 = entry2[1:] + + star = StarSet.rect_to_star(data1, data2) + star_tube.append([time,star]) #KB: TODO: where is min for last time point and first time point - star_tube.pop() + # star_tube.pop() return star_tube - + else: reach = gen_starsets_post_sim(self, sim_func, time_horizon, time_step, mode_label=mode_label) star_tube = [] diff --git a/verse/utils/star_diams.py b/verse/utils/star_diams.py new file mode 100644 index 00000000..b6dbd1ce --- /dev/null +++ b/verse/utils/star_diams.py @@ -0,0 +1,90 @@ +import numpy as np +from verse.stars.starset import StarSet, HalfSpace +from typing_extensions import List, Callable, Dict +from scipy.integrate import ode +import matplotlib.pyplot as plt +from matplotlib.patches import Ellipse +from z3 import * +from verse.utils.star_manhattan import * + + +def over_approx_rectangle(trace, time_horizon, time_step): + time_steps = np.linspace(0, time_horizon, int(time_horizon/time_step) + 1, endpoint= True) + for i in range(len(time_steps) - 1): + reach_tubes = reach_star(trace, time_steps[i] - time_step*.05, time_steps[i+1] + time_step*.05) + agents = list(reach_tubes.keys()) + for agent in agents: + if reach_tubes[agent] is not None: + if len(reach_tubes[agent]) > 0: + star = reach_tubes[agent][0] + rect = [time_steps[i], star.overapprox_rectangle()] + print(rect) + + +def manhattan(a, b): + return sum(abs(val1-val2) for val1, val2 in zip(a,b)) + +#using Alex's reach_at_fix function +def time_step_diameter(trace, time_horizon, time_step): + time_steps = np.linspace(0, time_horizon, int(time_horizon/time_step) + 1, endpoint= True) + time_steps = np.append(time_steps, [time_steps[-1] + time_step]) + diameters = [] + #breakpoint() + for i in range(len(time_steps) - 1): + #breakpoint() + curr_diam = [] + reach_tubes = reach_star(trace, time_steps[i] - time_step*.1, time_steps[i+1] - time_step*.1) + agents = list(reach_tubes.keys()) + #breakpoint() + for agent in agents: + if reach_tubes[agent] is not None: + if len(reach_tubes[agent]) > 0: + for j in range(0, len(reach_tubes[agent])): + star = reach_tubes[agent][j] + diam, result = star_manhattan_distance(star.center, star.basis, star.C, star.g) + if(result == 1): + curr_diam.append(diam) + else: + rect = star.overapprox_rectangle() + curr_diam.append(manhattan(rect[0], rect[1])) + # breakpoint() + # print(time_steps[i]) + # print(time_steps[i+1]) + # print(reach_tubes) + # print(star.overapprox_rectangle()) + if len(curr_diam) > 0: + #print(curr_diam) + diameters.append(max(curr_diam)) + #breakpoint() + #print({"timesteps": time_steps[i]}) + + #print(diameters) + return diameters + +### assuming mode is not a parameter +def reach_star(traces, t_l: float = 0, t_u: float = None) -> Dict[str, List[StarSet]]: + reach = {} + + nodes = traces.nodes + agents = list(nodes[0].trace.keys()) + + # if t_u is None: + # for agent in trace: + # t_u = trace[agent][-1][0] # T + # break + + for node in nodes: + trace = node.trace + for agent in agents: + length = len(trace[agent]) + for i in range(length): + cur = trace[agent][i] + if cur[0]t_u: + break + if agent not in reach: + reach[agent] = [] + reach[agent].append(cur[1]) # just store the star set + + return reach \ No newline at end of file