def state(self, new_x):
self.center = Point(new_x[0], new_x[1])
self.velocity = Point(new_x[2], new_x[3])
self.heading = new_x[4]
self.angular_velocity = new_x[5]
self.acceleration = new_x[6]
self.buildGeometry()
def __init__(
self,
center: Point,
heading: float,
movable: bool = True,
friction: float = 0.0,
min_speed: float = 0.0,
max_speed: float = math.inf,
min_acc: float = -math.inf,
max_acc: float = math.inf,
):
self.center = center # this is x, y
self.heading = heading
self.movable = movable
self.color = "ghost white"
self.collidable = True
self.obj = None # MUST be set by subclasses.
if movable:
self.friction = friction
self.velocity = Point(0, 0) # this is xp, yp
self.acceleration = 0 # this is vp (or speedp)
self.angular_velocity = 0 # this is headingp
self.inputSteering = 0
self.inputAcceleration = 0
self.min_speed = min_speed
self.max_speed = max_speed
self.min_acc = min_acc
self.max_acc = max_acc
self.entity_dynamics = get_entity_dynamics(
friction, self.min_speed, self.max_speed, self.min_acc, self.max_acc, xnp=np
)
def corners(self):
ec = self.edge_centers
c = np.array([self.center.x, self.center.y])
corners = []
corners.append(Point(*(ec[1] + ec[0] - c)))
corners.append(Point(*(ec[2] + ec[1] - c)))
corners.append(Point(*(ec[3] + ec[2] - c)))
corners.append(Point(*(ec[0] + ec[3] - c)))
return corners
def reset(self):
self.world.reset()
# create buildings
self.buildings = [
Building(Point(30, 60), Point(22, 10), "gray80"),
Building(Point(90, 60), Point(22, 10), "gray80"),
# Building(Point(62, 90), Point(2, 60), "gray80"),
]
# create cars
# h_y = 5
r_y = 5
self.cars = {
# "H": Car(Point(58.5, h_y), np.pi / 2, "gray"),
"R": Car(Point(60., r_y), np.pi / 2, "blue")
}
# h_yvel, r_yvel = 10, 10
r_yvel = 10
# self.cars["H"].velocity = Point(0, h_yvel)
self.cars["R"].velocity = Point(0, r_yvel)
# add the objects
for building in self.buildings:
self.world.add(building)
self.world.add(self.cars["R"])
# self.world.add(self.cars["H"]) # order in which dynamic agents are added determines concatenated state/actions
self.step_num = 0
return self._get_obs()
def __init__(
self,
center: Point,
heading: float,
color: str = "red",
min_acc: float = -4.0,
max_acc: float = 4.0,
):
size = Point(3.0, 2.0)
movable = True
friction = 0.06
super(Car, self).__init__(
center, heading, size, movable, friction, min_acc=min_acc, max_acc=max_acc
)
self.color = color
self.collidable = True
def reset(self):
self.world.reset()
# create buildings
self.buildings = [
Building(Point(21, self.height / 2.), Point(42, self.height),
"gray80"),
Building(Point(69, self.height / 2.), Point(42, self.height),
"gray80"),
#Building(Point(28.5, 60), Point(57, self.height), "gray80"),
#Building(Point(91.5, 60), Point(57, self.height), "gray80"),
#Building(Point(62, 90), Point(2, 60), "gray80"),
]
# create cars
h_y, r_y = 5, 5
self.cars = {
#"H": Car(Point(58.5, h_y), np.pi / 2, "gray"),
#"R": Car(Point(61.5, r_y), np.pi / 2, "blue")
"H": Car(Point(43.5, h_y), np.pi / 2, "gray"),
"R": Car(Point(46.5, r_y), np.pi / 2, "blue")
}
h_yvel, r_yvel = 10, 10
self.cars["H"].velocity = Point(0, h_yvel)
self.cars["R"].velocity = Point(0, r_yvel)
# add the objects
for building in self.buildings:
self.world.add(building)
self.world.add(
self.cars["H"]
) # order in which dynamic agents are added determines concatenated state/actions
self.world.add(self.cars["R"])
self.step_num = 0
#self.success = False
return self._get_obs()
class Entity:
def __init__(
self,
center: Point,
heading: float,
movable: bool = True,
friction: float = 0.0,
min_speed: float = 0.0,
max_speed: float = math.inf,
min_acc: float = -math.inf,
max_acc: float = math.inf,
):
self.center = center # this is x, y
self.heading = heading
self.movable = movable
self.color = "ghost white"
self.collidable = True
self.obj = None # MUST be set by subclasses.
if movable:
self.friction = friction
self.velocity = Point(0, 0) # this is xp, yp
self.acceleration = 0 # this is vp (or speedp)
self.angular_velocity = 0 # this is headingp
self.inputSteering = 0
self.inputAcceleration = 0
self.min_speed = min_speed
self.max_speed = max_speed
self.min_acc = min_acc
self.max_acc = max_acc
self.entity_dynamics = get_entity_dynamics(
friction, self.min_speed, self.max_speed, self.min_acc, self.max_acc, xnp=np
)
@property
def speed(self) -> float:
return self.velocity.norm(p=2) if self.movable else 0
def set_control(self, inputSteering: float, inputAcceleration: float):
self.inputSteering = inputSteering
self.inputAcceleration = inputAcceleration
@property
def state(self):
return np.array(
(
self.x,
self.y,
self.xp,
self.yp,
self.heading,
self.angular_velocity,
self.acceleration,
)
)
@state.setter
def state(self, new_x):
self.center = Point(new_x[0], new_x[1])
self.velocity = Point(new_x[2], new_x[3])
self.heading = new_x[4]
self.angular_velocity = new_x[5]
self.acceleration = new_x[6]
self.buildGeometry()
def tick(self, dt: float):
if self.movable:
x = self.state
u = np.array((self.inputSteering, self.inputAcceleration))
new_x = self.entity_dynamics(x, u, dt)
self.state = new_x
def buildGeometry(self): # builds the obj
raise NotImplementedError
def collidesWith(self, other: Union["Point", "Entity"]) -> bool:
if isinstance(other, Entity):
return self.obj.intersectsWith(other.obj)
elif isinstance(other, Point):
return self.obj.intersectsWith(other)
else:
raise NotImplementedError
def distanceTo(self, other: Union["Point", "Entity"]) -> float:
if isinstance(other, Entity):
return self.obj.distanceTo(other.obj)
elif isinstance(other, Point):
return self.obj.distanceTo(other)
else:
raise NotImplementedError
def copy(self):
return copy.deepcopy(self)
@property
def x(self):
return self.center.x
@property
def y(self):
return self.center.y
@property
def xp(self):
return self.velocity.x
@property
def yp(self):
return self.velocity.y