def world6(know_model=True): dyn = dynamics.CarDynamics(0.1) world = World() clane = lane.StraightLane([0., -1.], [0., 1.], 0.13) world.lanes += [clane, clane.shifted(1), clane.shifted(-1)] world.roads += [clane] world.fences += [clane.shifted(2), clane.shifted(-2), clane.shifted(2.5), clane.shifted(-2.5)] world.cars.append(car.SimpleOptimizerCar(dyn, [-0.13, 0., math.pi/2., 0.5], color='red')) if know_model: world.cars.append(car.NestedOptimizerCar(dyn, [0., 0.05, math.pi/2., 0.5], color='yellow')) else: world.cars.append(car.SimpleOptimizerCar(dyn, [0., 0.05, math.pi/2., 0.5], color='yellow')) world.cars[0].reward = world.simple_reward(world.cars[0], speed=0.6) world.cars[0].default_u = np.asarray([0., 1.]) @feature.feature def goal(t, x, u): return -(10.*(x[0]+0.13)**2+0.5*(x[1]-2.)**2) if know_model: world.cars[1].human = world.cars[0] r_h = world.simple_reward([world.cars[1].traj], speed=0.6)+100.*feature.bounded_control(world.cars[0].bounds) r_r = 10*goal+world.simple_reward([world.cars[1].traj_h], speed=0.5) world.cars[1].rewards = (r_h, r_r) else: r = 10*goal+world.simple_reward([world.cars[0].linear], speed=0.5) world.cars[1].reward = r return world
def world9(): dyn = dynamics.CarDynamics(0.1) world = World() clane = lane.StraightLane([0., -1.], [0., 1.], 0.13) world.lanes += [clane, clane.shifted(1), clane.shifted(-1)] world.roads += [clane] world.fences += [ clane.shifted(2), clane.shifted(-2), clane.shifted(2.5), clane.shifted(-2.5) ] world.cars.append( car.SimpleOptimizerCar(dyn, [-0.13, 0., math.pi / 2, 0.5], color='red')) world.cars.append( car.SimpleOptimizerCar(dyn, [-0.13, -1.3, math.pi / 2, 0.5], color='yellow')) world.cars.append( car.SimpleOptimizerCar(dyn, [-0.13, -0.5, math.pi / 2, 0.5], color='orange')) world.cars[0].reward = world.simple_reward(world.cars[0], speed=0.5) world.cars[1].reward = world.simple_reward(world.cars[1], speed=0.5) + 100 * veh_follow world.cars[2].reward = world.simple_reward(world.cars[2], speed=0.5) return world
def world11(): dyn = dynamics.CarDynamics(0.1) world = World(midlevel_exists=True) clane = lane.StraightLane([0., -1.], [0., 1.], lane.DEFAULT_WIDTH) world.lanes += [clane, clane.shifted(1)] world.roads += [clane] world.fences += [ clane.shifted(2), clane.shifted(-1), clane.shifted(2.5), clane.shifted(-1.5) ] world.cars.append( car.SwitchOptimizerCar(dyn, [-0.13, 0.3, math.pi / 2., 0.5], color='red', iamrobot=True)) world.cars.append( car.SimpleOptimizerCar(dyn, [-0.13, 0., math.pi / 2., 0.5], color='yellow', iamrobot=False)) world.cars.append( car.SwitchOptimizerCar(dyn, [-0.13, -0.3, math.pi / 2., 0.5], color='red', iamrobot=True)) world.cars.append( car.SimpleOptimizerCar(dyn, [0, 0.1, math.pi / 2., 0.5], color='yellow', iamrobot=False)) world.cars.append( car.SimpleOptimizerCar(dyn, [0, -0.25, math.pi / 2., 0.5], color='yellow', iamrobot=False)) world.cars[0].baseline_reward = world.simple_reward(world.cars[0], speed=0.6) world.cars[0].default_u = np.asarray([0., 1.]) world.cars[1].reward = world.simple_reward(world.cars[1], speed=0.6) world.cars[1].default_u = np.asarray([0., 1.]) world.cars[2].baseline_reward = world.simple_reward(world.cars[2], speed=0.6) world.cars[2].default_u = np.asarray([0., 1.]) world.cars[3].reward = world.simple_reward(world.cars[3], speed=0.6) world.cars[3].default_u = np.asarray([0., 1.]) world.cars[4].reward = world.simple_reward(world.cars[4], speed=0.6) world.cars[4].default_u = np.asarray([0., 1.]) return world
def fast_merge_right_run(): theta = [0.12118441, -50.98300634, 23.10744109, 3.11221324, -57.56491208] dyn = dynamics.CarDynamics(0.1) world = World() clane = lane.StraightLane([0., -1.], [0., 1.], 0.13) world.lanes = [clane, clane.shifted(1), clane.shifted(-1)] world.roads = [clane] world.fences = [clane.shifted(2), clane.shifted(-2)] d = shelve.open('cache', writeback=True) world.cars.append( car.SimpleOptimizerCar(dyn, [0.0, -0.2, math.pi / 2., 0.], color='white')) world.cars.append( car.SimpleOptimizerCar(dyn, [0, 0, math.pi / 2., .0], color='red')) world.cars[0].reward = world.simple_reward(world.cars[0], speed=1, theta=theta) world.cars[1].reward = world.simple_reward(world.cars[1], speed=0.5) return world
def world_test(): dyn = dynamics.CarDynamics(0.1) world = World() clane = lane.StraightLane([0., -1.], [0., 1.], 0.13) world.lanes += [clane, clane.shifted(1), clane.shifted(-1)] world.roads += [clane] world.fences += [clane.shifted(2), clane.shifted(-2)] world.cars.append(car.UserControlledCar(dyn, [-0.13, 0., math.pi/2., 0.3], color='red')) world.cars.append(car.SimpleOptimizerCar(dyn, [0.0, 0.5, math.pi/2., 0.3], color='yellow')) world.cars[1].reward = world.simple_reward(world.cars[1], speed=0.5) return world
def world10(): dyn = dynamics.CarDynamics(0.1) world = World() clane = lane.StraightLane([0., -1.], [0., 1.], 0.13) world.lanes += [clane, clane.shifted(1), clane.shifted(-1)] world.roads += [clane] world.fences += [ clane.shifted(2), clane.shifted(-2), clane.shifted(2.5), clane.shifted(-2.5) ] world.cars.append( car.SimpleOptimizerCar(dyn, [-0.13, 0., math.pi / 2, 0.5], color='red')) world.cars.append( car.NestedOptimizerCar(dyn, [-0.13, -1.3, math.pi / 2, 0.5], color='yellow')) world.cars.append( car.SimpleOptimizerCar(dyn, [-0.13, -0.3, math.pi / 2, 0.5], color='orange')) world.cars[1].human = world.cars[2] #r_h = world.simple_reward([world.cars[1].traj])+100.*feature.bounded_control(world.cars[0].bounds) @feature.feature def veh_follow(t, x, u): return -( (world.cars[0].traj.x[t][0] - world.cars[1].traj.x[t][0])**4 + (world.cars[0].traj.x[t][1] - 0.3 - world.cars[1].traj.x[t][1])**2) r_r = world.simple_reward(world.cars[1], speed=0.5) + 100 * veh_follow r_h = world.simple_reward(world.cars[2], speed=0.5) world.cars[0].reward = world.simple_reward(world.cars[0], speed=0.5) world.cars[1].rewards = (r_h, r_r) world.cars[2].reward = r_h return world
def irl_ground(): dyn = dynamics.CarDynamics(0.1) world = World() clane = lane.StraightLane([0., -1.], [0., 1.], 0.13) world.lanes += [clane, clane.shifted(1), clane.shifted(-1)] world.roads += [clane] world.fences += [clane.shifted(2), clane.shifted(-2)] d = shelve.open('cache', writeback=True) cars = [ (-0.13, .1, .9, -0.13), (0, .4, .8, 0.0), (.13, 0, .6, .13), (0, .8, .5, 0.), (0., 1., 0.5, 0.), (-.13, -0.5, 0.9, -0.13), (.13, -.8, 1., 0.13), (-.13, 1.0, 0.6, -0.13), (.13, 1.9, .5, 0.13), (0, 1.5, 0.5, 0), ] def goal(g): @feature.feature def r(t, x, u): return -(x[0] - g)**2 return r for i, (x, y, s, gx) in enumerate(cars): if str(i) not in d: d[str(i)] = [] world.cars.append( car.SimpleOptimizerCar(dyn, [x, y, math.pi / 2., s], color='yellow')) world.cars[-1].cache = d[str(i)] def f(j): def sync(cache): d[str(j)] = cache d.sync() return sync world.cars[-1].sync = f(i) for c, (x, y, s, gx) in zip(world.cars, cars): c.reward = world.simple_reward(c, speed=s) + 10. * goal(gx) world.cars.append( car.UserControlledCar(dyn, [0., -0.5, math.pi / 2., 0.7], color='red')) world.cars = world.cars[-1:] + world.cars[:-1] return world
def lane_cut_run(): dyn = dynamics.CarDynamics(0.1) world = highway() world.cars.append( car.SimpleOptimizerCar(dyn, [-.13, 0.2, math.pi / 2., 0.], color='white')) world.cars[0].reward = world.simple_reward( world.cars[0], speed=1, theta=[ -1264.1224982, -1472.43401049, -205.60746282, -885.73160945, -1394.14040446, 181.50131822, -1356.62990406 ]) return world
def world_test(initial_states='far_overtaking', interaction_data=None, init_planner=True): # lanes center_lane = lane.StraightLane([0., -1.], [0., 1.], constants.LANE_WIDTH_VIS) left_lane = center_lane.shifted(1) right_lane = center_lane.shifted(-1) lanes = [center_lane, left_lane, right_lane] roads = [] # fences fences = [center_lane.shifted(2), center_lane.shifted(-2)] # dynamics dyn = dynamics.CarDynamics # cars x0_h = np.array([-constants.LANE_WIDTH_VIS, 0., np.pi / 2., 0.3]) human_car = car.UserControlledCar(x0_h, constants.DT, dyn, constants.CAR_CONTROL_BOUNDS, horizon=config.HORIZON, color=constants.COLOR_H, name=constants.NAME_H) x0_r = np.array([0.0, 0.5, np.pi / 2., 0.3]) robot_car = car.SimpleOptimizerCar(x0_r, constants.DT, dyn, constants.CAR_CONTROL_BOUNDS, horizon=config.HORIZON, color=constants.COLOR_R, name=constants.NAME_R) cars = [robot_car, human_car] name = 'world_test' world = World(name, constants.DT, cars, robot_car, human_car, lanes, roads, fences) # rewards robot_car.reward = reward.simple_reward(world, [human_car.traj_linear], speed=0.5) # initialize planners print("number of robot cars", len(world.robot_cars)) for c in world.robot_cars: if hasattr(c, 'init_planner'): print('Initializing planner for ' + c.name) c.init_planner() return world
def fast_merge_right_demo(): dyn = dynamics.CarDynamics(0.1) world = World() clane = lane.StraightLane([0., -1.], [0., 1.], 0.13) world.lanes = [clane, clane.shifted(1), clane.shifted(-1)] world.roads = [clane] world.fences = [clane.shifted(2), clane.shifted(-2)] d = shelve.open('cache', writeback=True) world.cars.append( car.UserControlledCar(dyn, [0.0, -0.2, math.pi / 2., 0.], color='white')) world.cars.append( car.SimpleOptimizerCar(dyn, [0, 0, math.pi / 2., .0], color='red')) world.cars[1].reward = world.simple_reward(world.cars[1], speed=0.5) return world
def world9(): dyn = dynamics.CarDynamics(0.1) world = World(midlevel_exists=True) clane = lane.StraightLane([0., -1.], [0., 1.], lane.DEFAULT_WIDTH) world.lanes += [clane, clane.shifted(1)] world.roads += [clane] world.fences += [ clane.shifted(2), clane.shifted(-1), clane.shifted(2.5), clane.shifted(-1.5) ] car_set = [{'lane': -0.14382104671193202, 'pos': -5.4614089828930892, 'isRobot': True}, {'lane': -0.076485355678699476, 'pos': -5.287241227862939,'isRobot': True},\ {'lane': 0.041717183967274571, 'pos': -5.1860610599902248,'isRobot': True},\ {'lane': -0.13005408356976389, 'pos': -5.6439957056000463, 'isRobot': True},\ {'lane': -0.0017838032137736108, 'pos': 1.900418486064837, 'isRobot':False},\ {'lane': -0.1255197374127382, 'pos': 2.9384294124012125, 'isRobot':False},\ {'lane': -0.0017836856576096698, 'pos': 2.2542116035764277,'isRobot': False},\ {'lane': -0.0017545585415078939, 'pos': 3.5441324751687593,'isRobot': False},\ {'lane': -0.15426277999902296, 'pos': -5.2395293216772982,'isRobot': True},\ {'lane': -0.0016715524262676368, 'pos': 3.1864204004814645,'isRobot': False},\ {'lane': -0.078191570164785756, 'pos': -5.4227005075263781,'isRobot': True},\ {'lane': -0.17759460092156229, 'pos': -5.0843811548205187,'isRobot': True},\ {'lane': -0.0017836890619227796, 'pos': 4.1049603048534156,'isRobot': False},\ {'lane': -0.0017781598554608973, 'pos': 4.5832853862108536,'isRobot': False},\ {'lane': -0.12587224415597753, 'pos': 3.3265390539200665,'isRobot':False},\ {'lane': -0.043776679064340018, 'pos': -5.1047807513629015,'isRobot': True},\ {'lane': -0.12555872982148231, 'pos': 4.7986304907386588, 'isRobot':False},\ {'lane': -0.1256044714880388, 'pos': 5.2102349853652896, 'isRobot':False},\ {'lane': -0.0017403147363880666, 'pos': 5.0194332605150267,'isRobot': False},\ {'lane': -0.0017840323712066659, 'pos': 5.5120519126069265, 'isRobot':False}] for car_dict in car_set: if car_dict['isRobot']: world.cars.append( car.SwitchOptimizerCar( dyn, [car_dict['lane'], car_dict['pos'], math.pi / 2., 0.5], color='red', iamrobot=True)) else: world.cars.append( car.SimpleOptimizerCar( dyn, [car_dict['lane'], car_dict['pos'], math.pi / 2., 0.5], color='yellow')) return world
def two_merge_traj(): dyn = dynamics.CarDynamics(0.1) world = highway() world.cars.append( car.UserControlledCar(dyn, [0.13, 0.2, math.pi / 2., 0.], color='white')) world.cars.append( car.UserControlledCar(dyn, [-.13, 0.2, math.pi / 2., 0.], color='white')) world.cars.append( car.SimpleOptimizerCar(dyn, [0, -.3, math.pi / 2., .0], color='red')) with open('data/two_merge_traj/two_merge_traj-1490744314.pickle') as f: feed_u, feed_x = pickle.load(f) world.cars[0].fix_control(feed_u[0]) with open('data/two_merge_traj/two_merge_traj-1490744613.pickle') as f: feed_u, feed_x = pickle.load(f) world.cars[1].fix_control(feed_u[1]) world.cars[2].reward = world.simple_reward(world.cars[2], speed=.7) return world
def world_test_human(): dyn = dynamics.CarDynamics(0.1) world = highway() world.cars.append( car.UserControlledCar(dyn, [-0.13, 0., math.pi / 2., 0.3], color='red')) world.cars.append( car.SimpleOptimizerCar(dyn, [0.0, 0.3, math.pi / 2., 0.8], color='yellow')) world.cars[1].reward = world.simple_reward(world.cars[1], speed=0.5) with open('data/run1/world_test_human-1486145445.pickle') as f: feed_u, feed_x = pickle.load(f) #traj_h = pickle.load(f) print feed_u.__class__.__name__ print feed_u[0].__class__.__name__ #world.cars[0].follow= traj_h world.cars[0].fix_control(feed_u[0]) return world
def world_kex1(know_model=True): start_human = -0.13 start_robot = -0.00 dyn = dynamics.CarDynamics(0.1) world = World() clane = lane.StraightLane([0., -1.], [0., 1.], 0.13) world.lanes += [clane, clane.shifted(1), clane.shifted(-1)] world.roads += [clane] world.fences += [ clane.shifted(2), clane.shifted(-2), clane.shifted(2.5), clane.shifted(-2.5) ] #world.cars.append(car.SimpleOptimizerCar(dyn, [start_human, 0., math.pi/2., 0.5], color='red')) # red car is human world.cars.append( car.NestedOptimizerCar(dyn, [start_human, 0., math.pi / 2., 0.5], color='red')) # red car is human if know_model: # yellow car is the robot that uses nested optimizer to find the way world.cars.append( car.NestedOptimizerCar(dyn, [start_robot, 0.0, math.pi / 2., 0.5], color='yellow')) else: world.cars.append( car.SimpleOptimizerCar(dyn, [start_robot, 0.0, math.pi / 2., 0.5], color='yellow')) world.cars[0].reward = world.simple_reward(world.cars[0], speed=0.6) world.cars[0].default_u = np.asarray([0., 1.]) @feature.feature def goal(t, x, u): # doesnt need this k = -(10. * (x[0] + 0.13)**2 + 0.5 * (x[1] - 2.)**2) #ASK Elis #print("--------", x[0].auto_name) #print("--------", x[1].auto_name) #exit() return k # object-------------- world.cars.append( car.SimpleOptimizerCar(dyn, [-0.13, 0.5, math.pi / 2., 0.0], color='blue')) # blue car is obstacle #world.cars.append(car.NestedOptimizerCar(dyn, [-0.13, 0.5, math.pi/2., 0.0], color='blue')) # blue car is obstacle #print(world.cars) #exit() world.cars[2].reward = world.simple_reward(world.cars[2], speed=0.0) #world.cars[2].reward = 1 world.cars[2].default_u = np.asarray([0., 0.]) world.cars[2].movable = False #------------------ if know_model: world.cars[1].human = world.cars[ 0] # [1] is robot, asigns that the robot knows who is the human world.cars[1].obstacle = world.cars[2] world.cars[0].obstacle = world.cars[2] world.cars[0].human = world.cars[1] # reward with respect to the robot trajectory: world.cars[1].traj r_h = world.simple_reward( [world.cars[1].traj], speed=0.5) + 100. * feature.bounded_control( world.cars[0].bounds) + 100. * feature.bounded_control( world.cars[2].bounds) #r_r = 10*goal+world.simple_reward([world.cars[1].traj_h], speed=0.5 r_r = world.simple_reward( [world.cars[1].traj_h], speed=0.5) + 100. * feature.bounded_control(world.cars[2].bounds) r_h2 = world.simple_reward( [world.cars[1].traj_h], speed=0.5) + 100. * feature.bounded_control(world.cars[0].bounds) +100. * feature.bounded_control(world.cars[2].bounds) #r_r = 10*goal+world.simple_reward([world.cars[1].traj_h], speed=0.5 r_r2 = world.simple_reward( [world.cars[1].traj], speed=0.5) + 100. * feature.bounded_control(world.cars[2].bounds) #r_obj = world.simple_reward([world.cars[1].traj_h], speed=0.0) world.cars[1].rewards = (r_h, r_r) #ADD: r_object world.cars[0].rewards = (r_h2, r_r2) #(optimize on, the car) #print(r_h) #print(r_r) #print(world.cars[1].rewards) #exit() else: r = 10 * goal + world.simple_reward([world.cars[0].linear], speed=0.5) world.cars[1].reward = r #world.cars.append(static_obj.SimpleOptimizerCar(dyn, [-0.13, 0.5, math.pi/2., 0.0], color='blue')) # blue car is obstacle) return world
def world_kex_old(know_model=True): dyn = dynamics.CarDynamics2(0.1) #dyn.dt = 1.0 #dyn.fiction = 0.0 world = World() # clane = lane.StraightLane([0., -1.], [0., 1.], 0.13) # world.lanes += [clane, clane.shifted(1), clane.shifted(-1)] # world.roads += [clane] # world.fences += [clane.shifted(2), clane.shifted(-2), clane.shifted(2.5), clane.shifted(-2.5)] clane = lane.StraightLane([0., -1.], [0., 1.], 0.13) world.lanes += [clane, clane.shifted(1)] #world.roads += [clane, clane.shifted(1)] world.fences += [clane.shifted(2), clane.shifted(-1)] human_is_follower = False # CAR 0 = Human # CAR 1 = Robot # CAR 2 = Obstacle # IMPORTANT: Folower must be created first # depending on what our human is, follower or leader we create the cars differently if human_is_follower: # Create the cars----- # Human Car #world.cars.append(car.NestedOptimizerCarFollower(dyn, [-0.13, 0.0, math.pi/2., 0.5], color='red', T=3)) world.cars.append( car.NestedOptimizerCarFollower2(dyn, [-0.13, 0.0, math.pi / 2., 0.5], color='red', T=3)) # Robot Car world.cars.append( car.NestedOptimizerCarLeader(dyn, [-0., 0., math.pi / 2., 0.5], color='yellow', T=3)) #world.cars[0].leader = world.cars[1] #world.cars[0].leader1 = world.cars[1] # -------------------- else: # Create the cars----- # Human Car world.cars.append( car.NestedOptimizerCarFollower2(dyn, [0., 0., math.pi / 2., 0.5], color='yellow', T=3)) world.cars.append( car.NestedOptimizerCarLeader(dyn, [-0.13, 0.0, math.pi / 2., 0.5], color='red', T=3)) # Robot Car #world.cars.append(car.NestedOptimizerCarFollower(dyn, [0., 0., math.pi/2., 0.5], color='yellow', T=3)) #world.cars[1].leader = world.cars[0] #world.cars[1].leader1 = world.cars[0] # -------------------- # Obstacle Car #world.cars.append(car.SimpleOptimizerCar(dyn, [-0.13, 0.5, math.pi/2., 0.5], color='blue')) # doesnt work because it cant force the car to turn around world.cars.append( car.SimpleOptimizerCar(dyn, [-0.13, 2, math.pi / 4., 0.], color='blue')) # -------------------- # Reward and default for the Human --- # speed did not change here # world.cars[0].reward = world.simple_reward(world.cars[0], speed=0.6) world.cars[0].default_u = np.asarray([0., 1.]) # ------------------------------------ # Reward and default for the Robot --- # world.cars[1].reward = world.simple_reward(world.cars[1], speed=0.6) world.cars[1].default_u = np.asarray([0., 1.]) # ------------------------------------ # Reward and default for the Obstacle --- world.cars[2].reward = world.simple_reward(world.cars[2], speed=0.) world.cars[2].default_u = np.asarray([0., 0.]) world.cars[2].movable = False # ------------------------------------ # CAR 0 = Human # CAR 1 = Robot # CAR 2 = Obstacle if human_is_follower: world.cars[0].leader = world.cars[1] world.cars[0].obstacle = world.cars[2] world.cars[1].follower = world.cars[0] world.cars[1].obstacle = world.cars[2] else: world.cars[1].follower = world.cars[0] world.cars[1].obstacle = world.cars[2] world.cars[0].leader = world.cars[1] world.cars[0].obstacle = world.cars[2] # CAR 0 = Human # CAR 1 = Robot # CAR 2 = Obstacle # TODO: Fix this part, unsure how to make the world.simplereward # calculates the dynamic(chaning) rewards for the cars depending on their speed and collision with other cars and obstacles #TODO: this is what is wrong, they need to be the same # TODO: cars dont want to slow down, find a solution that works if human_is_follower: # HUMAN #r_h = world.simple_reward([world.cars[1].traj], speed=0.6)+100.*feature.bounded_control(world.cars[0].bounds)+world.simple_reward(world.cars[0].traj_o, speed=0.) # Reward for the human r_h = world.simple_reward( [world.cars[1].traj], speed=0.80) + 100. * feature.bounded_control( world.cars[0].bounds) + 1 * world.simple_reward( world.cars[1].traj_o, speed=0.80) # Reward for the human # ROBOT r_r = world.simple_reward( [world.cars[1].traj_h], speed=0.8) + 100. * feature.bounded_control( world.cars[1].bounds) + 1 * world.simple_reward( world.cars[1].traj_o, speed=0.8) # Reward for the robot else: # HUMAN r_h = world.simple_reward( [world.cars[1].traj_h], speed=0.8) + 100. * feature.bounded_control( world.cars[0].bounds) + 1 * world.simple_reward( world.cars[1].traj_o, speed=0.8) # Reward for the human # ROBOT r_r = world.simple_reward( [world.cars[1].traj], speed=0.8) + 100. * feature.bounded_control( world.cars[1].bounds) + 1 * world.simple_reward( world.cars[1].traj_o, speed=0.8) # Reward for the robot r_o = 1. * feature.bounded_control(world.cars[2].bounds) #r_o = world.simple_reward([world.cars[0].traj_o], speed=0.) world.cars[0].rewards = (r_r, r_h, r_o) world.cars[1].rewards = (r_h, r_r, r_o) # ------------------------------------ return world
def car_from(dyn, definition): if "kind" not in definition: raise Exception("car definition must include 'kind'") if "x0" not in definition: raise Exception("car definition must include 'x0'") if "color" not in definition: raise Exception("car definition must include 'color'") if "T" not in definition: raise Exception("car definition must include 'T'") if definition["kind"] == CAR_SIMPLE: return car.SimpleOptimizerCar(dyn, definition["x0"], color=definition["color"], T=definition["T"]) if definition["kind"] == CAR_USER: return car.UserControlledCar(dyn, definition["x0"], color=definition["color"], T=definition["T"]) if definition["kind"] == CAR_NESTED: return car.NestedOptimizerCar(dyn, definition["x0"], color=definition["color"], T=definition["T"]) if definition["kind"] == CAR_BELIEF: return car.BeliefOptimizerCar(dyn, definition["x0"], color=definition["color"], T=definition["T"]) if definition["kind"] == CAR_CANNED: c = car.CannedCar(dyn, definition["x0"], color=definition["color"], T=definition["T"]) if "controls" not in definition: raise Exception("definition doesn't contain 'controls' key") c.follow(definition["controls"]) return c if definition["kind"] == CAR_NEURAL: c = car.NeuralCar(dyn, definition["x0"], color=definition["color"], T=definition["T"]) if "model" not in definition: raise Exception("definition doesn't contain 'model' key") mu = None if "mu" in definition: mu = definition["mu"] c.use(neural.load(definition["model"]), mu=mu) return c if definition["kind"] == CAR_COPY: c = car.CopyCar(dyn, definition["x0"], color=definition["color"], T=definition["T"]) return c raise Exception("car kind not recognized " + definition["kind"])
self.paused = False self.subframes = 6 self.frame = 0 self.autoquit = True pyglet.clock.schedule(self.output_loop) self.event_loop.run() if __name__ == '__main__' and False: import lane dyn = dynamics.CarDynamics(0.1) vis = Visualizer(dyn.dt) vis.lanes.append(lane.StraightLane([0., -1.], [0., 1.], 0.13)) vis.lanes.append(vis.lanes[0].shifted(1)) vis.lanes.append(vis.lanes[0].shifted(-1)) vis.cars.append(car.UserControlledCar(dyn, [0., 0., math.pi/2., .1])) vis.cars.append(car.SimpleOptimizerCar(dyn, [0., 0.5, math.pi/2., 0.], color='red')) r = -60.*vis.cars[0].linear.gaussian() r = r + vis.lanes[0].gaussian() r = r + vis.lanes[1].gaussian() r = r + vis.lanes[2].gaussian() r = r - 30.*vis.lanes[1].shifted(1).gaussian() r = r - 30.*vis.lanes[2].shifted(-1).gaussian() r = r + 30.*feature.speed(0.5) r = r + 10.*vis.lanes[0].gaussian(10.) r = r + .1*feature.control() vis.cars[1].reward = r vis.main_car = vis.cars[0] vis.paused = True vis.set_heat(r) #vis.set_heat(vis.lanes[0].gaussian()+vis.lanes[1].gaussian()+vis.lanes[2].gaussian()) #vis.set_heat(-vis.cars[1].traj.gaussian()+vis.lanes[0].gaussian()+vis.lanes[1].gaussian()+vis.lanes[2].gaussian())
def world11(): dyn = dynamics.CarDynamics(0.1) world = World() clane = lane.StraightLane([0., -1.], [0., 1.], 0.13) world.lanes += [clane, clane.shifted(1), clane.shifted(-1)] world.roads += [clane] world.fences += [ clane.shifted(2), clane.shifted(-2), clane.shifted(2.5), clane.shifted(-2.5) ] world.cars.append( car.SimpleOptimizerCar(dyn, [-0.13, 0., math.pi / 2, 0.5], color='red')) world.cars.append( car.NestedOptimizerCar(dyn, [-0.13, -1.3, math.pi / 2, 0.5], color='yellow')) world.cars.append( car.SimpleOptimizerCar(dyn, [-0.13, -0.5, math.pi / 2, 0.5], color='orange')) world.cars[1].human = world.cars[2] #r_h = world.simple_reward([world.cars[1].traj])+100.*feature.bounded_control(world.cars[0].bounds) """ This goal just tries to get the cars close to each other """ """ @feature.feature def veh_follow(t, x, u): return -((world.cars[0].traj.x[t][0]-world.cars[1].traj.x[t][0])**2 + (world.cars[0].traj.x[t][1]-0.3-world.cars[1].traj.x[t][1])**2) """ """ For the first few time steps, try to get the vehicles close together in the y-dimension. After a number of steps, get them together in the x-dimension. This allows for strategic lane switching. """ """ @feature.feature def veh_follow(t, x, u): if (t>3): s = -(5*(world.cars[0].traj.x[t][0]-world.cars[1].traj.x[t][0])**2 + (world.cars[0].traj.x[t][1]-0.3-world.cars[1].traj.x[t][1])**2) else : s = -((world.cars[0].traj.x[t][1]-0.3-world.cars[1].traj.x[t][1])**2); return s """ """ This goal ramps up how much the x-dimension matters as the vehicle gets closer to platooning position. Also, the y-position goal saturates. """ @feature.feature def veh_follow(t, x, u): follow_loc = 0.3 distance_sq = (world.cars[0].traj.x[t][0] - world.cars[1].traj.x[t][0] )**2 + (world.cars[0].traj.x[t][1] - world.cars[1].traj.x[t][1])**2 distance = np.sqrt(distance_sq) # if we are not close to the goal car, it doesn't matter what lane we're in (hence the exp term -- importance of being in the correct lane decays exponentially with y-distance from target) x_penalty = -10 * tt.exp(-10.0 * (distance - follow_loc)) * ( world.cars[0].traj.x[t][0] - world.cars[1].traj.x[t][0])**2 # The y penalty should saturate at a certain distance because a very distant car shouldn't engage in very risky maneuvers. # Because of this we have the exponential saturation term. #y_penalty = -(world.cars[0].traj.x[t][1]-follow_loc-world.cars[1].traj.x[t][1])**2 y_penalty = -( -1.0 / 2.0 + 100.0 / (1.0 + tt.exp(-1.0 / 10.0 * (world.cars[0].traj.x[t][1] - world.cars[1].traj.x[t][1] - follow_loc)**2))) s = x_penalty + y_penalty return s r_r = world.simple_reward(world.cars[1], speed=0.5) + 100 * veh_follow r_h = world.simple_reward(world.cars[2], speed=0.5) world.cars[0].reward = world.simple_reward(world.cars[0], speed=0.5) world.cars[1].rewards = (r_h, r_r) world.cars[2].reward = r_h return world
def irl_ground_redo(): start_thetas = [ [-49.41780328, 7.56889171, -29.88617277, -43.16951913, 2.66180459], [31.76720093, -41.00733137, -11.04480014, 3.10109911, -17.74631041], [-35.28080999, 7.16573391, 39.76757247, -29.46527668, -46.8011004], [-9.15809793, 22.46761354, 16.02720722, -16.42384674, 13.66530697], [22.74288425, -30.45558916, 21.02526639, 40.52208768, -22.98279955], [-11.82435415, -45.55631605, -48.66029143, 4.7003656, 34.83941114] ] learned_thetas = [ [0.63050464, -0.25920318, 6.62276118, 1.36460598, -27.45587143], [0.29136727, -45.62213084, 19.73830621, 2.16081166, -40.14207103], [ 8.67710219e-01, -1.04749109e-02, 3.39600499e+01, 1.47286358e+00, -4.74656408e+01 ], [0.41207963, -0.15903006, 19.09811664, 1.42279691, -23.45312591], [0.87659958, -5.29046734, 34.99991666, 1.62502487, -36.36590119], [0.45852171, -45.9455371, -4.8938928, 1.38068336, -28.57903624] ] #off second run theta, #[array([-21.64797415, -41.91799587, 17.57049892, 2.9955055 , 0.06245884]), array([-48.09720879, 30.55382214, 12.59564428, -14.51636909, -33.88826816])] #[array([-17.59146916, -41.91799587, 17.5817344 , 2.74549597, -8.84827121]), array([-27.52673679, 30.55382214, 12.65934233, 0.87643322, -55.85940466])] #off fifth run theta, #[-30.61573957 -33.34307184 -36.49309123 -20.69403454 -57.9208694 ] #T=1 cuts #[ 0.38375847 -19.84508972 5.3975739 -46.28047522 -113.86290679] old_theta = [3., -50., 10., 20., -60.] dyn = dynamics.CarDynamics(0.1) world = World() clane = lane.StraightLane([0., -1.], [0., 1.], 0.13) world.lanes += [clane, clane.shifted(1), clane.shifted(-1)] world.roads += [clane] world.fences += [clane.shifted(2), clane.shifted(-2)] d = shelve.open('cache', writeback=True) cars = [ (-0.13, .1, .9, -0.13), (0, .4, .8, 0.0), (.13, 0, .6, .13), (0, .8, .5, 0.), (0., 1., 0.5, 0.), (-.13, -0.5, 0.9, -0.13), (.13, -.8, 1., 0.13), (-.13, 1.0, 0.6, -0.13), (.13, 1.9, .5, 0.13), (0, 1.5, 0.5, 0), ] def goal(g): @feature.feature def r(t, x, u): return -(x[0] - g)**2 return r for i, (x, y, s, gx) in enumerate(cars): if str(i) not in d: d[str(i)] = [] world.cars.append( car.SimpleOptimizerCar(dyn, [x, y, math.pi / 2., s], color='yellow')) world.cars[-1].cache = d[str(i)] def f(j): def sync(cache): d[str(j)] = cache d.sync() return sync world.cars[-1].sync = f(i) for c, (x, y, s, gx) in zip(world.cars, cars): c.reward = world.simple_reward(c, speed=s) + 10. * goal(gx) world.cars.append( car.SimpleOptimizerCar(dyn, [0, -0.5, math.pi / 2., .7], color='red')) world.cars[-1].reward = world.simple_reward( world.cars[-1], speed=1, theta=learned_thetas[5]) #+300*goal(.13) world.cars = world.cars[-1:] + world.cars[:-1] return world
def world7(prob_aut=0.5): num_cars, prob_aut = 20, prob_aut aut_list = np.zeros(num_cars) dyn = dynamics.CarDynamics(0.1) world = World(midlevel_exists=True) world.aut_level = prob_aut clane = lane.StraightLane([0., -1.], [0., 1.], lane.DEFAULT_WIDTH) world.lanes += [clane, clane.shifted(1)] world.roads += [clane] world.fences += [ clane.shifted(2), clane.shifted(-1), clane.shifted(2.5), clane.shifted(-1.5) ] # world.cars.append(car.SimpleOptimizerCar(dyn, [-0.13, 0., math.pi/2., 0.5], color='yellow')) # world.cars.append(car.SwitchOptimizerCar(dyn, [0, 0.15, math.pi/2., 0.5], color='red', iamrobot=True)) # world.cars.append(car.SwitchOptimizerCar(dyn, [0, -0.3, math.pi/2., 0.5], color='red', iamrobot = True)) lane_1_cur_pos = -10 lane_2_cur_pos = -10.5 for i in xrange(num_cars): temp_pos = None temp_lane = np.random.binomial(1, 0.5) * (-0.13) if temp_lane < 0: temp_pos = lane_1_cur_pos lane_1_cur_pos += np.random.uniform(0.25, 0.5) else: temp_pos = lane_2_cur_pos lane_2_cur_pos += np.random.uniform(0.25, 0.5) if np.random.random() <= prob_aut: world.cars.append( car.SwitchOptimizerCar( dyn, [temp_lane, temp_pos, math.pi / 2., 0.5], color='red', iamrobot=True)) aut_list[i] = 1 else: world.cars.append( car.SimpleOptimizerCar( dyn, [temp_lane, temp_pos, math.pi / 2., 0.5], color='yellow')) print("\n\n\n\n\n\n\n\n\n\n\n\nAUT LIST IS: {} \n\n\n\n".format(aut_list)) # world.cars[0].reward = world.simple_reward(world.cars[0], speed=0.6) # world.cars[0].default_u = np.asarray([0., 1.]) # world.cars[1].baseline_reward = world.simple_reward(world.cars[1], speed=0.6) # world.cars[1].default_u = np.asarray([0., 1.]) # world.cars[2].baseline_reward = world.simple_reward(world.cars[2], speed=0.6) # world.cars[2].default_u = np.asarray([0., 1.]) # world.cars[3].reward = world.simple_reward(world.cars[3], speed=0.6) # world.cars[3].default_u = np.asarray([0., 1.]) for i in xrange(num_cars): if aut_list[i] == 1: world.cars[i].baseline_reward = world.simple_reward(world.cars[i], speed=0.6) world.cars[i].default_u = np.asarray([0., 1.]) else: world.cars[i].reward = world.simple_reward(world.cars[i], speed=0.6) world.cars[i].default_u = np.asarray([0., 1.]) return world
def world_kex(know_model=True): dyn = dynamics.CarDynamics2(0.1) world = World() clane = lane.StraightLane([0., -1.], [0., 1.], 0.13) world.lanes += [clane, clane.shifted(1)] #world.roads += [clane, clane.shifted(1)] world.fences += [clane.shifted(2), clane.shifted(-1)] # both behind: pos=0.027 (both begind) and pos=0.028 (different behaviours) # both infront: pos=0.128 (different behaviours) and pos=0.129 (both infront) to switch # We run: # T_stepls = 3 # step_per_u = 2 # speed = 0.80 # we have 3 different results: # 1. both begind # 2. both infront # 3. different behaviour depending on role # to get the distance take the pos/0.13 to get it to irl meters left_is_follower = False pos = 0.15 #pos = 0.15 #pos=0.0 T_steps = 3 speed = 0.80 #pos = 0.128 #pos = 0.028 # THIS WORKS # steps per u is 2 #left_is_follower = False #T_steps = 3 #pos = 0.10 #WORKS #speed = 0.80 # Demonstration left_color = "green" right_color = "blue-dark" # Real #follower_color = "yellow" #leader_color = "red" # Follower must alwasy be created first, otherwise it won't move if left_is_follower: world.cars.append( car.NestedOptimizerCarFollower2(dyn, [-0.13, pos, math.pi / 2., speed], color=left_color, T=T_steps)) world.cars.append( car.NestedOptimizerCarLeader(dyn, [-0.0, 0.0, math.pi / 2., speed], color=right_color, T=T_steps)) else: world.cars.append( car.NestedOptimizerCarFollower2(dyn, [-0.0, 0.0, math.pi / 2., speed], color=right_color, T=T_steps)) world.cars.append( car.NestedOptimizerCarLeader(dyn, [-0.13, pos, math.pi / 2., speed], color=left_color, T=T_steps)) #world.cars.append(car.SimpleOptimizerCar(dyn, [-0.13, 2, math.pi/4., 0.], color='blue')) # THE OBSTACLE IT WORKS WITH #world.cars.append(car.SimpleOptimizerCar(dyn, [-0.20, 1, math.pi/4., 0.], color='blue')) # THE OBSTACLE FOR DEMONSTRATIONS world.cars.append( car.SimpleOptimizerCar(dyn, [-0.20, 0.7, math.pi / 4., 0.], color='gray')) # default_u for the cars world.cars[0].default_u = np.asarray([0., 1.]) world.cars[1].default_u = np.asarray([0., 1.]) # Reward and default for the Obstacle --- world.cars[2].reward = world.simple_reward(world.cars[2], speed=0.) world.cars[2].default_u = np.asarray([0., 0.]) world.cars[2].movable = False # tells the cars who is the follower and who is the leader world.cars[0].leader = world.cars[1] world.cars[1].follower = world.cars[0] world.cars[0].obstacle = world.cars[2] world.cars[1].obstacle = world.cars[2] r_leader = world.simple_reward( [world.cars[1].traj_h, world.cars[1].traj_o, world.cars[1].traj_o], speed=speed) # leader doesnt need bounded controls, only the follower r_follower = world.simple_reward( [world.cars[1].traj, world.cars[1].traj_o, world.cars[1].traj_o], speed=speed) + 100. * feature.bounded_control(world.cars[0].bounds) r_o = 0. #r_o = world.simple_reward([world.cars[0].traj_o], speed=0.) world.cars[0].rewards = (r_leader, r_follower) world.cars[1].rewards = (r_follower, r_leader) # ------------------------------------ return world
self.feed_u = history_u pyglet.clock.schedule_interval(self.animation_loop, 0.02) pyglet.clock.schedule_interval(self.control_loop, self.dt) self.event_loop.run() if __name__ == '__main__' and False: import lane dyn = dynamics.CarDynamics(0.1) vis = Visualizer(dyn.dt) vis.lanes.append(lane.StraightLane([0., -1.], [0., 1.], 0.13)) vis.lanes.append(vis.lanes[0].shifted(1)) vis.lanes.append(vis.lanes[0].shifted(-1)) vis.cars.append(car.UserControlledCar(dyn, [0., 0., math.pi / 2., .1])) vis.cars.append( car.SimpleOptimizerCar(dyn, [0., 0.5, math.pi / 2., 0.], color='red')) r = -60. * vis.cars[0].linear.gaussian() r = r + vis.lanes[0].gaussian() r = r + vis.lanes[1].gaussian() r = r + vis.lanes[2].gaussian() r = r - 30. * vis.lanes[1].shifted(1).gaussian() r = r - 30. * vis.lanes[2].shifted(-1).gaussian() r = r + 30. * feature.speed(0.5) r = r + 10. * vis.lanes[0].gaussian(10.) r = r + .1 * feature.control() vis.cars[1].reward = r vis.main_car = vis.cars[0] vis.paused = True vis.set_heat(r) #vis.set_heat(vis.lanes[0].gaussian()+vis.lanes[1].gaussian()+vis.lanes[2].gaussian()) #vis.set_heat(-vis.cars[1].traj.gaussian()+vis.lanes[0].gaussian()+vis.lanes[1].gaussian()+vis.lanes[2].gaussian())
def world7(): dyn = dynamics.CarDynamics(0.1) world = World() clane = lane.StraightLane([0., -1.], [0., 1.], 0.13) world.lanes += [clane, clane.shifted(1), clane.shifted(-1)] world.roads += [clane] world.fences += [ clane.shifted(2), clane.shifted(-2), clane.shifted(2.5), clane.shifted(-2.5) ] world.cars.append( car.SimpleOptimizerCar(dyn, [-0.13, 0.6, math.pi / 2, 0.5], color='red')) world.cars.append( car.SimpleOptimizerCar(dyn, [-0.13, 0.3, math.pi / 2, 0.5], color='red')) world.cars.append( car.SimpleOptimizerCar(dyn, [-0.13, 0., math.pi / 2., 0.5], color='red')) world.cars.append( car.SimpleOptimizerCar(dyn, [-0.13, -0.3, math.pi / 2, 0.5], color='red')) world.cars.append( car.SimpleOptimizerCar(dyn, [-0.13, -0.6, math.pi / 2, 0.5], color='red')) world.cars.append( car.SimpleOptimizerCar(dyn, [0., 0.3, math.pi / 2, 0.5], color='orange')) world.cars.append( car.SimpleOptimizerCar(dyn, [0., 0.05, math.pi / 2., 0.5], color='orange')) world.cars.append( car.SimpleOptimizerCar(dyn, [0., -0.3, math.pi / 2, 0.5], color='orange')) world.cars.append( car.SimpleOptimizerCar(dyn, [0., -0.6, math.pi / 2, 0.5], color='orange')) world.cars.append( car.SimpleOptimizerCar(dyn, [0.13, 0.3, math.pi / 2, 0.5], color='yellow')) world.cars.append( car.SimpleOptimizerCar(dyn, [0.13, 0., math.pi / 2, 0.5], color='yellow')) world.cars.append( car.SimpleOptimizerCar(dyn, [0.13, -0.3, math.pi / 2, 0.5], color='yellow')) world.cars.append( car.SimpleOptimizerCar(dyn, [0.13, -0.6, math.pi / 2, 0.5], color='yellow')) world.cars[0].reward = world.simple_reward(world.cars[0], speed=0.5) world.cars[0].default_u = np.asarray([0., 1.]) world.cars[1].reward = world.simple_reward(world.cars[1], speed=0.55) world.cars[1].default_u = np.asarray([0., 1.]) world.cars[2].reward = world.simple_reward(world.cars[2], speed=0.5) world.cars[2].default_u = np.asarray([0., 1.]) world.cars[3].reward = world.simple_reward(world.cars[3], speed=1.5) world.cars[3].default_u = np.asarray([0., 1.]) world.cars[4].reward = world.simple_reward(world.cars[4], speed=0.45) world.cars[4].default_u = np.asarray([0., 1.]) world.cars[5].reward = world.simple_reward(world.cars[5], speed=0.5) world.cars[5].default_u = np.asarray([0., 1.]) world.cars[6].reward = world.simple_reward(world.cars[6], speed=0.5) world.cars[6].default_u = np.asarray([0., 1.]) world.cars[7].reward = world.simple_reward(world.cars[7], speed=0.5) world.cars[7].default_u = np.asarray([0., 1.]) world.cars[8].reward = world.simple_reward(world.cars[8], speed=0.55) world.cars[8].default_u = np.asarray([0., 1.]) world.cars[9].reward = world.simple_reward(world.cars[9], speed=0.5) world.cars[9].default_u = np.asarray([0., 1.]) world.cars[10].reward = world.simple_reward(world.cars[10], speed=0.5) world.cars[10].default_u = np.asarray([0., 1.]) world.cars[11].reward = world.simple_reward(world.cars[11], speed=0.45) world.cars[11].default_u = np.asarray([0., 1.]) world.cars[12].reward = world.simple_reward(world.cars[12], speed=0.5) world.cars[12].default_u = np.asarray([0., 1.]) return world