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