def world0(): 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.NestedOptimizerCar(dyn, [0.0, 0.5, math.pi / 2., 0.3], color='yellow')) world.cars[1].human = world.cars[0] r_h = world.simple_reward([ world.cars[1].traj ]) + 100. * feature.bounded_control(world.cars[0].bounds) @feature.feature def human_speed(t, x, u): return -world.cars[1].traj_h.x[t][3]**2 r_r = world.simple_reward(world.cars[1], speed=0.5) world.cars[1].rewards = (r_h, r_r) return world
def world8(flag=False): dyn = dynamics.CarDynamics(0.1) world = World() vlane = lane.StraightLane([0., -1.], [0., 1.], 0.15) hlane = lane.StraightLane([-1., 0.], [1., 0.], 0.15) world.lanes += [vlane.shifted(0.5), vlane.shifted(-0.5), hlane.shifted(0.5), hlane.shifted(-0.5)] world.fences += [hlane.shifted(-0.5), hlane.shifted(0.5)] world.cars.append(car.UserControlledCar(dyn, [0., -.3, math.pi/2., 0.0], color='red')) world.cars.append(car.NestedOptimizerCar(dyn, [-0.3, 0., 0., 0.], color='blue')) world.cars[1].human = world.cars[0] world.cars[0].bounds = [(-3., 3.), (-2., 2.)] if flag: world.cars[0].follow = world.cars[1].traj_h world.cars[1].bounds = [(-3., 3.), (-2., 2.)] @feature.feature def horizontal(t, x, u): return -x[2]**2 r_h = world.simple_reward([world.cars[1].traj], lanes=[vlane], fences=[vlane.shifted(-1), vlane.shifted(1)]*2)+100.*feature.bounded_control(world.cars[0].bounds) @feature.feature def human(t, x, u): return -tt.exp(-10*(world.cars[1].traj_h.x[t][1]-0.13)/0.1) r_r = human*10.+horizontal*30.+world.simple_reward(world.cars[1], lanes=[hlane]*3, fences=[hlane.shifted(-1), hlane.shifted(1)]*3+[hlane.shifted(-1.5), hlane.shifted(1.5)]*2, speed=0.9) world.cars[1].rewards = (r_h, r_r) return world
def world3(flag=False): 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.UserControlledCar(dyn, [0., 0., math.pi / 2., 0.3], color='red')) world.cars.append( car.NestedOptimizerCar(dyn, [0., 0.3, math.pi / 2., 0.3], color='yellow')) world.cars[1].human = world.cars[0] world.cars[0].bounds = [(-3., 3.), (-1., 1.)] if flag: world.cars[0].follow = world.cars[1].traj_h r_h = world.simple_reward([ world.cars[1].traj ]) + 100. * feature.bounded_control(world.cars[0].bounds) @feature.feature def human(t, x, u): return (world.cars[1].traj_h.x[t][0]) * 10 r_r = 300. * human + world.simple_reward(world.cars[1], speed=0.5) world.cars[1].rewards = (r_h, r_r) #world.objects.append(Object('firetruck', [0., 0.7])) return world
def world1(flag=False): 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.NestedOptimizerCar(dyn, [0.0, 0.5, math.pi / 2., 0.3], color='yellow')) world.cars[1].human = world.cars[0] if flag: world.cars[0].follow = world.cars[1].traj_h r_h = world.simple_reward( [world.cars[1].traj], speed_import=.2 if flag else 1., speed=0.8 if flag else 1.) + 100. * feature.bounded_control(world.cars[0].bounds) @feature.feature def human_speed(t, x, u): return -world.cars[1].traj_h.x[t][3]**2 r_r = 300. * human_speed + world.simple_reward(world.cars[1], speed=0.5) if flag: world.cars[0].follow = world.cars[1].traj_h world.cars[1].rewards = (r_h, r_r) #world.objects.append(Object('cone', [0., 1.8])) return world
def playground(): dyn = dynamics.CarDynamics(0.1) world = World() clane = lane.StraightLane([0., -1.], [0., 1.], 0.17) 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., 0., math.pi/2., 0.], color='orange')) world.cars.append(car.UserControlledCar(dyn, [-0.17, -0.17, math.pi/2., 0.], color='white')) return world
def two_merge_demo(): dyn = dynamics.CarDynamics(0.1) world = highway() world.cars.append( car.UserControlledCar(dyn, [0, 0.1, math.pi / 2., .0], color='red')) 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')) with open('data/two_merge_traj/two_merge_traj-1490744314.pickle') as f: feed_u, feed_x = pickle.load(f) world.cars[1].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[2].fix_control(feed_u[1]) #world.cars[0].reward = world.simple_reward(world.cars[0], speed=.7) 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 world_features(num=0): 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.Car(dyn, [0., 0.1, math.pi/2.+math.pi/5, 0.], color='yellow')) world.cars.append(car.Car(dyn, [-0.13, 0.2, math.pi/2.-math.pi/5, 0.], color='yellow')) world.cars.append(car.Car(dyn, [0.13, -0.2, math.pi/2., 0.], color='yellow')) #world.cars.append(car.NestedOptimizerCar(dyn, [0.0, 0.5, math.pi/2., 0.3], color='yellow')) 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 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 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
self.reset() self.feed_x = history_x 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)
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"])