def run_in_series(self, next_waypoint: Transform,
**kwargs) -> VehicleControl:
"""
Execute one step of control invoking both lateral and longitudinal
PID controllers to reach a target waypoint at a given target_speed.
Args:
vehicle: New vehicle state
next_waypoint: target location encoded as a waypoint
**kwargs:
Returns:
Next Vehicle Control
"""
curr_speed = Vehicle.get_speed(self.agent.vehicle)
if curr_speed < 60:
self._lat_controller.k_d = OPTIMIZED_LATERAL_PID_VALUES[60].K_D
self._lat_controller.k_i = OPTIMIZED_LATERAL_PID_VALUES[60].K_I
self._lat_controller.k_p = OPTIMIZED_LATERAL_PID_VALUES[60].K_P
elif curr_speed < 100:
self._lat_controller.k_d = OPTIMIZED_LATERAL_PID_VALUES[100].K_D
self._lat_controller.k_i = OPTIMIZED_LATERAL_PID_VALUES[100].K_I
self._lat_controller.k_p = OPTIMIZED_LATERAL_PID_VALUES[100].K_P
elif curr_speed < 150:
self._lat_controller.k_d = OPTIMIZED_LATERAL_PID_VALUES[150].K_D
self._lat_controller.k_i = OPTIMIZED_LATERAL_PID_VALUES[150].K_I
self._lat_controller.k_p = OPTIMIZED_LATERAL_PID_VALUES[150].K_P
acceptable_target_speed = self.target_speed
if abs(self.agent.vehicle.control.steering) < 0.05:
acceptable_target_speed += 20 # eco boost
acceleration = self._lon_controller.run_step(acceptable_target_speed)
current_steering = self._lat_controller.run_step(next_waypoint)
control = VehicleControl()
if acceleration >= 0.0:
control.throttle = min(acceleration, self.max_throttle)
# control.brake = 0.0
else:
control.throttle = 0
# control.brake = min(abs(acceleration), self.max_brake)
# Steering regulation: changes cannot happen abruptly, can't steer too much.
if current_steering > self.past_steering + 0.1:
current_steering = self.past_steering + 0.1
elif current_steering < self.past_steering - 0.1:
current_steering = self.past_steering - 0.1
if current_steering >= 0:
steering = min(self.max_steer, current_steering)
else:
steering = max(-self.max_steer, current_steering)
if abs(current_steering) > 0.03 and curr_speed > 110:
# if i am doing a sharp (>0.5) turn, i do not want to step on full gas
control.throttle = -1
control.steering = steering
self.past_steering = steering
return control
def smoothen_control(self, control: VehicleControl):
if abs(control.throttle) > abs(self.prev_control.throttle
) and self.prev_control.throttle > 0.15:
# ensure slower increase, faster decrease. 0.15 barely drives the car
control.throttle = (self.prev_control.throttle * self.throttle_smoothen_factor + control.throttle) / \
(self.throttle_smoothen_factor + 1)
if abs(control.steering) < abs(self.prev_control.steering):
# slowly turn back
control.steering = (
self.prev_control.steering * self.steering_smoothen_factor_backward + control.steering) / \
(self.steering_smoothen_factor_backward + 1)
elif abs(control.steering) < abs(self.prev_control.steering):
control.steering = (self.prev_control.steering * self.steering_smoothen_factor_forward + control.steering) / \
(self.steering_smoothen_factor_backward + 1)
self.prev_control = control
return control
def run_in_series(self, next_waypoint: Transform,
**kwargs) -> VehicleControl:
"""
Execute one step of control invoking both lateral and longitudinal
PID controllers to reach a target waypoint at a given target_speed.
Args:
vehicle: New vehicle state
next_waypoint: target location encoded as a waypoint
**kwargs:
Returns:
Next Vehicle Control
"""
self.update_lon_control_k_values()
self.update_lat_control_k_values()
acceleration = self._lon_controller.run_step(self.target_speed)
current_steering = self._lat_controller.run_step(next_waypoint)
control = VehicleControl()
if acceleration >= 0.0:
control.throttle = min(acceleration, self.max_throttle)
# control.brake = 0.0
else:
control.throttle = 0
# control.brake = min(abs(acceleration), self.max_brake)
# Steering regulation: changes cannot happen abruptly, can't steer too much.
if current_steering > self.past_steering + 0.1:
current_steering = self.past_steering + 0.1
elif current_steering < self.past_steering - 0.1:
current_steering = self.past_steering - 0.1
if current_steering >= 0:
steering = min(self.max_steer, current_steering)
else:
steering = max(-self.max_steer, current_steering)
control.steering = steering
self.past_steering = steering
abs_throttle = 0.2
return self.throttle_regularization(control=control,
min_throttle=-abs_throttle,
max_throttle=abs_throttle)
def lateral_pid_control(self, next_waypoint: Transform,
control: VehicleControl):
# calculate a vector that represent where you are going
v_begin = self.agent.vehicle.transform.location.to_array()
direction_vector = np.array([
-np.sin(
np.deg2rad(-self.agent.vehicle.transform.rotation.yaw)
), # i think the direction of yaw is different in simulation vs in iOS
0,
-np.cos(np.deg2rad(-self.agent.vehicle.transform.rotation.yaw))
])
v_end = v_begin + direction_vector
v_vec = np.array([(v_end[0] - v_begin[0]), 0, (v_end[2] - v_begin[2])])
# calculate error projection
w_vec = np.array([
next_waypoint.location.x - v_begin[0],
0,
next_waypoint.location.z - v_begin[2],
])
v_vec_normed = v_vec / np.linalg.norm(v_vec)
w_vec_normed = w_vec / np.linalg.norm(w_vec)
error = np.arccos(v_vec_normed @ w_vec_normed.T)
_cross = np.cross(v_vec_normed, w_vec_normed)
# print(error, _cross, v_vec_normed, w_vec_normed)
# print(f"error: {error} | yaw = {self.agent.vehicle.transform.rotation.yaw} | v_vec = {v_vec_normed.tolist()} | w_vec_normed = {w_vec_normed.tolist()} | _cross[1] = {_cross[1]}")
if _cross[1] > 0:
error *= -1
self.lat_error_queue.append(error)
if len(self.lat_error_queue) >= 2:
_de = (self.lat_error_queue[-1] -
self.lat_error_queue[-2]) / self._dt
_ie = sum(self.lat_error_queue) * self._dt
else:
_de = 0.0
_ie = 0.0
# k_p, k_d, k_i = PIDController.find_k_values(config=self.config, vehicle=self.agent.vehicle)
k_p, k_d, k_i = 1, 0, 0
lat_control = float(
np.clip((k_p * error) + (k_d * _de) + (k_i * _ie), -1, 1))
control.steering = lat_control
def lateral_pid_control(self, next_waypoint: Transform,
control: VehicleControl):
x_error = (next_waypoint.location.x -
self.center_x) / next_waypoint.location.z
yaw_error = np.deg2rad(next_waypoint.rotation.yaw - 0)
# there are error in yaw detection, i am resolving the error by using an average.
self.yaw_error_buffer.append(yaw_error)
error = x_error * self.x_error_weight + np.average(
self.yaw_error_buffer) * self.yaw_error_weight
error_dt = 0 if len(
self.lat_error_queue) == 0 else error - self.lat_error_queue[-1]
self.lat_error_queue.append(error)
error_it = sum(self.lat_error_queue)
e_p = self.lat_kp * error
e_d = self.lat_kd * error_dt
e_i = self.lat_ki * error_it
lat_control = np.clip((e_p + e_d + e_i), -1, 1)
control.steering = lat_control
def run_in_series(self, next_waypoint: Transform) -> VehicleControl:
super(VehicleMPCController, self).run_in_series(next_waypoint)
# get vehicle location (x, y)
# location = self.vehicle.transform.location
location = self.agent.vehicle.control.location
x, y = location.x, location.y
# get vehicle rotation
# rotation = self.vehicle.transform.rotation
rotation = self.agent.vehicle.control.rotation
ψ = rotation.yaw / 180 * np.pi # transform into radient
cos_ψ = np.cos(ψ)
sin_ψ = np.sin(ψ)
# get vehicle speed
# v = Vehicle.get_speed(self.vehicle)
v = Vehicle.get_speed(self.agent.vehicle)
# get next waypoint location
wx, wy = next_waypoint.location.x, next_waypoint.location.y
# debug logging
# self.logger.debug(f"car location: ({x}, {y})")
# self.logger.debug(f"car ψ: {ψ}")
# self.logger.debug(f"car speed: {v}")
# self.logger.debug(f"next waypoint: ({wx}, {wy})")
### 3D ###
# get the index of next waypoint
# waypoint_index = self.get_closest_waypoint_index_3D(location,
# next_waypoint.location)
# # find more waypoints index to fit a polynomial
# waypoint_index_shifted = waypoint_index - 2
# indeces = waypoint_index_shifted + self.steps_poly * np.arange(
# self.poly_degree + 1)
# indeces = indeces % self.track_DF.shape[0]
# # get waypoints for polynomial fitting
# pts = np.array([[self.track_DF.iloc[i][0], self.track_DF.iloc[i][
# 1]] for i in indeces])
### 2D ###
index_2D = self.get_closest_waypoint_index_2D(location,
next_waypoint.location)
index_2D_shifted = index_2D - 5
indeces_2D = index_2D_shifted + self.steps_poly * np.arange(
self.poly_degree + 1)
indeces_2D = indeces_2D % self.pts_2D.shape[0]
pts = self.pts_2D[indeces_2D]
# self.logger.debug(f'\nwaypoint index:\n {index_2D}')
# self.logger.debug(f'\nindeces:\n {indeces_2D}')
# transform waypoints from world to car coorinate
pts_car = VehicleMPCController.transform_into_cars_coordinate_system(
pts,
x,
y,
cos_ψ,
sin_ψ
)
# fit the polynomial
poly = np.polyfit(pts_car[:, 0], pts_car[:, 1], self.poly_degree)
# Debug
# self.logger.debug(f'\nwaypoint index:\n {waypoint_index}')
# self.logger.debug(f'\nindeces:\n {indeces}')
# self.logger.debug(f'\npts for poly_fit:\n {pts}')
# self.logger.debug(f'\npts_car:\n {pts_car}')
###########
cte = poly[-1]
eψ = -np.arctan(poly[-2])
init = (0, 0, 0, v, cte, eψ, *poly)
self.state0 = self.get_state0(v, cte, eψ, self.steer, self.throttle,
poly)
result = self.minimize_cost(self.bounds, self.state0, init)
# self.steer = -0.6 * cte - 5.5 * (cte - self.prev_cte)
# self.prev_cte = cte
# self.throttle = VehicleMPCController.clip_throttle(self.throttle,
# v, self.target_speed)
control = VehicleControl()
if 'success' in result.message:
self.steer = result.x[-self.steps_ahead]
self.throttle = result.x[-2 * self.steps_ahead]
else:
self.logger.debug('Unsuccessful optimization')
control.steering = self.steer
control.throttle = self.throttle
return control
