def standard_2lane_road(): # lanes left_lane = lane.StraightLane([0., -1.], [0., 1.], constants.LANE_WIDTH_VIS) right_lane = left_lane.shifted(-1) lanes = [left_lane, right_lane] # roads roads = [left_lane] # fences (road boundaries) right_fence = lane.Fence([0., -1.], [0., 1.], constants.LANE_WIDTH_VIS, side=1) left_fence = lane.Fence([0., -1.], [0., 1.], constants.LANE_WIDTH_VIS, side=-1) fences = [right_fence.shifted(-1.5), left_fence.shifted(0.5)] return lanes, roads, fences
def world_highway_truck_cut_in(initial_states='truck_cut_in_far_overtaking', interaction_data=None, init_planner=True): # If very long horizon, increase bracket depth in Theano compilation to avoid # "fatal error: bracket nesting level exceeded maximum of 256." if config.HORIZON > 10: th.config.gcc.cxxflags = '-fbracket-depth=1024' # lanes left_lane = lane.StraightLane([0., -1.], [0., 1.], constants.LANE_WIDTH_VIS) right_lane = left_lane.shifted(-1) lanes = [left_lane, right_lane] # roads roads = [left_lane] # fences (road boundaries) right_fence = lane.Fence([0., -1.], [0., 1.], constants.LANE_WIDTH_VIS, side=1) left_fence = lane.Fence([0., -1.], [0., 1.], constants.LANE_WIDTH_VIS, side=-1) fences = [right_fence.shifted(-1.5), left_fence.shifted(0.5)] # dynamics dyn = dynamics.CarDynamics # asynchronous setting config.ASYNCHRONOUS = False # MATLAB file setup fine_behind_h = config.FINE_BEHIND_H fine_behind_r = config.FINE_BEHIND_R human_beta = config.HUMAN_BETA strat_val_data_dir = config.TRUCK_CUT_IN_STRATEGIC_VALUE_DATA_DIR if fine_behind_h == True: if human_beta is None: print('Not using strategic value.') elif human_beta == float('inf'): # then rational human (= deterministic). mat_name = strat_val_data_dir + 'DSG_fine_det.mat' else: beta_str = '%.2E' % Decimal(config.HUMAN_BETA) mat_name = strat_val_data_dir + 'beta_{0}/DSG_fine_beta_{0}.mat'.format( beta_str) else: if human_beta is None: print('Not using strategic value.') elif human_beta == float('inf'): # then rational human (= deterministic). mat_name = strat_val_data_dir + 'DSG_not_fine_det.mat' else: beta_str = '%.2E' % Decimal(config.HUMAN_BETA) mat_name = strat_val_data_dir + 'beta_{0}/DSG_fine_beta_{0}.mat'.format( beta_str) # Strategic state projection (use strategic grid dimension to get the correct # projection) proj = eval('projection.ProjectionTruckCutInStrategicValue{0}D'.format(config.STRAT_DIM))() # Initial states x0_r, x0_h, x0_t = get_initial_states(initial_states) # Robot car setup #ref_speed_r = constants.METERS_TO_VIS * 35.0 ref_speed_r = constants.METERS_TO_VIS * 34.0 # speed for gap creation if config.ROBOT_CAR == 'car.HierarchicalCar': robot_car = car.HierarchicalCar(x0_r, constants.DT, dyn, constants.CAR_CONTROL_BOUNDS, horizon=config.HORIZON, color=constants.COLOR_R, name=constants.NAME_R, mat_name=mat_name, use_second_order=config.USE_SECOND_ORDER, proj=proj, strat_dim=config.STRAT_DIM) elif config.ROBOT_CAR == 'car.NestedCar': robot_car = car.NestedCar(x0_r, constants.DT, dyn, constants.CAR_CONTROL_BOUNDS, horizon=config.HORIZON, color=constants.COLOR_R, name=constants.NAME_R, use_second_order=config.USE_SECOND_ORDER) elif config.ROBOT_CAR == 'car.PredictReactCar': robot_car = car.PredictReactCar(x0_r, constants.DT, dyn, constants.CAR_CONTROL_BOUNDS, horizon=config.HORIZON, color=constants.COLOR_R, name=constants.NAME_R) elif config.ROBOT_CAR == 'car.IteratedBestResponseCar': robot_car = car.IteratedBestResponseCar(x0_r, constants.DT, dyn, constants.CAR_CONTROL_BOUNDS, horizon=config.HORIZON, color=constants.COLOR_R, name=constants.NAME_R) else: print('"{0}" is currently an unsupported robot car type'.format(config.ROBOT_CAR)) sys.exit() # Human car setup ref_speed_h = constants.METERS_TO_VIS * 31. # x0_h[3] human_car = eval(config.HUMAN_CAR)(x0_h, constants.DT, dyn, constants.CAR_CONTROL_BOUNDS, horizon=config.HORIZON, color=constants.COLOR_H, name=constants.NAME_H) # Truck setup truck = car.Truck(x0_t, constants.DT, dyn, constants.CAR_CONTROL_BOUNDS, horizon=config.HORIZON, color=constants.COLOR_TRUCK, name=constants.NAME_TRUCK) # Information structure robot_car.human = human_car # robot creates its own traj for human if human_car.is_follower: # give follower car access to the robot car human_car.robot = robot_car human_car.traj_r = robot_car.traj # human knows the robot traj robot_car.truck = truck human_car.truck = truck # world setup cars = [robot_car, human_car, truck] name = 'world_highway_truck_cut_in' world = World(name, constants.DT, cars, robot_car, human_car, lanes, roads, fences, interaction_data=interaction_data) # rewards w_lanes = [4., 2.] w_control = -0.1 w_bounded_control_h = -50.0 # bounded control weight for human w_bounded_control_r = -50.0 # bounded control weight for robot # Rewards and strategic value modeled by the robot # (for both human and robot, respectively) if config.R_BELIEF_H_KNOWS_TRAJ_R: # robot believes human knows robot trajectory robot_r_h_traj = robot_car.traj else: # robot believes human doesn't know robot trajectory robot_r_h_traj = robot_car.traj_linear robot_r_h = reward.Reward(world, [robot_r_h_traj], other_truck_trajs=[truck.traj], w_lanes=w_lanes, w_control=w_control, w_bounded_control=w_bounded_control_h, speed=ref_speed_h, fine_behind=fine_behind_h, is_human=True)#, # strategic_value_mat_name=mat_name, robot_car=robot_car, # proj_np=proj_np, proj_th=proj_th) robot_r_r = reward.Reward(world, [robot_car.traj_h], other_truck_trajs=[truck.traj], w_lanes=w_lanes, w_control=w_control, w_bounded_control=w_bounded_control_r, speed=ref_speed_r, fine_behind=fine_behind_r)#, # strategic_value_mat_name=mat_name, robot_car=robot_car, # proj_np=proj_np, proj_th=proj_th) if config.PREDICT_HUMAN_IGNORES_ROBOT: # Reward for a human that ignores the existence of the robot. robot_r_h = reward.Reward(world, other_car_trajs=[], w_lanes=w_lanes, w_control=w_control, w_bounded_control=w_bounded_control_h, speed=ref_speed_h, fine_behind=fine_behind_h, is_human=True) robot_car.reward = robot_r_r robot_car.reward_h = robot_r_h if config.ROBOT_CAR == 'car.HierarchicalCar': # Robot's model of the human strategic value robot_strat_val_h = StrategicValue(robot_car.traj, robot_car.traj_h, proj, mat_name, config.STRAT_DIM, config.STRATEGIC_VALUE_SCALE, min_val=config.MIN_STRAT_VAL, max_val=config.MAX_STRAT_VAL, traj_truck=truck.traj) # Robot's strategic value # TODO: just trying out human_car.traj to debug heatmap vis # robot_strat_val = StrategicValue(robot_car.traj, human_car.traj, # proj, mat_name, config.STRAT_DIM, config.STRATEGIC_VALUE_SCALE, # min_val=config.MIN_STRAT_VAL, max_val=config.MAX_STRAT_VAL) robot_strat_val = StrategicValue(robot_car.traj, robot_car.traj_h, proj, mat_name, config.STRAT_DIM, config.STRATEGIC_VALUE_SCALE, min_val=config.MIN_STRAT_VAL, max_val=config.MAX_STRAT_VAL, traj_truck=truck.traj) robot_car.strat_val = robot_strat_val robot_car.strat_val_h = robot_strat_val_h # Rewards and strategic value modeled by the human WHEN SIMULATED # (for both human and robot, respectively) human_r_h = reward.Reward(world, [human_car.traj_r], other_truck_trajs=[truck.traj], w_lanes=w_lanes, w_control=w_control, w_bounded_control=w_bounded_control_h, speed=ref_speed_h, fine_behind=fine_behind_h, is_human=True)#, # strategic_value_mat_name=mat_name, robot_car=robot_car, # proj_np=proj_np, proj_th=proj_th) human_r_r = reward.Reward(world, [human_car.traj], other_truck_trajs=[truck.traj], w_lanes=w_lanes, w_control=w_control, w_bounded_control=w_bounded_control_r, speed=ref_speed_r, fine_behind=fine_behind_r)#, # strategic_value_mat_name=mat_name, robot_car=robot_car, # proj_np=proj_np, proj_th=proj_th) if config.HUMAN_IGNORES_ROBOT: human_r_h = reward.Reward(world, [], other_truck_trajs=[truck.traj], w_lanes=w_lanes, w_control=w_control, w_bounded_control=w_bounded_control_h, speed=ref_speed_h, fine_behind=fine_behind_h, is_human=True)#, human_car.reward = human_r_h human_car.reward_r = human_r_r if config.ROBOT_CAR == 'car.HierarchicalCar': # Human's strategic value human_strat_val = StrategicValue(human_car.traj_r, human_car.traj, proj, mat_name, config.STRAT_DIM, config.STRATEGIC_VALUE_SCALE, min_val=config.MIN_STRAT_VAL, max_val=config.MAX_STRAT_VAL, traj_truck=truck.traj) # Human's model of the robot strategic value human_strat_val_r = StrategicValue(human_car.traj_r, human_car.traj, proj, mat_name, config.STRAT_DIM, config.STRATEGIC_VALUE_SCALE, min_val=config.MIN_STRAT_VAL, max_val=config.MAX_STRAT_VAL, traj_truck=truck.traj) human_car.strat_val = human_strat_val human_car.strat_val_r = human_strat_val_r # set min and max strategic values config.MIN_STRAT_VAL = config.STRATEGIC_VALUE_SCALE * min( [min(robot_strat_val_h.vH_grid.flatten()), min(robot_strat_val_h.vR_grid.flatten()), min(robot_strat_val.vH_grid.flatten()), min(robot_strat_val.vR_grid.flatten()), min(human_strat_val.vH_grid.flatten()), min(human_strat_val.vR_grid.flatten()), min(human_strat_val_r.vH_grid.flatten()), min(human_strat_val_r.vR_grid.flatten())]) config.MAX_STRAT_VAL = config.STRATEGIC_VALUE_SCALE * max( [max(robot_strat_val_h.vH_grid.flatten()), max(robot_strat_val_h.vR_grid.flatten()), max(robot_strat_val.vH_grid.flatten()), max(robot_strat_val.vR_grid.flatten()), max(human_strat_val.vH_grid.flatten()), max(human_strat_val.vR_grid.flatten()), max(human_strat_val_r.vH_grid.flatten()), max(human_strat_val_r.vR_grid.flatten())]) # print the configuration pp = pprint.PrettyPrinter(indent=2) pp.pprint(world.get_config()) # initialize planners if init_planner: print('Initializing planners.') for c in world.cars: if hasattr(c, 'init_planner'): print 'Initializing planner for ' + c.name c.init_planner(config.INIT_PLAN_SCHEME[c.name]) print '\n' return world