class Omega2dSimulator(arcade.Window):
def __init__(self, sys_dynamics_func, config_file):
# local storage of sys dynamics
self.sys_dynamics = sys_dynamics_func
# load the configurations
print('Loading the configuration from ' + config_file + ' ...')
self.load_configs(config_file)
# create a symbolic model if needed
if(self.use_model_dump):
print('Loading the symbolic model from ' + self.model_dump_file + ' ...')
self.sym_model = SymbolicModel(self.model_dump_file, self.qnt_x.get_num_symbols(), self.qnt_u.get_num_symbols())
# create the controller object
print('Loading the controller from ' + self.controller_file + ' ...')
self.controller = Controller(self.controller_file)
# initialize the arcade thing
print('Initializing the Arcade simulation ...')
super().__init__(self.SCREEN_WIDTH, self.SCREEN_HEIGHT, "Omega2dSimulator: " + self.title)
# crete the system sprite
self.system = arcade.Sprite(self.model_image, self.model_image_scale)
self.last_action = [0.0]*len(self.x_lb)
self.last_action_symbol = -1
# set system's initial state in arrena
x_0_arena = self.translate_sys_to_arena(self.x_0)
self.system.center_x = x_0_arena[0]
self.system.center_y = x_0_arena[1]
if len(self.x_lb) == 3 and self.visualize_3rd_dim:
self.system.angle = x_0_arena[2]
# prepare for simulation
self.sys_state = self.x_0
self.sys_status = "stopped"
self.initial_delay = 100
self.time_elapsed = 0.0
self.avg_delta = 0.0
self.total_sim_time = 0.0
arcade.set_background_color(arcade.color.WHITE)
def load_configs(self, config_file):
# load some values form the config file
self.config_reader = ConfigReader(config_file)
self.x_lb = str2list(self.config_reader.get_value_string("system.states.first_symbol"))
self.x_ub = str2list(self.config_reader.get_value_string("system.states.last_symbol"))
self.x_eta = str2list(self.config_reader.get_value_string("system.states.quantizers"))
self.u_lb = str2list(self.config_reader.get_value_string("system.controls.first_symbol"))
self.u_ub = str2list(self.config_reader.get_value_string("system.controls.last_symbol"))
self.u_eta = str2list(self.config_reader.get_value_string("system.controls.quantizers"))
self.specs_formula = self.config_reader.get_value_string("specifications.ltl_formula")
self.X_initial = self.config_reader.get_value_string("system.states.initial_set")
self.X_initial_HR = str2hyperrects(self.X_initial)[0]
self.SCREEN_WIDTH = int(self.config_reader.get_value_string("simulation.window_width"))
self.SCREEN_HEIGHT = int(self.config_reader.get_value_string("simulation.window_height"))
self.title = self.config_reader.get_value_string("simulation.widow_title")
self.step_time = float(self.config_reader.get_value_string("simulation.step_time"))
self.skip_aps = self.config_reader.get_value_string("simulation.skip_APs").replace(" ", "").split(",")
self.visualize_3rd_dim = ( "true" == self.config_reader.get_value_string("simulation.visualize_3rdDim"))
self.model_image = self.config_reader.get_value_string("simulation.system_image")
self.model_image_scale = float(self.config_reader.get_value_string("simulation.system_image_scale"))
self.controller_file = self.config_reader.get_value_string("simulation.controller_file")
self.use_ODE = ( "true" == self.config_reader.get_value_string("simulation.use_ode"))
self.model_dump_file = self.config_reader.get_value_string("simulation.model_dump_file")
if(self.model_dump_file == "" or self.model_dump_file == None):
self.use_model_dump = False
else:
self.use_model_dump = True
self.x_0_str = self.config_reader.get_value_string("simulation.initial_state")
if self.x_0_str == "center":
self.x_0 = self.X_initial_HR.get_center_element()
elif self.x_0_str == "random":
self.x_0 = self.X_initial_HR.get_random_element()
else:
self.x_0 = str2list(self.x_0_str)
# create a quantizer and an ode solver
self.qnt_x = Quantizer(self.x_lb, self.x_eta, self.x_ub)
self.qnt_u = Quantizer(self.u_lb, self.u_eta, self.u_ub)
if self.use_ODE:
self.ode = RungeKuttaSolver(self.sys_dynamics, 5)
# configs for the arena
self.PADDING = 40
self.ZERO_BASE_X = self.PADDING
self.ZERO_BASE_Y = self.PADDING
self.X_GRID = (self.SCREEN_WIDTH-2*self.PADDING)/self.qnt_x.get_widths()[0]
self.Y_GRID = (self.SCREEN_HEIGHT-2*self.PADDING)/self.qnt_x.get_widths()[1]
self.ARENA_WIDTH = (self.qnt_x.get_widths()[0])*self.X_GRID
self.ARENA_HIGHT = (self.qnt_x.get_widths()[1])*self.Y_GRID
self.X_SCALE_FACTOR = self.ARENA_WIDTH/(self.x_ub[0] - self.x_lb[0] + self.x_eta[0])
self.Y_SCALE_FACTOR = self.ARENA_HIGHT/(self.x_ub[1] - self.x_lb[1] + self.x_eta[1])
self.Z_SCALE_FACTOR = 180.0/math.pi # from rad to deg
self.arena_mdl_lb = self.translate_sys_to_arena(self.x_lb)
self.arena_mdl_ub = self.translate_sys_to_arena(self.x_ub)
# others: subsets
self.subset_names = self.config_reader.get_value_string("system.states.subsets.names").replace(" ","").split(",")
self.subset_HRs = []
for subset_name in self.subset_names:
skip = False
for skip_ap in self.skip_aps:
if skip_ap == subset_name:
skip = True
if skip:
continue
subset_str = self.config_reader.get_value_string("system.states.subsets.mapping_" + subset_name)
if subset_str == '':
continue
HRs = str2hyperrects(subset_str)
HRs_modified = []
for HR in HRs:
conc_lb = HR.get_lb()
conc_ub = HR.get_ub()
for i in range(len(self.x_lb)):
if conc_lb[i] < self.x_lb[i]:
conc_lb[i] = self.x_lb[i] - self.x_eta[i]/2
if conc_ub[i] > self.x_ub[i]:
conc_ub[i] = self.x_ub[i] + self.x_eta[i]/2
lb = self.translate_sys_to_arena(conc_lb)
ub = self.translate_sys_to_arena(conc_ub)
HR.set_lb(lb)
HR.set_ub(ub)
HRs_modified.append(HR)
self.subset_HRs.append(HRs_modified)
def draw_subsets(self):
idx = 0
for subset_hyperrects in self.subset_HRs:
for HR in subset_hyperrects:
lb = HR.get_lb()
ub = HR.get_ub()
width = ub[0] - lb[0]
hight = ub[1] - lb[1]
arcade.draw_rectangle_filled(lb[0] + width/2, lb[1] + hight/2, width, hight, COLORS[idx % len(COLORS)])
arcade.draw_text(self.subset_names[idx], lb[0] + 5, lb[1] + 5, arcade.color.BLACK, 16)
idx += 1
pass
def draw_arena(self):
# Draw the background
arcade.draw_rectangle_filled(self.ZERO_BASE_X + self.ARENA_WIDTH/2, self.ZERO_BASE_Y + self.ARENA_HIGHT/2, self.ARENA_WIDTH, self.ARENA_HIGHT, arcade.color.LIGHT_GRAY)
# Draw the subsets
self.draw_subsets()
# draw grid
num_x_lines = self.qnt_x.get_widths()[0] + 1
for i in range(num_x_lines):
arcade.draw_line(self.ZERO_BASE_X+i*self.X_GRID, self.ZERO_BASE_Y, self.ZERO_BASE_X+i*self.X_GRID, self.ZERO_BASE_Y+self.ARENA_HIGHT, arcade.color.BLACK)
num_y_lines = self.qnt_x.get_widths()[1] + 1
for i in range(num_y_lines):
arcade.draw_line(self.ZERO_BASE_X, self.ZERO_BASE_Y+i*self.Y_GRID, self.ZERO_BASE_X+self.ARENA_WIDTH, self.ZERO_BASE_Y+i*self.Y_GRID, arcade.color.BLACK)
# info
arcade.draw_text("Spec.: " + self.specs_formula, self.ZERO_BASE_X, self.ZERO_BASE_Y + self.ARENA_HIGHT + 15, arcade.color.BLACK, 12)
# draw lb/ub markers
arcade.draw_rectangle_filled(self.arena_mdl_lb[0], self.arena_mdl_lb[1], 5, 5, arcade.color.BLUE)
arcade.draw_rectangle_filled(self.arena_mdl_ub[0], self.arena_mdl_ub[1], 5, 5, arcade.color.BLUE)
arcade.draw_text("x_first", self.arena_mdl_lb[0] - 15, self.arena_mdl_lb[1] - 17, arcade.color.BLACK, 14)
arcade.draw_text("x_last", self.arena_mdl_ub[0] - 15, self.arena_mdl_ub[1] - 17, arcade.color.BLACK, 14)
def start(self):
arcade.run()
def get_current_symbol(self):
return self.qnt_x.conc_to_flat(self.sys_state)
def print_info(self):
txt = "Status: " + self.sys_status
txt += " | Time (sec.): " + str(round(self.time_elapsed))
txt += " | FPS: " + str(round(1/self.avg_delta))
txt += "\nState: x_" + str(self.get_current_symbol()) + " = (" + list2str(self.sys_state) + ")"
txt += " | Control action: "
if self.last_action_symbol == -1:
txt += "not_issued"
else:
txt += "u_" + str(self.last_action_symbol) + " (" + list2str(self.last_action) + ")"
arcade.draw_text(txt, self.ZERO_BASE_X, self.ZERO_BASE_Y - 35, arcade.color.BLACK, 10)
def on_draw(self):
arcade.start_render()
self.draw_arena()
self.system.draw()
self.print_info()
def translate_sys_to_arena(self, state):
arena_x = self.ZERO_BASE_X + (state[0] - self.x_lb[0] + self.x_eta[0]/2)*self.X_SCALE_FACTOR
arena_y = self.ZERO_BASE_Y + (state[1] - self.x_lb[1] + self.x_eta[1]/2)*self.Y_SCALE_FACTOR
if len(state) == 3:
arena_t = state[2]*self.Z_SCALE_FACTOR
return [arena_x, arena_y, arena_t]
else:
return [arena_x, arena_y]
def update_system(self):
if self.sys_status == "stopped":
sym_state = self.get_current_symbol()
actions_list = self.controller.get_control_actions(sym_state)
self.last_action_symbol = actions_list[0]
self.last_action = self.qnt_u.flat_to_conc(self.last_action_symbol)
self.sub_steps = 0
self.sys_state_step_0 = self.sys_state[:]
self.steps_in_tau = math.floor(self.step_time/self.avg_delta) - 1
self.sys_status = "moving"
# solve the ode for one delta
if self.use_ODE:
sim_time = self.avg_delta
if self.sub_steps == 0:
self.total_sim_time = 0.0
left_time = self.step_time - self.steps_in_tau*self.avg_delta
sim_time += left_time
if self.sub_steps == self.steps_in_tau:
sim_time = self.step_time - self.total_sim_time
if sim_time > 0:
self.sys_state = self.ode.RK4(self.sys_state, self.last_action, sim_time)
self.total_sim_time += sim_time
else:
if self.sub_steps == 0:
self.sys_state = self.sys_dynamics(self.sys_state, self.last_action)
# increment the sub-step
self.sub_steps += 1
# check the post state against the one stored in the model
if self.use_model_dump:
if self.last_action_symbol != -1 and self.sub_steps >= self.steps_in_tau:
x_flat = self.qnt_x.conc_to_flat(self.sys_state_step_0)
u_flat = self.last_action_symbol
x_post_HR = self.sym_model.get_HR(x_flat, u_flat)
if not x_post_HR.is_element(self.sys_state):
print("Warning: Simulation is unstable due to variations in CPU load causing FPS to affect exact step-time OR the supplied dynamics does not conform with the constructed symbolic model (please double-check!) for x_flat=" + str(x_flat) + ", u_flat=" + str(u_flat) + ". Dynamics report x_post=" + str(self.sys_state) + " which is not inside the post_HR=(lb:" + str(x_post_HR.get_lb()) + ",ub:" + str(x_post_HR.get_ub()) + "). As a repair measure, x_post will be replaced with a value inside post_HR.")
self.sys_state = x_post_HR.get_center_element()
# set state
state_arena = self.translate_sys_to_arena(self.sys_state)
self.system.center_x = state_arena[0]
self.system.center_y = state_arena[1]
if len(self.sys_state) == 3 and self.visualize_3rd_dim:
self.system.angle = state_arena[2]
# if number of delta_steps_in_tau is reached, time to stop
# and wait for new input
if self.sub_steps >= self.steps_in_tau:
self.sys_status = "stopped"
def update(self, delta_time):
if self.avg_delta == 0.0 or self.initial_delay > 0:
if self.avg_delta == 0.0:
self.avg_delta = delta_time
self.initial_delay -= 1
else:
self.update_system()
self.time_elapsed += delta_time
self.avg_delta = (self.avg_delta + delta_time)/2.0
