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 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 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 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 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 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 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"])
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
