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_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_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_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_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 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 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_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 test_draw_agents(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) ]) shape2 = CarLimousine() init_state = [0, 3, 2, 1] init_state2 = [0, 0, 5, 4] agent = Agent(init_state, behavior, dynamic, execution, shape, params.AddChild("agent")) agent2 = Agent(init_state2, behavior, dynamic, execution, shape2, params.AddChild("agent"))
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 test_one_agent_at_goal_state_limits_frenet(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() agent_params = param_server.AddChild("agent1") center_line = Line2d() center_line.AddPoint(Point2d(5.0, 5.0)) center_line.AddPoint(Point2d(10.0, 10.0)) center_line.AddPoint(Point2d(20.0, 10.0)) max_lateral_dist = (0.4, 1) max_orientation_diff = (0.08, 0.1) velocity_range = (20.0, 25.0) goal_definition = GoalDefinitionStateLimitsFrenet( center_line, max_lateral_dist, max_orientation_diff, velocity_range) # not at goal x,y, others yes agent1 = Agent(np.array([0, 6, 8, 3.14 / 4.0, velocity_range[0]]), behavior_model, dynamic_model, execution_model, agent_2d_shape, agent_params, goal_definition, None) # at goal x,y and others agent2 = Agent(np.array([0, 5.0, 5.5, 3.14 / 4.0, velocity_range[1]]), behavior_model, dynamic_model, execution_model, agent_2d_shape, agent_params, goal_definition, None) # not at goal x,y,v yes but not orientation agent3 = Agent( np.array( [0, 5, 5.5, 3.14 / 4.0 + max_orientation_diff[1] + 0.001, 20]), behavior_model, dynamic_model, execution_model, agent_2d_shape, agent_params, goal_definition, None) # not at goal x,y, orientation but not v agent4 = Agent( np.array([ 0, 5, 4.5, 3.14 / 4 - max_orientation_diff[0], velocity_range[0] - 0.01 ]), behavior_model, dynamic_model, execution_model, agent_2d_shape, agent_params, goal_definition, None) # at goal x,y, at lateral limit agent5 = Agent( np.array([ 0, 15, 10 - max_lateral_dist[0] + 0.05, 0, velocity_range[1] ]), behavior_model, dynamic_model, execution_model, agent_2d_shape, agent_params, goal_definition, None) # not at goal x,y slightly out of lateral limit agent6 = Agent( np.array([ 0, 15, 10 + max_lateral_dist[0] + 0.05, 3.14 / 4 + max_orientation_diff[0], velocity_range[0] ]), behavior_model, dynamic_model, execution_model, agent_2d_shape, agent_params, goal_definition, None) # not at goal x,y,v yes but not orientation agent7 = Agent( np.array( [0, 5, 5.5, 3.14 / 4.0 - max_orientation_diff[0] - 0.001, 20]), behavior_model, dynamic_model, execution_model, agent_2d_shape, agent_params, goal_definition, None) world = World(param_server) world.AddAgent(agent1) world.AddAgent(agent2) world.AddAgent(agent3) world.AddAgent(agent4) world.AddAgent(agent5) world.AddAgent(agent6) world.AddAgent(agent7) evaluator1 = EvaluatorGoalReached(agent1.id) evaluator2 = EvaluatorGoalReached(agent2.id) evaluator3 = EvaluatorGoalReached(agent3.id) evaluator4 = EvaluatorGoalReached(agent4.id) evaluator5 = EvaluatorGoalReached(agent5.id) evaluator6 = EvaluatorGoalReached(agent6.id) evaluator7 = EvaluatorGoalReached(agent7.id) world.AddEvaluator("success1", evaluator1) world.AddEvaluator("success2", evaluator2) world.AddEvaluator("success3", evaluator3) world.AddEvaluator("success4", evaluator4) world.AddEvaluator("success5", evaluator5) world.AddEvaluator("success6", evaluator6) world.AddEvaluator("success7", evaluator7) info = world.Evaluate() self.assertEqual(info["success1"], False) self.assertEqual(info["success2"], True) self.assertEqual(info["success3"], False) self.assertEqual(info["success4"], False) self.assertEqual(info["success5"], True) self.assertEqual(info["success6"], False) self.assertEqual(info["success7"], False)
def run_configuration(argv): params = ParameterServer() # NOTE: Modify these paths to specify your preferred path for checkpoints and summaries # params["ML"]["BehaviorTFAAgents"]["CheckpointPath"] = "/Users/hart/Development/bark-ml/checkpoints/" # params["ML"]["TFARunner"]["SummaryPath"] = "/Users/hart/Development/bark-ml/checkpoints/" params["Visualization"]["Agents"]["Alpha"]["Other"] = 0.2 params["Visualization"]["Agents"]["Alpha"]["Controlled"] = 0.2 params["Visualization"]["Agents"]["Alpha"]["Controlled"] = 0.2 params["ML"]["VisualizeCfWorlds"] = False params["ML"]["VisualizeCfHeatmap"] = False # params["ML"]["ResultsFolder"] = "/Users/hart/Development/bark-ml/results/data/" # viewer = MPViewer( # params=params, # x_range=[-35, 35], # y_range=[-35, 35], # follow_agent_id=True) # create environment bp = ContinuousMergingBlueprint(params, num_scenarios=2500, random_seed=0) observer = GraphObserver(params=params) behavior_model_pool = [] for count, a in enumerate([-5., 0., 5.]): local_params = params.AddChild("local_"+str(count)) local_params["BehaviorConstantAcceleration"]["ConstAcceleration"] = a behavior = BehaviorConstantAcceleration(local_params) behavior_model_pool.append(behavior) env = CounterfactualRuntime( blueprint=bp, observer=observer, render=False, params=params, behavior_model_pool=behavior_model_pool) sac_agent = BehaviorGraphSACAgent(environment=env, observer=observer, 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._environment._max_col_rate = 0. runner.Run(num_episodes=1, render=True) elif FLAGS.mode == "evaluate": for cr in np.arange(0, 1, 0.1): runner._environment._max_col_rate = cr runner.Run(num_episodes=250, render=False, max_col_rate=cr) runner._environment._tracer.Save( params["ML"]["ResultsFolder"] + "evaluation_results_runtime.pckl") goal_reached = runner._tracer.success_rate runner._tracer.Save( params["ML"]["ResultsFolder"] + "evaluation_results_runner.pckl")