def write_scenario_parameter(self, config_idx, folder): benchmark_config = super().get_benchmark_result().get_benchmark_config(config_idx) if benchmark_config is not None: params = benchmark_config.scenario.json_params p = ParameterServer() p.ConvertToParam(params) p.Save(os.path.join(folder, "scenario_parameters.json"))
def test_database_multiprocessing_runner(self): dbs = DatabaseSerializer(test_scenarios=4, test_world_steps=5, num_serialize_scenarios=5) dbs.process("data/database1") local_release_filename = dbs.release(version="test") db = BenchmarkDatabase(database_root=local_release_filename) evaluators = {"success" : "EvaluatorGoalReached", "collision" : "EvaluatorCollisionEgoAgent", "max_steps": "EvaluatorStepCount"} terminal_when = {"collision" :lambda x: x, "max_steps": lambda x : x>2} params = ParameterServer() # only for evaluated agents not passed to scenario! behaviors_tested = {"IDM": BehaviorIDMClassic(params), "Const" : BehaviorConstantAcceleration(params)} benchmark_runner = BenchmarkRunnerMP(benchmark_database=db, evaluators=evaluators, terminal_when=terminal_when, behaviors=behaviors_tested, log_eval_avg_every=10) result = benchmark_runner.run() df = result.get_data_frame() print(df) self.assertEqual(len(df.index), 20) # 2 Behaviors * 5 Serialize Scenarios * 2 scenario sets params2 = ParameterServer() viewer = MPViewer( params=params2, x_range=[5060, 5160], y_range=[5070,5150], use_world_bounds=True) rst = benchmark_runner.run_benchmark_config(10, viewer=viewer)
def test_database_multiprocessing_history(self): dbs = DatabaseSerializer(test_scenarios=4, test_world_steps=5, num_serialize_scenarios=2) dbs.process("data/database1") local_release_filename = dbs.release(version="test") db = BenchmarkDatabase(database_root=local_release_filename) evaluators = {"success" : "EvaluatorGoalReached", "collision" : "EvaluatorCollisionEgoAgent", "max_steps": "EvaluatorStepCount"} terminal_when = {"collision" :lambda x: x, "max_steps": lambda x : x>2} params = ParameterServer() # only for evaluated agents not passed to scenario! behaviors_tested = {"IDM": BehaviorIDMClassic(params), "Const" : BehaviorConstantAcceleration(params)} benchmark_runner = BenchmarkRunnerMP(benchmark_database=db, evaluators=evaluators, terminal_when=terminal_when, behaviors=behaviors_tested, log_eval_avg_every=10) rst = benchmark_runner.run(maintain_history=True) self.assertEqual(len(rst.get_histories()), 2*2*2) rst = benchmark_runner.run_benchmark_config(3, viewer=None, maintain_history=True) scenario_history = rst.get_histories()[3] print(scenario_history) params = ParameterServer() viewer = MPViewer( params=params, x_range=[5060, 5160], y_range=[5070,5150], use_world_bounds=True) viewer.drawWorld(world=scenario_history[1].GetWorldState(), eval_agent_ids=scenario_history[1].eval_agent_ids) viewer.show(block=True)
def test_planning_time(self): param_server = ParameterServer() # Model Definition behavior_model = BehaviorConstantAcceleration(param_server) execution_model = ExecutionModelInterpolate(param_server) dynamic_model = SingleTrackModel(param_server) # Agent Definition agent_2d_shape = CarLimousine() init_state = np.array([0, -191.789,-50.1725, 3.14*3.0/4.0, 150/3.6]) agent_params = param_server.AddChild("agent1") goal_polygon = Polygon2d([0, 0, 0], [Point2d(-4,-4), Point2d(-4,4), Point2d(4,4), Point2d(4,-4)]) goal_polygon = goal_polygon.Translate(Point2d(-191.789,-50.1725)) agent = Agent(init_state, behavior_model, dynamic_model, execution_model, agent_2d_shape, agent_params, GoalDefinitionPolygon(goal_polygon), None) world = World(param_server) world.AddAgent(agent) evaluator = EvaluatorPlanningTime(agent.id) world.AddEvaluator("time", evaluator) info = world.Evaluate() self.assertEqual(info["time"], 0.0)
def test_one_agent_at_goal_state_limits(self): param_server = ParameterServer() # Model Definition behavior_model = BehaviorConstantVelocity(param_server) execution_model = ExecutionModelInterpolate(param_server) dynamic_model = SingleTrackModel(param_server) # Agent Definition agent_2d_shape = CarLimousine() init_state = np.array( [0, -191.789, -50.1725, 3.14 * 3.0 / 4.0, 150 / 3.6]) agent_params = param_server.AddChild("agent1") goal_polygon = Polygon2d( [0, 0, 0], [Point2d(-1, -1), Point2d(-1, 1), Point2d(1, 1), Point2d(1, -1)]) goal_polygon = goal_polygon.Translate(Point2d(-191.789, -50.1725)) agent = Agent( init_state, behavior_model, dynamic_model, execution_model, agent_2d_shape, agent_params, GoalDefinitionStateLimits( goal_polygon, (3.14 * 3.0 / 4.0 - 0.08, 3.14 * 3.0 / 4.0 + 0.08)), None) world = World(param_server) world.AddAgent(agent) evaluator = EvaluatorGoalReached(agent.id) world.AddEvaluator("success", evaluator) info = world.Evaluate() self.assertEqual(info["success"], True)
def test_parameters(self): # initialize Params p = ParameterServer() # set new parameter self.assertTrue(p["LetsTest"]["hierarchy", "bla", True]) # check whether right value is recovered tester = p["Car"]["Length", "Car Length", 6] tester2 = p["Car"]["Length", "Car Length", 8] self.assertEqual(tester, 6) self.assertEqual( tester2, 6) # value should not change, since key already exists in dict # check whether access without description and default value is possible self.assertEqual(p["Car"]["Length"], 6) # check whether setting values works p["Age"] = 24 self.assertEqual(p["Age"], 24) p["Localization"]["Number of Particles"] = 2000 self.assertEqual(p["Localization"]["Number of Particles"], 2000) # C++ Test in /bark/commons/Params/params_test.h # write in parameters in C++ and check whether they can be accessed in python afterwards #ParamsTest(p) #self.assertEqual(p["param_cpp"], 16.5) # add child in python child = p.AddChild("ch") self.assertTrue(child["ChildTest"]["hierarchy", "bla", True]) # write parameters to json file p.Save("written_a_param_test.json")
def test_number_of_agents(self): # World Definition params = ParameterServer() world = World(params) # Model Definitions behavior_model = BehaviorConstantAcceleration(params) execution_model = ExecutionModelInterpolate(params) dynamic_model = SingleTrackModel(params) behavior_model2 = BehaviorConstantAcceleration(params) execution_model2 = ExecutionModelInterpolate(params) dynamic_model2 = SingleTrackModel(params) # Map Definition map_interface = MapInterface() xodr_map = MakeXodrMapOneRoadTwoLanes() map_interface.SetOpenDriveMap(xodr_map) world.SetMap(map_interface) agent_2d_shape = CarLimousine() init_state = np.array([0, 13, -1.75, 0, 5]) agent_params = params.AddChild("agent1") goal_polygon = Polygon2d( [1, 1, 0], [Point2d(0, 0), Point2d(0, 2), Point2d(2, 2), Point2d(2, 0)]) goal_polygon = goal_polygon.Translate(Point2d(50, -2)) agent = Agent(init_state, behavior_model, dynamic_model, execution_model, agent_2d_shape, agent_params, GoalDefinitionPolygon(goal_polygon), map_interface) world.AddAgent(agent) init_state2 = np.array([0, 16, -1.75, 0, 5]) agent2 = Agent(init_state2, behavior_model2, dynamic_model2, execution_model2, agent_2d_shape, agent_params, GoalDefinitionPolygon(goal_polygon), map_interface) world.AddAgent(agent2) evaluator = EvaluatorNumberOfAgents(agent.id) world.AddEvaluator("num_agents", evaluator) info = world.Evaluate() self.assertEqual(info["num_agents"], len(world.agents)) # do it once more self.assertEqual(info["num_agents"], len(world.agents)) world.RemoveAgentById(agent2.id) info = world.Evaluate() # evaluator should still hold two self.assertNotEqual(info["num_agents"], len(world.agents)) self.assertEqual(info["num_agents"], 2) world.Step(0.1) info = world.Evaluate() # evaluator should still hold two self.assertEqual(info["num_agents"], 2)
def test_agent_pickle_uct_planner(self): try: from bark.core.models.behavior import BehaviorUCTSingleAgentMacroActions except: print("Rerun test with ---define planner_uct=true") return params = ParameterServer() behavior = BehaviorUCTSingleAgentMacroActions(params) execution = ExecutionModelInterpolate(params) dynamic = SingleTrackModel(params) shape = CarLimousine() init_state = np.array([0, 0, 0, 0, 5]) goal_polygon = Polygon2d( [0, 0, 0], [Point2d(-1, -1), Point2d(-1, 1), Point2d(1, 1), Point2d(1, -1)]) goal_definition = GoalDefinitionPolygon(goal_polygon) agent = Agent(init_state, behavior, dynamic, execution, shape, params.AddChild("agent"), goal_definition) agent_after = pickle_unpickle(agent) self.assertTrue( isinstance(agent_after.behavior_model, BehaviorUCTSingleAgentMacroActions))
def test_agent_pickle(self): params = ParameterServer() behavior = BehaviorConstantVelocity(params) execution = ExecutionModelInterpolate(params) dynamic = SingleTrackModel(params) shape = CarLimousine() init_state = np.array([0, 0, 0, 0, 5]) goal_polygon = Polygon2d([0, 0, 0],[Point2d(-1,-1),Point2d(-1,1),Point2d(1,1), Point2d(1,-1)]) goal_definition = GoalDefinitionPolygon(goal_polygon) agent = Agent(init_state, behavior, dynamic, execution, shape, params.AddChild("agent"), goal_definition ) agent_after = pickle_unpickle(agent) self.assertEqual(agent_after.id , agent.id) self.assertTrue(np.array_equal(agent_after.state, agent.state) ) self.assertTrue(np.array_equal(agent_after.goal_definition.goal_shape.center, \ agent.goal_definition.goal_shape.center)) goal_definition_2 = GoalDefinitionStateLimits(goal_polygon, (0.2 , 0.5)) agent2 = Agent(init_state, behavior, dynamic, execution, shape, params.AddChild("agent"), goal_definition_2) agent_after2 = pickle_unpickle(agent2) self.assertEqual(agent_after2.id , agent2.id) self.assertTrue(np.array_equal(agent_after2.state, agent.state) ) self.assertTrue(np.array_equal(agent_after2.goal_definition.xy_limits.center, \ agent2.goal_definition.xy_limits.center)) agent_list = [] agent_list.append(agent) agent_list_after = pickle_unpickle(agent_list) self.assertEqual(agent_list_after[0].id , agent.id) self.assertTrue(np.array_equal(agent_list_after[0].state, agent.state) )
def test_agent_from_trackfile_centered(self): map_filename = os.path.join( os.path.dirname(__file__), "../tests/data/DR_DEU_Merging_MT_v01_centered.xodr") track_filename = os.path.join( os.path.dirname(__file__), "../tests/data/interaction_dataset_dummy_track.csv") agent_track_info = AgentTrackInfo(track_filename, track_id=1, start_offset=500, end_offset=1000) params = ParameterServer() params_id = params["Scenario"]["Generation"]["InteractionDataset"] params_id["MapFilename", "", map_filename] params_id["TrackFilename", "", track_filename] params_id["BehaviorModel", "", {}] track_params = ParameterServer() track_params["execution_model"] = 'ExecutionModelInterpolate' track_params["dynamic_model"] = 'SingleTrackModel' track_params["map_interface"] = None # world.map track_params["behavior_model"] = None scenario_info = ScenarioTrackInfo(map_filename, track_filename, agent_track_info, xy_offset=[-900, -900]) ds_reader = InteractionDatasetReader() agent = ds_reader.AgentFromTrackfile(track_params, params, scenario_info, agent_track_info.GetTrackId())
def get_scenario_generator(self, scenario_set_id): serialized_file_name = self.dataframe.iloc[scenario_set_id]["Serialized"] if os.path.exists(serialized_file_name): serialized_file_path = serialized_file_name else: serialized_file_path = os.path.join( self.database_root, serialized_file_name) cwd = None if os.path.exists(self.database_root): # move into database root that map files can be found cwd = os.getcwd() os.chdir(self.database_root) param_file_name = self.dataframe.iloc[scenario_set_id]["Params"] if not param_file_name: logging.warning("No param file found for scenario set {}. Using defaults...".format( self.dataframe.iloc[scenario_set_id]["SetName"])) params = ParameterServer() else: params = ParameterServer(filename=param_file_name) scenario_generation = ScenarioGeneration(params=params) scenario_generation.load_scenario_list(filename=serialized_file_name) SetMapfileDirectory(self.database_root) if cwd: os.chdir(cwd) scenario_set_name = self.dataframe.iloc[scenario_set_id]["SetName"] scenario_set_parameters = self.dataframe.iloc[scenario_set_id]["SetParameters"] return scenario_generation, scenario_set_name, scenario_set_parameters
def generate_uct_hypothesis_behavior(): ml_params = ParameterServer(filename="configuration/params/iqn_params_demo_full.json", log_if_default=True) behavior_ml_params = ml_params["ML"]["BehaviorMPMacroActions"] mcts_params = ParameterServer(filename="configuration/params/default_uct_params.json", log_if_default=True) mcts_params["BehaviorUctBase"]["EgoBehavior"] = behavior_ml_params behavior = BehaviorUCTHypothesis(mcts_params, []) mcts_params.Save(filename="./default_uct_params.json") return behavior, mcts_params
def __setup_map_interface__(self): params = ParameterServer() # we are creating a dummy scenario to get the map interface from it scenario = Scenario(map_file_name=self._map_filename, json_params=params.ConvertToDict()) world = scenario.GetWorldState() map_interface = world.map return map_interface
def main(): args = configure_args() if is_local: dir_prefix = "" else: dir_prefix="hy-iqn-lfd-full-exp.runfiles/hythe/" logging.info(f"Executing job: {args.jobname}") logging.info(f"Experiment server at: {os.getcwd()}") params = ParameterServer(filename=os.path.join(dir_prefix, params_file), log_if_default=True) params = configure_params(params, seed=args.jobname) experiment_id = params["Experiment"]["random_seed"] params_filename = os.path.join(params["Experiment"]["dir"], "params_{}.json".format(experiment_id)) behavior = BehaviorDiscreteMacroActionsML(params) evaluator = GoalReached(params) observer = NearestAgentsObserver(params) viewer = MPViewer(params=params, x_range=[-35, 35], y_range=[-35, 35], follow_agent_id=True) # extract params and save experiment parameters params["ML"]["BaseAgent"]["SummaryPath"] = os.path.join(params["Experiment"]["dir"], "agent/summaries") params["ML"]["BaseAgent"]["CheckpointPath"] = os.path.join(params["Experiment"]["dir"], "agent/checkpoints") params.Save(filename=params_filename) logging.info('-' * 60) logging.info("Writing params to :{}".format(params_filename)) logging.info('-' * 60) # database creation dbs1 = DatabaseSerializer(test_scenarios=1, test_world_steps=2, num_serialize_scenarios=num_demo_scenarios) dbs1.process(os.path.join(dir_prefix, "configuration/database"), filter_sets="**/**/interaction_merging_light_dense_1D.json") local_release_filename = dbs1.release(version="lfd_offline") db1 = BenchmarkDatabase(database_root=local_release_filename) scenario_generator1, _, _ = db1.get_scenario_generator(0) env = HyDiscreteHighway(params=params, scenario_generation=scenario_generator1, behavior=behavior, evaluator=evaluator, observer=observer, viewer=viewer, render=False) scenario, _ = scenario_generator1.get_next_scenario() world = scenario.GetWorldState() observer.Reset(world) assert env.action_space._n == 8, "Action Space is incorrect!" run(args, params, env, db=db1) params.Save(params_filename) logging.info('-' * 60) logging.info("Writing params to :{}".format(params_filename)) logging.info('-' * 60)
def make_initial_world(primitives): # must be within examples params folder params = ParameterServer() world = World(params) # Define two behavior models behavior_model = BehaviorMPContinuousActions(params) primitive_mapping = {} for prim in primitives: idx = behavior_model.AddMotionPrimitive( np.array(prim)) # adding action primitive_mapping[idx] = prim behavior_model.ActionToBehavior(0) # setting initial action execution_model = ExecutionModelInterpolate(params) dynamic_model = SingleTrackModel(params) behavior_model2 = BehaviorConstantVelocity(params) execution_model2 = ExecutionModelInterpolate(params) dynamic_model2 = SingleTrackModel(params) # Define the map interface and load a testing map map_interface = MapInterface() xodr_map = MakeXodrMapOneRoadTwoLanes() map_interface.SetOpenDriveMap(xodr_map) world.SetMap(map_interface) # Define the agent shapes agent_2d_shape = CarRectangle() init_state = np.array([0, 3, -5.25, 0, 20]) # Define the goal definition for agents center_line = Line2d() center_line.AddPoint(Point2d(0.0, -1.75)) center_line.AddPoint(Point2d(100.0, -1.75)) max_lateral_dist = (0.4, 0.5) max_orientation_diff = (0.08, 0.1) velocity_range = (5.0, 20.0) goal_definition = GoalDefinitionStateLimitsFrenet(center_line, max_lateral_dist, max_orientation_diff, velocity_range) # define two agents with the different behavior models agent_params = params.AddChild("agent1") agent = Agent(init_state, behavior_model, dynamic_model, execution_model, agent_2d_shape, agent_params, goal_definition, map_interface) world.AddAgent(agent) init_state2 = np.array([0, 25, -5.25, 0, 15]) agent2 = Agent(init_state2, behavior_model2, dynamic_model2, execution_model2, agent_2d_shape, agent_params, goal_definition, map_interface) world.AddAgent(agent2) return world
def test_set_item_using_delimiter(self): params = ParameterServer() _ = params["test_child"]["Child2"]["ValueFloat", "Desc", 2.0] params["test_child::Child2::ValueFloat"] = 3.2323 self.assertEqual(params["test_child"]["Child2"]["ValueFloat"], 3.2323) child = params.AddChild("test_child5::Child5") child["test_param2"] = "etesd99533sbgfgf" self.assertEqual(params["test_child5"]["Child5"]["test_param2", "Desc", 0], "etesd99533sbgfgf")
def create_map_interface(map_file_name, road_ids): params = ParameterServer() # we are creating a dummy scenario to get the map interface from it scenario = Scenario(map_file_name=map_file_name, json_params=params.ConvertToDict()) world = scenario.GetWorldState() map_interface = world.map map_interface.GenerateRoadCorridor(road_ids, XodrDrivingDirection.forward) return map_interface
def test_database_runner(self): dbs = DatabaseSerializer(test_scenarios=4, test_world_steps=5, num_serialize_scenarios=2) dbs.process("data/database1") local_release_filename = dbs.release(version="test") db = BenchmarkDatabase(database_root=local_release_filename) safe_dist_params = ParameterServer(log_if_default=True) test_python_params = ParameterServer(log_if_default=True) evaluators = { "success": "EvaluatorGoalReached", "collision": "EvaluatorCollisionEgoAgent", "max_steps": "EvaluatorStepCount", "safe_dist_lon": { "type": "EvaluatorDynamicSafeDist", "params": safe_dist_params }, "safe_dist_lat": { "type": "EvaluatorStaticSafeDist", "params": safe_dist_params }, "test_evaluator": { "type": "TestPythonEvaluator", "params": test_python_params }, "test_evaluator_serializable": TestPythonEvaluatorSerializable() } terminal_when = { "collision": lambda x: x, "max_steps": lambda x: x > 2, "safe_dist_lon": lambda x: x } params = ParameterServer( ) # only for evaluated agents not passed to scenario! behaviors_tested = { "IDM": BehaviorIDMClassic(params), "Const": BehaviorConstantAcceleration(params) } EvaluationConfig.AddEvaluationModule( "bark.benchmark.tests.test_evaluator") benchmark_runner = BenchmarkRunner(benchmark_database=db, evaluators=evaluators, log_eval_avg_every=1, terminal_when=terminal_when, behaviors=behaviors_tested) result = benchmark_runner.run() df = result.get_data_frame() print(df) self.assertEqual( len(df.index), 2 * 2 * 2) # 2 Behaviors * 2 Serialize Scenarios * 1 scenario sets result.load_benchmark_configs() groups = result.get_evaluation_groups() self.assertEqual(set(groups), set(["behavior", "scen_set"]))
def test_one_agent_at_goal_sequential(self): param_server = ParameterServer() # Model Definition dynamic_model = SingleTrackModel(param_server) behavior_model = BehaviorMPContinuousActions(param_server) idx = behavior_model.AddMotionPrimitive(np.array([1, 0])) behavior_model.ActionToBehavior(idx) execution_model = ExecutionModelInterpolate(param_server) # Agent Definition agent_2d_shape = CarLimousine() init_state = np.array([0, 0, 0, 0, 0]) agent_params = param_server.AddChild("agent1") goal_frame = Polygon2d([0, 0, 0], [Point2d(-1,-1), Point2d(-1,1), Point2d(1,1), Point2d(1,-1)]) goal_polygon1 = goal_frame.Translate(Point2d(10, 0)) goal_polygon2 = goal_frame.Translate(Point2d(20, 0)) goal_polygon3 = goal_frame.Translate(Point2d(30, 0)) goal_def1 = GoalDefinitionStateLimits(goal_polygon1, [-0.08, 0.08]) goal_def2 = GoalDefinitionStateLimits(goal_polygon2, [-0.08, 0.08]) goal_def3 = GoalDefinitionStateLimits(goal_polygon3, [-0.08, 0.08]) goal_definition = GoalDefinitionSequential([goal_def1, goal_def2, goal_def3]) self.assertEqual(len(goal_definition.sequential_goals),3) agent = Agent(init_state, behavior_model, dynamic_model, execution_model, agent_2d_shape, agent_params, goal_definition, None) world = World(param_server) world.AddAgent(agent) evaluator = EvaluatorGoalReached(agent.id) world.AddEvaluator("success", evaluator) # just drive with the single motion primitive should be successful for _ in range(0,1000): world.Step(0.2) info = world.Evaluate() if info["success"]: break self.assertEqual(info["success"], True) self.assertAlmostEqual(agent.state[int(StateDefinition.X_POSITION)], 30, delta=0.5)
def test_belief_observer_observation_space(self): params = ParameterServer( filename="configuration/params/iqn_params.json") splits = 8 params_behavior = ParameterServer( filename="configuration/params/1D_desired_gap_no_prior.json") behavior_space = BehaviorSpace(params_behavior) hypothesis_set, hypothesis_params = behavior_space.create_hypothesis_set_fixed_split(split=splits) observer = BeliefObserver(params, hypothesis_set, splits=splits) max_num_agents = observer._max_num_vehicles assert max_num_agents * len(hypothesis_set) == observer.max_beliefs
def main(): map_filename = "external/bark_ml_project/bark_ml/environments/blueprints/highway/city_highway_straight.xodr" params_filename = glob.glob(os.path.join(demo_root, "params_[!behavior]*")) params = ParameterServer(filename=params_filename[0], log_if_default=True) behavior = BehaviorDiscreteMacroActionsML(params) evaluator = GoalReached(params) if is_belief_observer: splits = 2 bparams_filename = glob.glob(os.path.join(demo_root, "behavior_*")) params_behavior = ParameterServer(filename=bparams_filename[0]) behavior_space = BehaviorSpace(params_behavior) hypothesis_set, hypothesis_params = behavior_space.create_hypothesis_set_fixed_split( split=splits) observer = BeliefObserver(params, hypothesis_set, splits=splits) else: observer = NearestAgentsObserver(params) viewer = MPViewer(params=params, x_range=[-35, 35], y_range=[-35, 35], follow_agent_id=True) env = HyDiscreteHighway(params=params, map_filename=map_filename, behavior=behavior, evaluator=evaluator, observer=observer, viewer=viewer, render=False) env.reset() ego_world_states = [] _, demonstrations = unpack_load_demonstrations(demo_root) for demo in demonstrations: ego_state = np.zeros((observer._len_ego_state + 1)) (nn_ip_state, action, reward, next_state, done, is_demo) = demo ego_nn_input_state = deepcopy(nn_ip_state[0:observer._len_ego_state]) ego_state[1:] = ego_nn_input_state reverted_observed_state = observer.rev_observe_for_ego_vehicle( ego_state) ego_world_states.append( (reverted_observed_state[int(StateDefinition.X_POSITION)], reverted_observed_state[int(StateDefinition.Y_POSITION)], reverted_observed_state[int(StateDefinition.THETA_POSITION)], reverted_observed_state[int(StateDefinition.VEL_POSITION)], action, int(is_demo))) df = pd.DataFrame(ego_world_states, columns=[ 'pos_x', 'pos_y', 'orientation', 'velocity', 'action', 'is_demo' ]) print(df.head(10)) df.to_pickle(os.path.join(demo_root, "demonstrations/demo_dataframe")) return
def test_gap_distance_front(self): # World Definition params = ParameterServer() world = World(params) gap = 10 # Model Definitions behavior_model = BehaviorConstantAcceleration(params) execution_model = ExecutionModelInterpolate(params) dynamic_model = SingleTrackModel(params) behavior_model2 = BehaviorConstantAcceleration(params) execution_model2 = ExecutionModelInterpolate(params) dynamic_model2 = SingleTrackModel(params) # Map Definition map_interface = MapInterface() xodr_map = MakeXodrMapOneRoadTwoLanes() map_interface.SetOpenDriveMap(xodr_map) world.SetMap(map_interface) agent_2d_shape = CarLimousine() init_state = np.array([0, 13, -1.75, 0, 5]) agent_params = params.AddChild("agent1") goal_polygon = Polygon2d( [1, 1, 0], [Point2d(0, 0), Point2d(0, 2), Point2d(2, 2), Point2d(2, 0)]) goal_polygon = goal_polygon.Translate(Point2d(50, -2)) agent = Agent(init_state, behavior_model, dynamic_model, execution_model, agent_2d_shape, agent_params, GoalDefinitionPolygon(goal_polygon), map_interface) world.AddAgent(agent) init_state2 = np.array([0, 13 + gap, -1.75, 0, 5]) agent2 = Agent(init_state2, behavior_model2, dynamic_model2, execution_model2, agent_2d_shape, agent_params, GoalDefinitionPolygon(goal_polygon), map_interface) world.AddAgent(agent2) world.Step(0.1) evaluator = EvaluatorGapDistanceFront(agent.id) world.AddEvaluator("gap", evaluator) info = world.Evaluate() self.assertAlmostEqual(info["gap"], gap - agent_2d_shape.front_dist - agent_2d_shape.rear_dist, places=4)
def generate_uct_hypothesis_behavior(): if is_local: dir_prefix = "" else: dir_prefix="hy-iqn-lfd-full-beliefs-exp.runfiles/hythe/" ml_params = ParameterServer(filename=os.path.join(dir_prefix, params_file), log_if_default=True) behavior_ml_params = ml_params["ML"]["BehaviorMPMacroActions"] mcts_params = ParameterServer(filename=os.path.join(dir_prefix, "configuration/params/default_uct_params.json"), log_if_default=True) mcts_params["BehaviorUctBase"]["EgoBehavior"] = behavior_ml_params behavior = BehaviorUCTHypothesis(mcts_params, []) mcts_params.Save(filename="./default_uct_params.json") return behavior, mcts_params
def _process_scenario_list(self, database_dir, scenario_set_dict): serialized_sets_dir = os.path.join(database_dir, "scenario_sets") if not os.path.exists(serialized_sets_dir): os.makedirs(serialized_sets_dir) process_result = True for scenario_set, json_params in scenario_set_dict.items(): json_params["Scenario"]["Generation"]["SetName"] = scenario_set params = ParameterServer(json=json_params) filename = os.path.join(serialized_sets_dir, "{}.json".format(scenario_set)) print(filename) params.save(filename) process_result = process_result and \ self._process_json_paramfile(filename, json=json_params) return process_result
def test_pickle_belief_observer(self): params = ParameterServer() # init behavior space splits = 2 behavior_params = ParameterServer() behavior_space = BehaviorSpace(behavior_params) hypothesis_set, _ = behavior_space.create_hypothesis_set_fixed_split( split=splits) observer = BeliefObserver(params, hypothesis_set, splits=splits) po = pickle_unpickle(observer) self.assertIsNotNone(po) self.assertTrue(isinstance(po, BeliefObserver)) self.assertEqual(po.splits, observer.splits) self.assertEqual(po.len_beliefs, observer.len_beliefs)
def main(): args = configure_args() exp_dir = args.exp_dir or "results/training/toy_evaluation" params_filename = glob.glob(os.path.join(exp_dir, "params_*")) params = ParameterServer(filename=params_filename[0]) behavior_params_filename = glob.glob( os.path.join(exp_dir, "behavior_params*")) if behavior_params_filename: splits = 8 behavior_params = ParameterServer(filename=behavior_params_filename[0]) behavior_space = BehaviorSpace(behavior_params) hypothesis_set, _ = behavior_space.create_hypothesis_set_fixed_split( split=splits) observer = BeliefObserver(params, hypothesis_set, splits=splits) behavior = BehaviorDiscreteMacroActionsML(behavior_params) else: behavior = BehaviorDiscreteMacroActionsML(params) observer = NearestAgentsObserver(params) evaluator = GoalReached(params) scenario_params = ParameterServer( filename= "configuration/database/scenario_sets/interaction_merging_light_dense_1D.json" ) scenario_generator = ConfigurableScenarioGeneration(params=scenario_params, num_scenarios=5) scenario_file = glob.glob(os.path.join(exp_dir, "scenarios_list*")) scenario_generator.load_scenario_list(scenario_file[0]) viewer = MPViewer(params=params, x_range=[-35, 35], y_range=[-35, 35], follow_agent_id=True) env = HyDiscreteHighway(behavior=behavior, observer=observer, evaluator=evaluator, viewer=viewer, scenario_generation=scenario_generator, render=True) env.reset() actions = [0, 1, 2, 3, 4, 5, 6] for action in actions: print(action) env.step(action) agent = FQFAgent(env=env, test_env=env, params=params) agent.load_models(os.path.join(exp_dir, "agent/checkpoints/final"))
def test_execution_model_pickle(self): params = ParameterServer() e = ExecutionModelInterpolate(params) ea = pickle_unpickle(e) self.assertTrue(isinstance(ea,ExecutionModelInterpolate))
def test_behavior_model_pickle(self): params = ParameterServer() b = BehaviorConstantVelocity(params) ba = pickle_unpickle(b) self.assertTrue(isinstance(ba, BehaviorConstantVelocity))
def test_write_params_agent(self): params = ParameterServer() behavior = BehaviorConstantAcceleration(params) execution = ExecutionModelInterpolate(params) dynamic = SingleTrackModel(params) shape = Polygon2d([1.25, 1, 0], [ Point2d(0, 0), Point2d(0, 2), Point2d(4, 2), Point2d(4, 0), Point2d(0, 0) ]) init_state = np.zeros(4) agent = Agent(init_state, behavior, dynamic, execution, shape, params.AddChild("agent")) params.Save("written_agents_param_test.json")
def run_configuration(argv): # params = ParameterServer(filename="examples/example_params/tfa_params.json") params = ParameterServer() # NOTE: Modify these paths in order to save the checkpoints and summaries # params["ML"]["BehaviorTFAAgents"]["CheckpointPath"] = "YOUR_PATH" # params["ML"]["TFARunner"]["SummaryPath"] = "YOUR_PATH" params["World"]["remove_agents_out_of_map"] = True # create environment bp = ContinuousMergingBlueprint(params, number_of_senarios=2500, random_seed=0) env = SingleAgentRuntime(blueprint=bp, render=False) # PPO-agent # ppo_agent = BehaviorPPOAgent(environment=env, # params=params) # env.ml_behavior = ppo_agent # runner = PPORunner(params=params, # environment=env, # agent=ppo_agent) # SAC-agent sac_agent = BehaviorSACAgent(environment=env, params=params) env.ml_behavior = sac_agent runner = SACRunner(params=params, environment=env, agent=sac_agent) if FLAGS.mode == "train": runner.SetupSummaryWriter() runner.Train() elif FLAGS.mode == "visualize": runner.Run(num_episodes=10, render=True) elif FLAGS.mode == "evaluate": runner.Run(num_episodes=100, render=False)