def choose_action(self, state, action_space, info=None):
body_state = make_body_state(state, self.index)
if self.replanning_counter >= self.replanning_frequency:
self.replanning_counter = 0
obstacles = [
make_body_state(state, i).position for i in range(len(state))
if i != self.index
]
self.body.planner_spline, self.target_velocities = self.path_plan(
body_state, obstacles)
if not self.body.planner_spline:
return self.noop_action
waypoint = self.body.planner_spline[0]
target_velocity = self.target_velocities[0]
throttle_action = make_throttle_action(body_state, self.body.constants,
self.time_resolution,
target_velocity,
self.noop_action)
target_orientation = math.atan2(waypoint.y - body_state.position.y,
waypoint.x - body_state.position.x)
steering_action = make_steering_action(body_state, self.body.constants,
self.time_resolution,
target_orientation,
self.noop_action)
return [throttle_action, steering_action]
def predict_obstacles(self, state):
other_body_states = [
make_body_state(state, i) for i in range(len(state))
if i != self.index
]
yield [
other_body_state.position for other_body_state in other_body_states
]
def predict_step(body_state): # assume noop action
distance_velocity = body_state.velocity * self.frenet_planner.constants.dt
return DynamicBodyState(position=Point(
x=body_state.position.x +
distance_velocity * math.cos(body_state.orientation),
y=body_state.position.y +
distance_velocity * math.sin(body_state.orientation)),
velocity=body_state.velocity,
orientation=body_state.orientation)
for _ in range(self.frenet_planner.constants.timesteps_max):
other_body_states = [
predict_step(other_body_state)
for other_body_state in other_body_states
]
yield [
other_body_state.position
for other_body_state in other_body_states
]
def features(self, state, target): # question: what does it mean for a feature to depend on an action and/or what does a Q value mean if it does not depend on an action?
ego_state = make_body_state(state, 0)
self_state = make_body_state(state, self.index)
ego_body = Car(ego_state, self.ego_constants)
self_body = Pedestrian(self_state, self.body.constants)
target_velocity, target_orientation = target
throttle_action = make_throttle_action(self_state, self.body.constants, self.time_resolution, target_velocity, self.noop_action)
steering_action = make_steering_action(self_state, self.body.constants, self.time_resolution, target_orientation, self.noop_action)
action = [throttle_action, steering_action]
joint_action = [ego_body.noop_action, action]
spawn_bodies = [ego_body, self_body]
for i, spawn_body in enumerate(spawn_bodies):
spawn_body.step(joint_action[i], self.time_resolution)
def normalise(value, min_bound, max_bound):
if value < min_bound:
return 0
elif value > max_bound:
return 1
else:
return (value - min_bound) / (max_bound - min_bound)
unnormalised_values = dict()
if self.feature_config.distance_x:
unnormalised_values["distance_x"] = self_body.state.position.distance_x(ego_body.state.position)
if self.feature_config.distance_y:
unnormalised_values["distance_y"] = self_body.state.position.distance_y(ego_body.state.position)
if self.feature_config.distance:
unnormalised_values["distance"] = self_body.state.position.distance(ego_body.state.position)
if self.feature_config.relative_angle:
unnormalised_values["relative_angle"] = abs(geometry.normalise_angle(geometry.Line(start=self_body.state.position, end=ego_body.state.position).orientation() - self_body.state.orientation))
if self.feature_config.heading:
unnormalised_values["heading"] = abs(geometry.normalise_angle(geometry.Line(start=ego_body.state.position, end=self_body.state.position).orientation() - ego_body.state.orientation))
if self.feature_config.on_road:
unnormalised_values["on_road"] = 1 if self_body.bounding_box().intersects(self.road_polgon) else 0
if self.feature_config.inverse_distance:
x = unnormalised_values["distance"] if "distance" in unnormalised_values else self_body.state.position.distance(ego_body.state.position)
unnormalised_values["inverse_distance"] = 1 - (x / self.x_max) ** self.n # thanks to Ram Varadarajan
normalised_values = {feature: normalise(feature_value, *self.feature_bounds[feature]) for feature, feature_value in unnormalised_values.items()}
return normalised_values
def process_feedback(self, previous_state, action, state, reward):
if self.body.planner_spline:
body_state = make_body_state(state, self.index)
waypoint = self.body.planner_spline[0]
if body_state.position.distance(waypoint) < 1:
self.body.planner_spline.pop(0)
self.target_velocities.pop(0)
self.replanning_counter += 1
def process_feedback(self, previous_state, action, state, reward):
body_state = make_body_state(state, self.index)
if self.waypoint is not None:
distance = body_state.position.distance(self.waypoint)
if distance < 1:
self.waypoint = None
self.target_orientation = self.prior_orientation
self.prior_orientation = None
if self.target_orientation is not None:
diff = self.target_orientation - body_state.orientation
if abs(math.atan2(math.sin(diff), math.cos(diff))) < TARGET_ERROR:
self.target_orientation = None
def choose_crossing_action(self, state, condition):
body_state = make_body_state(state, self.index)
if self.waypoint is None and self.target_orientation is None and condition:
closest_point = self.road_centre.closest_point_from(body_state.position)
relative_angle = math.atan2(closest_point.y - body_state.position.y, closest_point.x - body_state.position.x)
if self.initial_distance is None:
self.initial_distance = body_state.position.distance(closest_point)
self.waypoint = Point(
x=closest_point.x + self.initial_distance * math.cos(relative_angle),
y=closest_point.y + self.initial_distance * math.sin(relative_angle),
)
self.target_orientation = math.atan2(self.waypoint.y - body_state.position.y, self.waypoint.x - body_state.position.x)
self.prior_orientation = body_state.orientation
crossing_initiated = True
else:
crossing_initiated = False
steering_action = make_steering_action(body_state, self.body.constants, self.time_resolution, self.target_orientation, self.noop_action)
return [self.noop_action[0], steering_action], crossing_initiated
def features_opponent(self, state, action, opponent_index):
self_state = make_body_state(state, self.index)
opponent_state = make_body_state(state, opponent_index)
def one_step_lookahead(
body_state, throttle): # one-step lookahead with no steering
distance_velocity = body_state.velocity * self.time_resolution
return DynamicBodyState(
position=Point(
x=body_state.position.x +
distance_velocity * math.cos(body_state.orientation),
y=body_state.position.y +
distance_velocity * math.sin(body_state.orientation)),
velocity=max(
self.body.constants.min_velocity,
min(
self.body.constants.max_velocity, body_state.velocity +
(throttle * self.time_resolution))),
orientation=body_state.orientation)
def n_step_lookahead(body_state, throttle, n=2):
next_body_state = body_state
for _ in range(n):
next_body_state = one_step_lookahead(next_body_state, throttle)
return next_body_state
throttle_action, _ = action
self_state = n_step_lookahead(self_state, throttle_action)
opponent_state = n_step_lookahead(opponent_state, 0.0)
def normalise(value, min_bound, max_bound):
if value < min_bound:
return 0.0
elif value > max_bound:
return 1.0
else:
return (value - min_bound) / (max_bound - min_bound)
unnormalised_values = dict()
if self.feature_config.distance_x:
unnormalised_values["distance_x"] = self_state.position.distance_x(
opponent_state.position)
if self.feature_config.distance_y:
unnormalised_values["distance_y"] = self_state.position.distance_y(
opponent_state.position)
if self.feature_config.distance:
unnormalised_values["distance"] = self_state.position.distance(
opponent_state.position)
if self.feature_config.relative_angle:
unnormalised_values["relative_angle"] = abs(
geometry.normalise_angle(
geometry.Line(start=self_state.position,
end=opponent_state.position).orientation() -
self_state.orientation))
if self.feature_config.heading:
unnormalised_values["heading"] = abs(
geometry.normalise_angle(
geometry.Line(start=opponent_state.position,
end=self_state.position).orientation() -
opponent_state.orientation))
normalised_values = {
feature: normalise(feature_value, *self.feature_bounds[feature])
for feature, feature_value in unnormalised_values.items()
}
return normalised_values