def perform_action(sim, agent_id, vehicle, action, controller_state,
sensor_state, action_space):
if action is None:
return
if action_space == ActionSpaceType.Continuous:
vehicle.control(
throttle=np.clip(action[0], 0.0, 1.0),
brake=np.clip(action[1], 0.0, 1.0),
steering=np.clip(action[2], -1, 1),
)
elif action_space == ActionSpaceType.ActuatorDynamic:
ActuatorDynamicController.perform_action(vehicle,
action,
controller_state,
dt_sec=sim.timestep_sec)
elif action_space == ActionSpaceType.Trajectory:
TrajectoryTrackingController.perform_trajectory_tracking_PD(
action,
vehicle,
controller_state,
dt_sec=sim.timestep_sec,
)
elif action_space == ActionSpaceType.MPC:
TrajectoryTrackingController.perform_trajectory_tracking_MPC(
action, vehicle, controller_state, sim.timestep_sec)
elif action_space == ActionSpaceType.LaneWithContinuousSpeed:
LaneFollowingController.perform_lane_following(
sim,
agent_id,
vehicle,
controller_state,
sensor_state,
action[0],
action[1],
)
elif action_space == ActionSpaceType.Lane:
perform_lane_following = partial(
LaneFollowingController.perform_lane_following,
sim=sim,
agent_id=agent_id,
vehicle=vehicle,
controller_state=controller_state,
sensor_state=sensor_state,
)
# 12.5 m/s (45 km/h) is used as the nominal speed for lane change.
# For keep_lane, the nominal speed is set to 15 m/s (54 km/h).
if action == "keep_lane":
perform_lane_following(target_speed=15, lane_change=0)
elif action == "slow_down":
perform_lane_following(target_speed=0, lane_change=0)
elif action == "change_lane_left":
perform_lane_following(target_speed=12.5, lane_change=1)
elif action == "change_lane_right":
perform_lane_following(target_speed=12.5, lane_change=-1)
else:
raise ValueError(
f"perform_action(action_space={action_space}, ...) has failed "
"inside controller")
def act(self, obs):
ego = obs.ego_vehicle_state
lane_index = min(len(obs.waypoint_paths) - 1, ego.lane_index)
wps = obs.waypoint_paths[lane_index]
for wp_path in obs.waypoint_paths:
if wp_path[0].lane_index == lane_index:
wps = wp_path
break
# wps = min(
# obs.waypoint_paths,
# key=lambda wps: abs(wps[0].signed_lateral_error(ego.position)),
# )
num_trajectory_points = len(wps)
trajectory = [
[wps[i].pos[0] for i in range(num_trajectory_points)],
[wps[i].pos[1] for i in range(num_trajectory_points)],
[wps[i].heading for i in range(num_trajectory_points)],
]
curvature = abs(
TrajectoryTrackingController.curvature_calculation(trajectory,
num_points=10))
gains = {
"theta": 3.0 * 0.05,
"position": 1.0 * 100,
"obstacle": 0.0,
"u_accel": 0.01,
"u_yaw_rate": 0.1,
"terminal": 0.01,
"impatience": 0.0,
"speed": 0.5 * 100,
"rate": 1 * 10,
}
self.gain = Gain(**gains)
if curvature < 100:
gains = {
"theta": 16 * 0.05,
"position": 1 * 120,
"obstacle": 3.0,
"u_accel": 0.01,
"u_yaw_rate": 0.1,
"terminal": 0.0001,
"impatience": 0.01,
"speed": 0.5 * 100,
"rate": 0.1 * 1,
}
self.gain = Gain(**gains)
if curvature < 10:
gains = {
"theta": 1 * 5,
"position": 40 * 6,
"obstacle": 3.0,
"u_accel": 0.1 * 1,
"u_yaw_rate": 0.01,
"terminal": 0.01,
"impatience": 0.01,
"speed": 0.3 * 400,
"rate": 0.001,
}
self.gain = Gain(**gains)
if curvature > 200:
gains = {
"theta": 0.1 * 0.05,
"position": 1 * 10,
"obstacle": 3.0,
"u_accel": 0.1,
"u_yaw_rate": 0.1,
"terminal": 0.01,
"impatience": 0.01,
"speed": 0.5 * 250 * 2,
"rate": 10,
}
self.gain = Gain(**gains)
wps = wps[:self.WP_N]
# repeat the last waypoint to fill out self.WP_N waypoints
wps += [wps[-1]] * (self.WP_N - len(wps))
flat_wps = [
wp_param for wp in wps for wp_param in
[wp.pos[0], wp.pos[1],
float(wp.heading) + math.pi * 0.5]
]
if self.SV_N == 0:
flat_svs = []
elif len(obs.neighborhood_vehicle_states) == 0 and self.SV_N > 0:
# We have no social vehicles in the scene, create placeholders far away
flat_svs = [
ego.position[0] + 100000,
ego.position[1] + 100000,
0,
0,
] * self.SV_N
else:
# Give the closest SV_N social vehicles to the planner
social_vehicles = sorted(
obs.neighborhood_vehicle_states,
key=lambda sv: np.linalg.norm(sv.position - ego.position),
)[:self.SV_N]
# repeat the last social vehicle to ensure SV_N social vehicles
social_vehicles += [social_vehicles[-1]
] * (self.SV_N - len(social_vehicles))
flat_svs = [
sv_param for sv in social_vehicles for sv_param in [
sv.position[0],
sv.position[1],
float(sv.heading) + math.pi * 0.5,
sv.speed,
]
]
ego_params = [
ego.position[0],
ego.position[1],
float(ego.heading) + math.pi * 0.5,
ego.speed,
]
impatience = self.update_impatience(ego)
solver_params = (list(self.gain) + ego_params + flat_svs + flat_wps +
[impatience, wps[0].speed_limit])
neutral_bias = [0 for i in range(12)]
resp = self.solver.run(solver_params, initial_guess=neutral_bias)
self.last_position = ego.position
if resp is not None:
u_star = resp.solution
self.prev_solution = u_star
ego_model = VehicleModel(*ego_params)
xs = []
ys = []
headings = []
speeds = []
for u in zip(u_star[::2], u_star[1::2]):
ego_model.step(U(*u), self.ts)
headings.append(Heading(ego_model.theta - math.pi * 0.5))
xs.append(ego_model.x)
ys.append(ego_model.y)
speeds.append(ego_model.speed)
traj = [xs, ys, headings, speeds]
if self.debug:
self._draw_debug_panel(xs, ys, wps, flat_svs, ego, u_star)
return traj
else:
# Failed to find a solution.
# re-init the planner and stay still, hopefully once we've re-initialized, we can recover
self.init_planner()
return None
def perform_lane_following(
cls,
sim,
agent_id,
vehicle,
controller_state,
sensor_state,
target_speed=12.5,
lane_change=0,
):
assert isinstance(vehicle.chassis, AckermannChassis)
state = controller_state
# This lookahead value is coupled with a few calculations below, changing it
# may affect stability of the controller.
print(f"start wp_paths")
wp_paths = sensor_state.mission_planner.waypoint_paths_at(sim,
vehicle.pose,
lookahead=30)
print(f"in l_f_c, wps {wp_paths}")
current_lane = LaneFollowingController.find_current_lane(
wp_paths, vehicle.position)
wp_path = wp_paths[np.clip(current_lane + lane_change, 0,
len(wp_paths) - 1)]
# we compute a road "curviness" to inform our throttle activation.
# We should move slowly when we are on curvy roads.
ewma_road_curviness = 0.0
for wp_a, wp_b in reversed(list(zip(wp_path, wp_path[1:]))):
ewma_road_curviness = lerp(
ewma_road_curviness,
math.degrees(abs(wp_a.relative_heading(wp_b.heading))),
0.03,
)
# print(f"[wp] point is {wp_a}")
road_curviness_normalization = 2.5
road_curviness = np.clip(
ewma_road_curviness / road_curviness_normalization, 0, 1)
# Number of trajectory point used for curvature calculation.
num_trajectory_points = min([10, len(wp_path)])
trajectory = [
[wp_path[i].pos[0] for i in range(num_trajectory_points)],
[wp_path[i].pos[1] for i in range(num_trajectory_points)],
[wp_path[i].heading for i in range(num_trajectory_points)],
]
# The following calculates the radius of curvature for the 4th
# waypoints in the waypoint list. Value 4 is chosen to ensure
# that the heading error correction is triggered before the vehicle
# reaches to a sharp turn defined be min_curvature.
look_ahead_curvature = abs(
TrajectoryTrackingController.curvature_calculation(trajectory, 4))
# Minimum curvature limit for pushing forward the waypoint
# which is used for heading error calculation.
min_curvature = 2
# If the look_ahead_curvature is less than the min_curvature, then
# update the location of the points which its curvature is less than
# min_curvature.
if look_ahead_curvature <= min_curvature:
state.min_curvature_location = (wp_path[4].pos[0],
wp_path[4].pos[1])
# LOOK AHEAD ERROR SETTING
# look_ahead_wp_num is the ahead waypoint which is used to
# calculate the lateral error TODO: use adaptive setting to
# choose the ahead waypoint
# if the road_curviness is high(i.e. > 0.5), we reduce the
# look_ahead_wp_num to calculate a more accurate lateral error
# normal look ahead distant is set to 8 meters which is reduced
# to 6 meters when the curvature increases.
# Note: waypoints are spaced at roughly 1 meter apart
if road_curviness > 0.5:
look_ahead_wp_num = 3
else:
look_ahead_wp_num = 4
look_ahead_wp_num = min(look_ahead_wp_num, len(wp_path) - 1)
reference_heading = wp_path[0].heading
look_ahead_wp = wp_path[look_ahead_wp_num]
look_ahead_dist = look_ahead_wp.dist_to(vehicle.position)
vehicle_look_ahead_pt = [
vehicle.position[0] - look_ahead_dist * math.sin(vehicle.heading),
vehicle.position[1] + look_ahead_dist * math.cos(vehicle.heading),
]
# 5.56 m/s (20 km/h), 6.94 m/s (25 km/h) are desired speed for different thresholds
# for road curviness
# 0.5 , 0.8 are dimensionless thresholds for road_curviness.
# 1.8 and 0.6 are the longitudinal velocity controller
# proportional gains for different road curvinesss.
if road_curviness < 0.3:
raw_throttle = (-METER_PER_SECOND_TO_KM_PER_HR * 1.8 *
(vehicle.speed - target_speed))
elif road_curviness > 0.3 and road_curviness < 0.8:
raw_throttle = (-0.6 * METER_PER_SECOND_TO_KM_PER_HR *
(vehicle.speed - np.clip(target_speed, 0, 6.94)))
else:
raw_throttle = (-0.6 * METER_PER_SECOND_TO_KM_PER_HR *
(vehicle.speed - np.clip(target_speed, 0, 5.56)))
speed_error = vehicle.speed - target_speed
state.integral_speed_error += speed_error * sim.timestep_sec
velocity_error_damping_term = (speed_error -
state.speed_error) / sim.timestep_sec
# 0.2 is the coefficent of d-controller for speed tracking
# 0.1 is the coefficent of I-controller for speed tracking
# 5.5 is the gain of feedforward term. This term is directly
# related to the steering angle, this is added to further enhance
# the speed tracking performance.
raw_throttle += (
-0.2 * velocity_error_damping_term -
0.1 * state.integral_speed_error +
abs(5.5 *
math.sin(state.steering_state * vehicle.max_steering_wheel)))
state.speed_error = speed_error
# If the distance of the vehicle to the ahead point for which
# the waypoint curvature is less than min_curvature is less than
# 2 meters, then push forward the waypoint which is used to
# calculate the heading error.
if (state.min_curvature_location != (None, None)) and math.sqrt(
(vehicle.position[0] - state.min_curvature_location[0])**2 +
(vehicle.position[1] - state.min_curvature_location[1])**2) < 2:
reference_heading = wp_path[look_ahead_wp_num].heading
# Desired closed loop poles of the lateral dynamics
# The higher the absolute value, the closed loop response will
# be faster for that state, the four states of that are used for
# Linearization of the lateral dynamics are:
# [lateral error, heading error, yaw_rate, side_slip angle]
desired_poles = np.array([
cls.lateral_error,
cls.heading_error,
cls.yaw_rate,
cls.side_slip_angle,
])
LaneFollowingController.calculate_lateral_gains(
sim, state, vehicle, desired_poles, target_speed)
# LOOK AHEAD CONTROLLER
controller_lat_error = wp_path[look_ahead_wp_num].signed_lateral_error(
vehicle_look_ahead_pt)
abs_heading_error = min(
abs((vehicle.heading % (2 * math.pi)) - reference_heading),
abs(2 * math.pi - abs((vehicle.heading %
(2 * math.pi)) - reference_heading)),
)
curvature_radius = TrajectoryTrackingController.curvature_calculation(
trajectory)
brake_norm = 0
if raw_throttle < 0:
brake_norm = np.clip(-raw_throttle, 0, 1)
throttle_norm = 0
else:
# The term involving absolute value of the lateral speed is
# added as a traction control strategy, The traction controller
# gain is set to 4.5, the lower the value, the vehicle becomes
# more agile but may result in instability in harsh curves
# with high speeds.
if vehicle.speed > 70 / 3.6 and abs(curvature_radius) <= 1e3:
traction_gain = 4.5
elif 40 / 3.6 <= vehicle.speed <= 70 / 3.6 and abs(
curvature_radius) <= 3:
traction_gain = 2.5
else:
traction_gain = 0.5
throttle_norm = np.clip(
raw_throttle - traction_gain * METER_PER_SECOND_TO_KM_PER_HR *
abs(vehicle.chassis.longitudinal_lateral_speed[1]),
0,
1,
)
# The feedback term involving yaw rate is added to reduce
# the oscillation in vehicle heading, the proportional gain for
# yaw rate is set to 2.75, the higher value results in less
# oscillation in heading angle. 0.3 is the integral controller
# gain for lateral error. The feedforward term based on the
# curvature is added to enhance the transient performance when
# the road curvature changes locally.
state.lateral_integral_error += sim.timestep_sec * controller_lat_error
# The feed forward term for the steering controller. This
# term is proportionate to Ux^2/R. The coefficient 0.15 is
# chosen to enhance the transient tracking performance.
# This coefficient also depends on the inertia properties
# and the cornering stiffness of the tires. See:
# https://www.tandfonline.com/doi/full/10.1080/00423114.2015.1055279
steering_feed_forward_gain = 0.15
if abs(curvature_radius) < 7:
steering_feed_forward_gain = 0.45
steering_controller_feed_forward = (1 * steering_feed_forward_gain *
(1 / curvature_radius) *
(vehicle.speed)**2)
normalized_speed = np.clip(vehicle.speed * 3.6 / 100, 0, 1)
heading_speed_gain = lerp(0.5, 14, normalized_speed)
yaw_rate_speed_gain = lerp(5.75, 11.75, normalized_speed)
lateral_speed_gain = np.clip(lerp(-1, 14, normalized_speed), 1, 2)
steering_norm = np.clip(
-heading_speed_gain * math.degrees(state.heading_error_gain) *
(abs_heading_error * np.sign(reference_heading -
(vehicle.heading % (2 * math.pi)))) +
lateral_speed_gain * state.lateral_error_gain *
(controller_lat_error) +
yaw_rate_speed_gain * vehicle.chassis.yaw_rate[2] +
0.3 * state.lateral_integral_error -
steering_controller_feed_forward,
-1,
1,
)
# The steering low pass filter, 5.5 is the constant of the
# first order linear low pass filter.
steering_filter_constant = 5.5
state.steering_state = low_pass_filter(
steering_norm,
state.steering_state,
steering_filter_constant,
sim.timestep_sec,
)
# The Throttle low pass filter, 2 is the constant of the
# first order linear low pass filter.
# TODO: Add low pass filter for brake.
throttle_filter_constant = 2
state.throttle_state = low_pass_filter(
throttle_norm,
state.throttle_state,
throttle_filter_constant,
sim.timestep_sec,
lower_bound=0,
)
# Applying control actions to the vehicle
vehicle.control(
throttle=state.throttle_state,
brake=brake_norm,
steering=state.steering_state,
)
LaneFollowingController._update_target_lane_if_reached_end_of_lane(
agent_id, vehicle, controller_state, sensor_state)
def perform_action(
sim,
agent_id,
vehicle,
action,
controller_state,
sensor_state,
action_space,
vehicle_type,
):
print("in controller")
if action is None:
return
if vehicle_type == "bus":
assert action_space == ActionSpaceType.Trajectory
if action_space == ActionSpaceType.Continuous:
vehicle.control(
throttle=np.clip(action[0], 0.0, 1.0),
brake=np.clip(action[1], 0.0, 1.0),
steering=np.clip(action[2], -1, 1),
)
elif action_space == ActionSpaceType.ActuatorDynamic:
ActuatorDynamicController.perform_action(vehicle,
action,
controller_state,
dt_sec=sim.timestep_sec)
elif action_space == ActionSpaceType.Trajectory:
TrajectoryTrackingController.perform_trajectory_tracking_PD(
action,
vehicle,
controller_state,
dt_sec=sim.timestep_sec,
)
elif action_space == ActionSpaceType.MPC:
TrajectoryTrackingController.perform_trajectory_tracking_MPC(
action, vehicle, controller_state, sim.timestep_sec)
elif action_space == ActionSpaceType.LaneWithContinuousSpeed:
change_lane_set = [-1, 0, 1]
assert action[1] in change_lane_set, "target lane is not valid"
print(f"in Controllers: speed {action[0]}; lane {action[1]}")
LaneFollowingController.perform_lane_following(
sim,
agent_id,
vehicle,
controller_state,
sensor_state,
action[0],
int(action[1]),
)
elif action_space == ActionSpaceType.AnchorPoint:
# TODO: check action (AnchorPoint) is valid
# assert action is change_lane_set, "target lane is not valid"
target_speed = 15
LaneFollowingController.perform_lane_following_with_anchor(
sim,
agent_id,
vehicle,
controller_state,
sensor_state,
target_speed,
action,
)
elif action_space == ActionSpaceType.Lane:
print("in controller-dis")
perform_lane_following = partial(
LaneFollowingController.perform_lane_following,
sim=sim,
agent_id=agent_id,
vehicle=vehicle,
controller_state=controller_state,
sensor_state=sensor_state,
)
# 12.5 m/s (45 km/h) is used as the nominal speed for lane change.
# For keep_lane, the nominal speed is set to 15 m/s (54 km/h).
if action == "keep_lane":
perform_lane_following(target_speed=15, lane_change=0)
elif action == "slow_down":
perform_lane_following(target_speed=0, lane_change=0)
elif action == "change_lane_left":
perform_lane_following(target_speed=12.5, lane_change=1)
elif action == "change_lane_right":
perform_lane_following(target_speed=12.5, lane_change=-1)
# # 20.0 m/s (72 km/h) is used as the nominal speed for lane change.
# # For keep_lane, the nominal speed is set to 33.3 m/s (120 km/h).
# if action == "keep_lane":
# perform_lane_following(target_speed=33.3, lane_change=0)
# elif action == "slow_down":
# perform_lane_following(target_speed=0, lane_change=0)
# elif action == "change_lane_left":
# perform_lane_following(target_speed=20.0, lane_change=1)
# elif action == "change_lane_right":
# perform_lane_following(target_speed=20.0, lane_change=-1)
else:
raise ValueError(
f"perform_action(action_space={action_space}, ...) has failed "
"inside controller")
def perform_action(
sim,
agent_id,
vehicle,
action,
controller_state,
sensor_state,
action_space,
vehicle_type,
):
"""Calls control for the given vehicle based on a given action space and action.
Args:
sim:
A simulation instance.
agent_id:
An agent within the simulation that is associated with a vehicle.
vehicle:
A vehicle within the simulation that is associated with an agent.
action:
The action for the controller to perform.
controller_state:
The last vehicle controller state as relates to its action space.
sensor_state:
The state of a vehicle sensor as relates to vehicle sensors.
action_space:
The action space of the provided action.
vehicle_type:
Vehicle type information about the given vehicle.
"""
if action is None:
return
if vehicle_type == "bus":
assert action_space == ActionSpaceType.Trajectory
if action_space == ActionSpaceType.Continuous:
vehicle.control(
throttle=np.clip(action[0], 0.0, 1.0),
brake=np.clip(action[1], 0.0, 1.0),
steering=np.clip(action[2], -1, 1),
)
elif action_space == ActionSpaceType.ActuatorDynamic:
ActuatorDynamicController.perform_action(vehicle,
action,
controller_state,
dt_sec=sim.last_dt)
elif action_space == ActionSpaceType.Trajectory:
TrajectoryTrackingController.perform_trajectory_tracking_PD(
action,
vehicle,
controller_state,
dt_sec=sim.last_dt,
)
elif action_space == ActionSpaceType.MPC:
TrajectoryTrackingController.perform_trajectory_tracking_MPC(
action, vehicle, controller_state, sim.last_dt)
elif action_space == ActionSpaceType.LaneWithContinuousSpeed:
LaneFollowingController.perform_lane_following(
sim,
agent_id,
vehicle,
controller_state,
sensor_state,
action[0],
action[1],
)
elif action_space == ActionSpaceType.Lane:
perform_lane_following = partial(
LaneFollowingController.perform_lane_following,
sim=sim,
agent_id=agent_id,
vehicle=vehicle,
controller_state=controller_state,
sensor_state=sensor_state,
)
# 12.5 m/s (45 km/h) is used as the nominal speed for lane change.
# For keep_lane, the nominal speed is set to 15 m/s (54 km/h).
if action == "keep_lane":
perform_lane_following(target_speed=15, lane_change=0)
elif action == "slow_down":
perform_lane_following(target_speed=0, lane_change=0)
elif action == "change_lane_left":
perform_lane_following(target_speed=12.5, lane_change=1)
elif action == "change_lane_right":
perform_lane_following(target_speed=12.5, lane_change=-1)
elif action_space == ActionSpaceType.Imitation:
ImitationController.perform_action(sim.last_dt, vehicle, action)
else:
# Note: TargetPose and MultiTargetPose use a MotionPlannerProvider directly
raise ValueError(
f"perform_action(action_space={action_space}, ...) has failed "
"inside controller")