最近F-1にハマっています。
自分でもF-1っぽいものを作れないかなと調べたところ、Pythonでできそうなものを見つけました。
highway-envを試してみる
PID制御での走行
ソースコードを書くこと自体にはあまり力を入れません。ChatGPTに丸投げです。
サーキットで車を走らせます。車は道路の中心を認識して、簡単なPID制御で走るようにします。
python
from __future__ import annotations
import argparse
import math
import time
from dataclasses import dataclass
import gymnasium as gym
import highway_env # noqa: F401 - registers highway-env environments
import numpy as np
if hasattr(gym, "register_envs"):
gym.register_envs(highway_env)
def clamp(value: float, low: float, high: float) -> float:
return max(low, min(high, value))
def wrap_to_pi(angle: float) -> float:
return (angle + math.pi) % (2 * math.pi) - math.pi
@dataclass
class PID:
kp: float
ki: float
kd: float
integral_limit: float = 10.0
def __post_init__(self) -> None:
self.integral = 0.0
self.previous_error = 0.0
self.has_previous = False
def reset(self) -> None:
self.integral = 0.0
self.previous_error = 0.0
self.has_previous = False
def step(self, error: float, dt: float) -> float:
self.integral = clamp(
self.integral + error * dt,
-self.integral_limit,
self.integral_limit,
)
derivative = 0.0
if self.has_previous and dt > 0:
derivative = (error - self.previous_error) / dt
self.previous_error = error
self.has_previous = True
return self.kp * error + self.ki * self.integral + self.kd * derivative
class CircuitPIDController:
"""Two small PID loops: one for speed, one for lane-center tracking."""
def __init__(
self,
target_speed: float,
speed_pid: PID,
lateral_pid: PID,
heading_gain: float,
) -> None:
self.target_speed = target_speed
self.speed_pid = speed_pid
self.lateral_pid = lateral_pid
self.heading_gain = heading_gain
def reset(self) -> None:
self.speed_pid.reset()
self.lateral_pid.reset()
def act(self, env: gym.Env, dt: float) -> np.ndarray:
vehicle = env.unwrapped.vehicle
lane = vehicle.lane
lane_longitudinal, lane_lateral = lane.local_coordinates(vehicle.position)
lane_heading = lane.heading_at(lane_longitudinal)
heading_error = wrap_to_pi(lane_heading - vehicle.heading)
speed_error = self.target_speed - vehicle.speed
acceleration = self.speed_pid.step(speed_error, dt)
# highway-env uses positive lateral coordinates on one side of the lane.
# Steering toward -lane_lateral pulls the car back to the lane center.
lateral_error = -lane_lateral
steering = self.lateral_pid.step(lateral_error, dt) + self.heading_gain * heading_error
return np.array(
[
clamp(acceleration, -1.0, 1.0),
clamp(steering, -1.0, 1.0),
],
dtype=np.float32,
)
def make_env(
env_id: str,
render_mode: str | None,
duration: int,
other_vehicles: int,
) -> gym.Env:
env = gym.make(env_id, render_mode=render_mode)
env.unwrapped.configure(
{
"observation": {
"type": "Kinematics",
"vehicles_count": max(1, other_vehicles + 1),
"features": ["presence", "x", "y", "vx", "vy", "cos_h", "sin_h"],
"absolute": True,
"normalize": False,
},
"action": {
"type": "ContinuousAction",
"longitudinal": True,
"lateral": True,
"dynamical": True,
},
"controlled_vehicles": 1,
"other_vehicles": other_vehicles,
"duration": duration,
"simulation_frequency": 15,
"policy_frequency": 15,
"screen_width": 1000,
"screen_height": 800,
"centering_position": [0.5, 0.5],
"scaling": 7.0,
"show_trajectories": True,
"terminate_off_road": True,
}
)
return env
def run(args: argparse.Namespace) -> None:
render_mode = "human" if not args.no_render else None
env = make_env(args.env_id, render_mode, args.duration, args.traffic)
controller = CircuitPIDController(
target_speed=args.target_speed,
speed_pid=PID(args.speed_kp, args.speed_ki, args.speed_kd),
lateral_pid=PID(args.lateral_kp, args.lateral_ki, args.lateral_kd),
heading_gain=args.heading_gain,
)
dt = 1.0 / env.unwrapped.config["policy_frequency"]
obs, info = env.reset(seed=args.seed)
controller.reset()
total_reward = 0.0
for step in range(args.duration * env.unwrapped.config["policy_frequency"]):
action = controller.act(env, dt)
obs, reward, terminated, truncated, info = env.step(action)
total_reward += reward
if args.log_every and step % args.log_every == 0:
vehicle = env.unwrapped.vehicle
_, lateral = vehicle.lane.local_coordinates(vehicle.position)
print(
f"step={step:04d} speed={vehicle.speed:5.2f}m/s "
f"lateral_error={lateral:6.2f}m action={action.tolist()}"
)
if render_mode == "human":
env.render()
time.sleep(dt * args.slowdown)
if terminated or truncated:
break
print(f"finished: steps={step + 1}, total_reward={total_reward:.2f}")
env.close()
def parse_args() -> argparse.Namespace:
parser = argparse.ArgumentParser(
description="PID-controlled car on a highway-env racetrack circuit."
)
parser.add_argument(
"--env-id",
default="racetrack-v1",
choices=[
"racetrack-v0",
"racetrack-v1",
"racetrack-large-v0",
"racetrack-large-v1",
"racetrack-oval-v0",
"racetrack-oval-v1",
],
help="Racetrack variant.",
)
parser.add_argument("--target-speed", type=float, default=8.0, help="Target speed in m/s.")
parser.add_argument("--duration", type=int, default=90, help="Episode duration in seconds.")
parser.add_argument("--traffic", type=int, default=0, help="Number of other vehicles.")
parser.add_argument("--seed", type=int, default=7)
parser.add_argument("--no-render", action="store_true", help="Run headless.")
parser.add_argument("--slowdown", type=float, default=0.8, help="Rendering delay multiplier.")
parser.add_argument("--log-every", type=int, default=15, help="Print every N control steps; 0 disables.")
parser.add_argument("--speed-kp", type=float, default=0.35)
parser.add_argument("--speed-ki", type=float, default=0.03)
parser.add_argument("--speed-kd", type=float, default=0.02)
parser.add_argument("--lateral-kp", type=float, default=0.25)
parser.add_argument("--lateral-ki", type=float, default=0.00)
parser.add_argument("--lateral-kd", type=float, default=0.06)
parser.add_argument("--heading-gain", type=float, default=1.3)
return parser.parse_args()
if __name__ == "__main__":
run(parse_args())
結果はこちら。
サーキットっぽい所を車が走っていますね。とりあえず上出来です。
GUI追加
PID制御のゲインを毎回ソースコードで設定するのは面倒なので、GUIのつまみで速度やゲインを調整できるようにしてみます。
from __future__ import annotations
import argparse
import math
import threading
import time
from dataclasses import dataclass
from typing import Any
import gymnasium as gym
import highway_env # noqa: F401 - registers highway-env environments
import numpy as np
if hasattr(gym, "register_envs"):
gym.register_envs(highway_env)
def clamp(value: float, low: float, high: float) -> float:
return max(low, min(high, value))
def wrap_to_pi(angle: float) -> float:
return (angle + math.pi) % (2 * math.pi) - math.pi
@dataclass
class PID:
kp: float
ki: float
kd: float
integral_limit: float = 10.0
def __post_init__(self) -> None:
self.integral = 0.0
self.previous_error = 0.0
self.has_previous = False
def reset(self) -> None:
self.integral = 0.0
self.previous_error = 0.0
self.has_previous = False
def step(self, error: float, dt: float) -> float:
self.integral = clamp(
self.integral + error * dt,
-self.integral_limit,
self.integral_limit,
)
derivative = 0.0
if self.has_previous and dt > 0:
derivative = (error - self.previous_error) / dt
self.previous_error = error
self.has_previous = True
return self.kp * error + self.ki * self.integral + self.kd * derivative
class CircuitPIDController:
"""Two small PID loops: one for speed, one for lane-center tracking."""
def __init__(
self,
target_speed: float,
speed_pid: PID,
lateral_pid: PID,
heading_gain: float,
) -> None:
self.target_speed = target_speed
self.speed_pid = speed_pid
self.lateral_pid = lateral_pid
self.heading_gain = heading_gain
def reset(self) -> None:
self.speed_pid.reset()
self.lateral_pid.reset()
def update_params(self, params: dict[str, float]) -> None:
self.target_speed = params["target_speed"]
self.speed_pid.kp = params["speed_kp"]
self.speed_pid.ki = params["speed_ki"]
self.speed_pid.kd = params["speed_kd"]
self.lateral_pid.kp = params["lateral_kp"]
self.lateral_pid.ki = params["lateral_ki"]
self.lateral_pid.kd = params["lateral_kd"]
self.heading_gain = params["heading_gain"]
def act(self, env: gym.Env, dt: float) -> np.ndarray:
vehicle = env.unwrapped.vehicle
lane = vehicle.lane
lane_longitudinal, lane_lateral = lane.local_coordinates(vehicle.position)
lane_heading = lane.heading_at(lane_longitudinal)
heading_error = wrap_to_pi(lane_heading - vehicle.heading)
speed_error = self.target_speed - vehicle.speed
acceleration = self.speed_pid.step(speed_error, dt)
# highway-env uses positive lateral coordinates on one side of the lane.
# Steering toward -lane_lateral pulls the car back to the lane center.
lateral_error = -lane_lateral
steering = self.lateral_pid.step(lateral_error, dt) + self.heading_gain * heading_error
return np.array(
[
clamp(acceleration, -1.0, 1.0),
clamp(steering, -1.0, 1.0),
],
dtype=np.float32,
)
class SharedControls:
def __init__(self, args: argparse.Namespace) -> None:
self._lock = threading.Lock()
self.params = {
"target_speed": args.target_speed,
"speed_kp": args.speed_kp,
"speed_ki": args.speed_ki,
"speed_kd": args.speed_kd,
"lateral_kp": args.lateral_kp,
"lateral_ki": args.lateral_ki,
"lateral_kd": args.lateral_kd,
"heading_gain": args.heading_gain,
"slowdown": args.slowdown,
}
self.metrics: dict[str, float] = {
"step": 0.0,
"speed": 0.0,
"lateral_error": 0.0,
"acceleration": 0.0,
"steering": 0.0,
}
self.reset_requested = False
self.quit_requested = False
def set_param(self, name: str, value: float) -> None:
with self._lock:
self.params[name] = value
def request_reset(self) -> None:
with self._lock:
self.reset_requested = True
def request_quit(self) -> None:
with self._lock:
self.quit_requested = True
def snapshot(self) -> tuple[dict[str, float], bool, bool]:
with self._lock:
params = dict(self.params)
reset_requested = self.reset_requested
quit_requested = self.quit_requested
self.reset_requested = False
return params, reset_requested, quit_requested
def update_metrics(self, **metrics: float) -> None:
with self._lock:
self.metrics.update(metrics)
def metrics_snapshot(self) -> dict[str, float]:
with self._lock:
return dict(self.metrics)
def start_control_panel(shared: SharedControls) -> threading.Thread:
thread = threading.Thread(target=run_control_panel, args=(shared,), daemon=True)
thread.start()
return thread
def run_control_panel(shared: SharedControls) -> None:
import tkinter as tk
from tkinter import ttk
root = tk.Tk()
root.title("PID controls")
root.geometry("420x620")
main = ttk.Frame(root, padding=12)
main.pack(fill="both", expand=True)
status = tk.StringVar(value="starting...")
ttk.Label(main, textvariable=status, justify="left").pack(fill="x", pady=(0, 12))
initial = shared.snapshot()[0]
def add_slider(
name: str,
label: str,
low: float,
high: float,
step: float,
) -> None:
row = ttk.Frame(main)
row.pack(fill="x", pady=5)
value = tk.DoubleVar(value=initial[name])
text = tk.StringVar(value=f"{label}: {value.get():.3f}")
def changed(raw_value: Any) -> None:
rounded = round(float(raw_value) / step) * step
shared.set_param(name, rounded)
text.set(f"{label}: {rounded:.3f}")
ttk.Label(row, textvariable=text, width=24).pack(side="left")
scale = ttk.Scale(row, from_=low, to=high, orient="horizontal", variable=value, command=changed)
scale.pack(side="left", fill="x", expand=True)
add_slider("target_speed", "target speed [m/s]", 0.0, 20.0, 0.1)
add_slider("speed_kp", "speed Kp", 0.0, 2.0, 0.01)
add_slider("speed_ki", "speed Ki", 0.0, 0.5, 0.001)
add_slider("speed_kd", "speed Kd", 0.0, 0.5, 0.001)
add_slider("lateral_kp", "lateral Kp", 0.0, 2.0, 0.01)
add_slider("lateral_ki", "lateral Ki", 0.0, 0.5, 0.001)
add_slider("lateral_kd", "lateral Kd", 0.0, 0.5, 0.001)
add_slider("heading_gain", "heading gain", 0.0, 4.0, 0.01)
add_slider("slowdown", "render slowdown", 0.0, 2.0, 0.01)
buttons = ttk.Frame(main)
buttons.pack(fill="x", pady=(16, 0))
ttk.Button(buttons, text="Reset PID memory", command=shared.request_reset).pack(side="left")
ttk.Button(buttons, text="Quit", command=shared.request_quit).pack(side="right")
def refresh_status() -> None:
metrics = shared.metrics_snapshot()
status.set(
"step={step:.0f}\n"
"speed={speed:.2f} m/s\n"
"lateral_error={lateral_error:.2f} m\n"
"accel={acceleration:.3f}, steering={steering:.3f}".format(**metrics)
)
root.after(100, refresh_status)
def close() -> None:
shared.request_quit()
root.destroy()
root.protocol("WM_DELETE_WINDOW", close)
refresh_status()
root.mainloop()
def make_env(
env_id: str,
render_mode: str | None,
duration: int,
other_vehicles: int,
) -> gym.Env:
env = gym.make(env_id, render_mode=render_mode)
env.unwrapped.configure(
{
"observation": {
"type": "Kinematics",
"vehicles_count": max(1, other_vehicles + 1),
"features": ["presence", "x", "y", "vx", "vy", "cos_h", "sin_h"],
"absolute": True,
"normalize": False,
},
"action": {
"type": "ContinuousAction",
"longitudinal": True,
"lateral": True,
"dynamical": True,
},
"controlled_vehicles": 1,
"other_vehicles": other_vehicles,
"duration": duration,
"simulation_frequency": 15,
"policy_frequency": 15,
"screen_width": 1000,
"screen_height": 800,
"centering_position": [0.5, 0.5],
"scaling": 7.0,
"show_trajectories": True,
"terminate_off_road": True,
}
)
return env
def run(args: argparse.Namespace) -> None:
render_mode = "human" if not args.no_render else None
env = make_env(args.env_id, render_mode, args.duration, args.traffic)
shared = SharedControls(args)
if args.gui:
start_control_panel(shared)
controller = CircuitPIDController(
target_speed=args.target_speed,
speed_pid=PID(args.speed_kp, args.speed_ki, args.speed_kd),
lateral_pid=PID(args.lateral_kp, args.lateral_ki, args.lateral_kd),
heading_gain=args.heading_gain,
)
dt = 1.0 / env.unwrapped.config["policy_frequency"]
obs, info = env.reset(seed=args.seed)
controller.reset()
total_reward = 0.0
for step in range(args.duration * env.unwrapped.config["policy_frequency"]):
params, reset_requested, quit_requested = shared.snapshot()
controller.update_params(params)
if reset_requested:
controller.reset()
if quit_requested:
break
action = controller.act(env, dt)
obs, reward, terminated, truncated, info = env.step(action)
total_reward += reward
vehicle = env.unwrapped.vehicle
_, lateral = vehicle.lane.local_coordinates(vehicle.position)
shared.update_metrics(
step=float(step),
speed=float(vehicle.speed),
lateral_error=float(lateral),
acceleration=float(action[0]),
steering=float(action[1]),
)
if args.log_every and step % args.log_every == 0:
print(
f"step={step:04d} speed={vehicle.speed:5.2f}m/s "
f"lateral_error={lateral:6.2f}m action={action.tolist()}"
)
if render_mode == "human":
env.render()
time.sleep(dt * params["slowdown"])
if terminated or truncated:
break
print(f"finished: steps={step + 1}, total_reward={total_reward:.2f}")
env.close()
def parse_args() -> argparse.Namespace:
parser = argparse.ArgumentParser(
description="PID-controlled car on a highway-env racetrack circuit."
)
parser.add_argument(
"--env-id",
default="racetrack-v1",
choices=[
"racetrack-v0",
"racetrack-v1",
"racetrack-large-v0",
"racetrack-large-v1",
"racetrack-oval-v0",
"racetrack-oval-v1",
],
help="Racetrack variant.",
)
parser.add_argument("--target-speed", type=float, default=8.0, help="Target speed in m/s.")
parser.add_argument("--duration", type=int, default=90, help="Episode duration in seconds.")
parser.add_argument("--traffic", type=int, default=0, help="Number of other vehicles.")
parser.add_argument("--seed", type=int, default=7)
parser.add_argument("--no-render", action="store_true", help="Run headless.")
parser.add_argument("--gui", action=argparse.BooleanOptionalAction, default=True)
parser.add_argument("--slowdown", type=float, default=0.8, help="Rendering delay multiplier.")
parser.add_argument("--log-every", type=int, default=15, help="Print every N control steps; 0 disables.")
parser.add_argument("--speed-kp", type=float, default=0.35)
parser.add_argument("--speed-ki", type=float, default=0.03)
parser.add_argument("--speed-kd", type=float, default=0.02)
parser.add_argument("--lateral-kp", type=float, default=0.25)
parser.add_argument("--lateral-ki", type=float, default=0.00)
parser.add_argument("--lateral-kd", type=float, default=0.06)
parser.add_argument("--heading-gain", type=float, default=1.3)
return parser.parse_args()
if __name__ == "__main__":
run(parse_args())
結果はこちら。
GUIで調整できるようになりました。
ステアリング、アクセルペダル、ブレーキペダル
車を走らせることを想定すると、現実的にはステアリング、アクセルペダル、ブレーキペダルの3つを操作することになるはずです、
今はPIDで速度と先頭の方向を調整しているだけなので少し味気ないですね。
現実的に寄せてみることにします。
from __future__ import annotations
import argparse
import math
import threading
import time
from dataclasses import dataclass
from typing import Any
import gymnasium as gym
import highway_env # noqa: F401 - registers highway-env environments
import numpy as np
if hasattr(gym, "register_envs"):
gym.register_envs(highway_env)
def clamp(value: float, low: float, high: float) -> float:
return max(low, min(high, value))
def wrap_to_pi(angle: float) -> float:
return (angle + math.pi) % (2 * math.pi) - math.pi
@dataclass
class PID:
kp: float
ki: float
kd: float
integral_limit: float = 10.0
def __post_init__(self) -> None:
self.integral = 0.0
self.previous_error = 0.0
self.has_previous = False
def reset(self) -> None:
self.integral = 0.0
self.previous_error = 0.0
self.has_previous = False
def step(self, error: float, dt: float) -> float:
self.integral = clamp(
self.integral + error * dt,
-self.integral_limit,
self.integral_limit,
)
derivative = 0.0
if self.has_previous and dt > 0:
derivative = (error - self.previous_error) / dt
self.previous_error = error
self.has_previous = True
return self.kp * error + self.ki * self.integral + self.kd * derivative
@dataclass(frozen=True)
class VehicleCommand:
accelerator: float
brake: float
steering: float
virtual_drag: float
def as_highway_env_action(self) -> np.ndarray:
# highway-env ContinuousAction receives one longitudinal command.
# Keep pedals separate for learning, then add a small resistance model.
longitudinal = self.accelerator - self.brake - self.virtual_drag
return np.array([longitudinal, self.steering], dtype=np.float32)
class CircuitPIDController:
"""Speed planning plus two PID loops for speed and lane-center tracking."""
def __init__(
self,
target_speed: float,
speed_pid: PID,
lateral_pid: PID,
heading_gain: float,
cruise_throttle: float,
curve_lookahead: float,
rolling_drag: float,
aero_drag: float,
corner_drag: float,
brake_deadband: float,
pedal_smoothing: float,
) -> None:
self.target_speed = target_speed
self.speed_pid = speed_pid
self.lateral_pid = lateral_pid
self.heading_gain = heading_gain
self.cruise_throttle = cruise_throttle
self.curve_lookahead = curve_lookahead
self.rolling_drag = rolling_drag
self.aero_drag = aero_drag
self.corner_drag = corner_drag
self.brake_deadband = brake_deadband
self.pedal_smoothing = pedal_smoothing
self.speed_setpoint = target_speed
self.curve_severity = 0.0
self.virtual_drag = 0.0
self.previous_accelerator = 0.0
self.previous_brake = 0.0
self.previous_steering = 0.0
def reset(self) -> None:
self.speed_pid.reset()
self.lateral_pid.reset()
self.previous_accelerator = 0.0
self.previous_brake = 0.0
self.previous_steering = 0.0
def update_params(self, params: dict[str, float]) -> None:
self.target_speed = params["target_speed"]
self.speed_pid.kp = params["speed_kp"]
self.speed_pid.ki = params["speed_ki"]
self.speed_pid.kd = params["speed_kd"]
self.lateral_pid.kp = params["lateral_kp"]
self.lateral_pid.ki = params["lateral_ki"]
self.lateral_pid.kd = params["lateral_kd"]
self.heading_gain = params["heading_gain"]
self.cruise_throttle = params["cruise_throttle"]
self.curve_lookahead = params["curve_lookahead"]
self.rolling_drag = params["rolling_drag"]
self.aero_drag = params["aero_drag"]
self.corner_drag = params["corner_drag"]
self.brake_deadband = params["brake_deadband"]
self.pedal_smoothing = params["pedal_smoothing"]
def act(self, env: gym.Env, dt: float) -> VehicleCommand:
vehicle = env.unwrapped.vehicle
lane = vehicle.lane
lane_longitudinal, lane_lateral = lane.local_coordinates(vehicle.position)
lane_heading = lane.heading_at(lane_longitudinal)
heading_error = wrap_to_pi(lane_heading - vehicle.heading)
self.curve_severity = self.estimate_curve_severity(lane, lane_longitudinal, lane_heading)
self.speed_setpoint = self.target_speed
self.virtual_drag = self.estimate_virtual_drag(vehicle.speed)
speed_error = self.speed_setpoint - vehicle.speed
longitudinal_output = clamp(self.speed_pid.step(speed_error, dt), -1.0, 1.0)
overspeed = max(0.0, vehicle.speed - self.speed_setpoint - self.brake_deadband)
brake_raw = clamp(max(-longitudinal_output, overspeed / 5.0), 0.0, 1.0)
accelerator_raw = clamp(
self.virtual_drag + self.cruise_throttle + max(0.0, longitudinal_output) - 0.7 * brake_raw,
0.0,
1.0,
)
# highway-env uses positive lateral coordinates on one side of the lane.
# Steering toward -lane_lateral pulls the car back to the lane center.
lateral_error = -lane_lateral
steering = self.lateral_pid.step(lateral_error, dt) + self.heading_gain * heading_error
steering = clamp(steering, -1.0, 1.0)
accelerator = self.smooth(self.previous_accelerator, accelerator_raw, dt)
brake = self.smooth(self.previous_brake, brake_raw, dt)
steering = self.smooth(self.previous_steering, steering, dt)
self.previous_accelerator = accelerator
self.previous_brake = brake
self.previous_steering = steering
return VehicleCommand(
accelerator=accelerator,
brake=brake,
steering=steering,
virtual_drag=self.virtual_drag,
)
def estimate_virtual_drag(self, speed: float) -> float:
speed_ratio = clamp(speed / max(self.target_speed, 1.0), 0.0, 2.0)
rolling = self.rolling_drag
aero = self.aero_drag * speed_ratio * speed_ratio
corner = self.corner_drag * self.curve_severity * speed_ratio
return clamp(rolling + aero + corner, 0.0, 0.9)
def smooth(self, previous: float, current: float, dt: float) -> float:
if self.pedal_smoothing <= 0:
return current
alpha = clamp(dt / (self.pedal_smoothing + dt), 0.0, 1.0)
return previous + alpha * (current - previous)
def estimate_curve_severity(self, lane: Any, longitudinal: float, heading: float) -> float:
if self.curve_lookahead <= 0:
return 0.0
max_heading_change = 0.0
for ratio in (0.33, 0.66, 1.0):
try:
future_heading = lane.heading_at(longitudinal + self.curve_lookahead * ratio)
except Exception:
continue
max_heading_change = max(max_heading_change, abs(wrap_to_pi(future_heading - heading)))
return max_heading_change
class SharedControls:
def __init__(self, args: argparse.Namespace) -> None:
self._lock = threading.Lock()
self.params = {
"target_speed": args.target_speed,
"speed_kp": args.speed_kp,
"speed_ki": args.speed_ki,
"speed_kd": args.speed_kd,
"lateral_kp": args.lateral_kp,
"lateral_ki": args.lateral_ki,
"lateral_kd": args.lateral_kd,
"heading_gain": args.heading_gain,
"cruise_throttle": args.cruise_throttle,
"curve_lookahead": args.curve_lookahead,
"rolling_drag": args.rolling_drag,
"aero_drag": args.aero_drag,
"corner_drag": args.corner_drag,
"brake_deadband": args.brake_deadband,
"pedal_smoothing": args.pedal_smoothing,
"slowdown": args.slowdown,
}
self.metrics: dict[str, float] = {
"step": 0.0,
"speed": 0.0,
"speed_setpoint": args.target_speed,
"curve_severity": 0.0,
"virtual_drag": 0.0,
"lateral_error": 0.0,
"accelerator": 0.0,
"brake": 0.0,
"steering": 0.0,
}
self.reset_requested = False
self.quit_requested = False
def set_param(self, name: str, value: float) -> None:
with self._lock:
self.params[name] = value
def request_reset(self) -> None:
with self._lock:
self.reset_requested = True
def request_quit(self) -> None:
with self._lock:
self.quit_requested = True
def snapshot(self) -> tuple[dict[str, float], bool, bool]:
with self._lock:
params = dict(self.params)
reset_requested = self.reset_requested
quit_requested = self.quit_requested
self.reset_requested = False
return params, reset_requested, quit_requested
def update_metrics(self, **metrics: float) -> None:
with self._lock:
self.metrics.update(metrics)
def metrics_snapshot(self) -> dict[str, float]:
with self._lock:
return dict(self.metrics)
def start_control_panel(shared: SharedControls) -> threading.Thread:
thread = threading.Thread(target=run_control_panel, args=(shared,), daemon=True)
thread.start()
return thread
def run_control_panel(shared: SharedControls) -> None:
import tkinter as tk
from tkinter import ttk
root = tk.Tk()
root.title("PID driving controls")
root.geometry("760x650")
root.minsize(680, 560)
style = ttk.Style(root)
style.configure("Title.TLabel", font=("Segoe UI", 14, "bold"))
style.configure("Metric.TLabel", font=("Consolas", 11))
style.configure("Section.TLabelframe.Label", font=("Segoe UI", 10, "bold"))
main = ttk.Frame(root, padding=12)
main.pack(fill="both", expand=True)
header = ttk.Frame(main)
header.pack(fill="x", pady=(0, 8))
ttk.Label(header, text="PID Driving Monitor", style="Title.TLabel").pack(side="left")
status = tk.StringVar(value="starting...")
ttk.Label(header, textvariable=status, style="Metric.TLabel", justify="right").pack(side="right")
visual_frame = ttk.LabelFrame(main, text="Live operation", padding=10, style="Section.TLabelframe")
visual_frame.pack(fill="x", pady=(0, 10))
dashboard = tk.Canvas(
visual_frame,
width=720,
height=230,
bg="#111827",
highlightthickness=0,
)
dashboard.pack(fill="x", expand=True)
initial = shared.snapshot()[0]
controls = ttk.Frame(main)
controls.pack(fill="both", expand=True)
class CollapsibleSection:
def __init__(self, parent: ttk.Frame, title: str, open_by_default: bool = False) -> None:
self.is_open = tk.BooleanVar(value=open_by_default)
self.frame = ttk.Frame(parent)
self.frame.pack(fill="x", pady=3)
self.button = ttk.Button(self.frame, command=self.toggle)
self.button.pack(fill="x")
self.body = ttk.Frame(self.frame, padding=(10, 8, 10, 4))
self.title = title
self.refresh()
def toggle(self) -> None:
self.is_open.set(not self.is_open.get())
self.refresh()
def refresh(self) -> None:
mark = "-" if self.is_open.get() else "+"
self.button.configure(text=f"{mark} {self.title}")
if self.is_open.get():
self.body.pack(fill="x")
else:
self.body.pack_forget()
speed_section = CollapsibleSection(controls, "Speed PID", open_by_default=True)
lateral_section = CollapsibleSection(controls, "Lateral PID", open_by_default=False)
vehicle_section = CollapsibleSection(controls, "Vehicle model", open_by_default=False)
runtime_section = CollapsibleSection(controls, "Runtime", open_by_default=True)
def add_slider(
parent: ttk.Frame,
name: str,
label: str,
low: float,
high: float,
step: float,
) -> None:
row = ttk.Frame(parent)
row.pack(fill="x", pady=3)
value = tk.DoubleVar(value=initial[name])
text = tk.StringVar(value=f"{label}: {value.get():.3f}")
def changed(raw_value: Any) -> None:
rounded = round(float(raw_value) / step) * step
shared.set_param(name, rounded)
text.set(f"{label}: {rounded:.3f}")
ttk.Label(row, textvariable=text, width=24).pack(side="left")
scale = ttk.Scale(row, from_=low, to=high, orient="horizontal", variable=value, command=changed)
scale.pack(side="left", fill="x", expand=True)
add_slider(speed_section.body, "target_speed", "target speed [m/s]", 0.0, 20.0, 0.1)
add_slider(speed_section.body, "speed_kp", "speed Kp", 0.0, 2.0, 0.01)
add_slider(speed_section.body, "speed_ki", "speed Ki", 0.0, 0.5, 0.001)
add_slider(speed_section.body, "speed_kd", "speed Kd", 0.0, 0.5, 0.001)
add_slider(lateral_section.body, "lateral_kp", "lateral Kp", 0.0, 2.0, 0.01)
add_slider(lateral_section.body, "lateral_ki", "lateral Ki", 0.0, 0.5, 0.001)
add_slider(lateral_section.body, "lateral_kd", "lateral Kd", 0.0, 0.5, 0.001)
add_slider(lateral_section.body, "heading_gain", "heading gain", 0.0, 4.0, 0.01)
add_slider(vehicle_section.body, "cruise_throttle", "cruise throttle", 0.0, 0.5, 0.01)
add_slider(vehicle_section.body, "curve_lookahead", "curve lookahead [m]", 0.0, 80.0, 1.0)
add_slider(vehicle_section.body, "rolling_drag", "rolling drag", 0.0, 0.3, 0.005)
add_slider(vehicle_section.body, "aero_drag", "aero drag", 0.0, 0.5, 0.005)
add_slider(vehicle_section.body, "corner_drag", "corner drag", 0.0, 1.5, 0.01)
add_slider(vehicle_section.body, "brake_deadband", "brake deadband [m/s]", 0.0, 4.0, 0.1)
add_slider(vehicle_section.body, "pedal_smoothing", "pedal smoothing [s]", 0.0, 2.0, 0.05)
add_slider(runtime_section.body, "slowdown", "render slowdown", 0.0, 2.0, 0.01)
buttons = ttk.Frame(main)
buttons.pack(fill="x", pady=(16, 0))
ttk.Button(buttons, text="Reset PID memory", command=shared.request_reset).pack(side="left")
ttk.Button(buttons, text="Quit", command=shared.request_quit).pack(side="right")
def draw_pedal(
x: int,
y: int,
width: int,
height: int,
value: float,
color: str,
label: str,
) -> None:
value = clamp(value, 0.0, 1.0)
fill_height = int(height * value)
dashboard.create_rectangle(x, y, x + width, y + height, outline="#4b5563", width=2)
dashboard.create_rectangle(
x + 4,
y + height - fill_height + 4,
x + width - 4,
y + height - 4,
fill=color,
outline="",
)
dashboard.create_text(
x + width / 2,
y + height + 18,
text=label,
fill="#e5e7eb",
font=("Segoe UI", 10, "bold"),
)
dashboard.create_text(
x + width / 2,
y - 14,
text=f"{value:.2f}",
fill=color,
font=("Consolas", 12, "bold"),
)
def draw_steering(center_x: int, center_y: int, radius: int, steering: float) -> None:
steering = clamp(steering, -1.0, 1.0)
angle = steering * math.radians(90)
dashboard.create_oval(
center_x - radius,
center_y - radius,
center_x + radius,
center_y + radius,
outline="#e5e7eb",
width=8,
)
dashboard.create_oval(
center_x - 9,
center_y - 9,
center_x + 9,
center_y + 9,
fill="#e5e7eb",
outline="",
)
for base_angle in (math.radians(-90), math.radians(30), math.radians(150)):
spoke = base_angle + angle
end_x = center_x + math.cos(spoke) * (radius - 8)
end_y = center_y + math.sin(spoke) * (radius - 8)
dashboard.create_line(center_x, center_y, end_x, end_y, fill="#e5e7eb", width=5)
needle_x = center_x + math.sin(angle) * (radius + 24)
needle_y = center_y - math.cos(angle) * (radius + 24)
dashboard.create_line(center_x, center_y, needle_x, needle_y, fill="#38bdf8", width=4, arrow=tk.LAST)
dashboard.create_text(
center_x,
center_y + radius + 28,
text=f"STEERING {steering:+.2f}",
fill="#38bdf8",
font=("Consolas", 13, "bold"),
)
def draw_dashboard(metrics: dict[str, float]) -> None:
dashboard.delete("all")
dashboard.create_text(
24,
22,
text=f"speed {metrics['speed']:.2f} m/s",
anchor="w",
fill="#f9fafb",
font=("Consolas", 14, "bold"),
)
dashboard.create_text(
24,
48,
text=f"speed setpoint {metrics['speed_setpoint']:.2f} m/s",
anchor="w",
fill="#cbd5e1",
font=("Consolas", 12),
)
dashboard.create_text(
24,
72,
text=f"lateral error {metrics['lateral_error']:+.2f} m",
anchor="w",
fill="#cbd5e1",
font=("Consolas", 12),
)
dashboard.create_text(
24,
96,
text=f"curve severity {metrics['curve_severity']:.2f} rad",
anchor="w",
fill="#cbd5e1",
font=("Consolas", 12),
)
dashboard.create_text(
24,
120,
text=f"virtual drag {metrics['virtual_drag']:.3f}",
anchor="w",
fill="#cbd5e1",
font=("Consolas", 12),
)
dashboard.create_line(360, 36, 660, 36, fill="#374151", width=2)
dashboard.create_text(360, 20, text="left", anchor="w", fill="#9ca3af", font=("Segoe UI", 9))
dashboard.create_text(660, 20, text="right", anchor="e", fill="#9ca3af", font=("Segoe UI", 9))
steering_x = 510 + metrics["steering"] * 120
dashboard.create_line(510, 30, steering_x, 30, fill="#38bdf8", width=5)
dashboard.create_oval(steering_x - 7, 23, steering_x + 7, 37, fill="#38bdf8", outline="")
draw_steering(230, 140, 60, metrics["steering"])
draw_pedal(455, 78, 70, 118, metrics["accelerator"], "#22c55e", "ACCEL")
draw_pedal(570, 78, 70, 118, metrics["brake"], "#ef4444", "BRAKE")
def refresh_status() -> None:
metrics = shared.metrics_snapshot()
draw_dashboard(metrics)
status.set(
"step={step:.0f}\n"
"speed_setpoint={speed_setpoint:.2f} m/s\n"
"accelerator={accelerator:.3f}, brake={brake:.3f}\n"
"steering={steering:.3f}".format(**metrics)
)
root.after(100, refresh_status)
def close() -> None:
shared.request_quit()
root.destroy()
root.protocol("WM_DELETE_WINDOW", close)
refresh_status()
root.mainloop()
def make_env(
env_id: str,
render_mode: str | None,
duration: int,
other_vehicles: int,
) -> gym.Env:
env = gym.make(env_id, render_mode=render_mode)
env.unwrapped.configure(
{
"observation": {
"type": "Kinematics",
"vehicles_count": max(1, other_vehicles + 1),
"features": ["presence", "x", "y", "vx", "vy", "cos_h", "sin_h"],
"absolute": True,
"normalize": False,
},
"action": {
"type": "ContinuousAction",
"longitudinal": True,
"lateral": True,
"dynamical": True,
},
"controlled_vehicles": 1,
"other_vehicles": other_vehicles,
"duration": duration,
"simulation_frequency": 15,
"policy_frequency": 15,
"screen_width": 1000,
"screen_height": 800,
"centering_position": [0.5, 0.5],
"scaling": 7.0,
"show_trajectories": True,
"terminate_off_road": True,
}
)
return env
def run(args: argparse.Namespace) -> None:
render_mode = "human" if not args.no_render else None
env = make_env(args.env_id, render_mode, args.duration, args.traffic)
shared = SharedControls(args)
if args.gui:
start_control_panel(shared)
controller = CircuitPIDController(
target_speed=args.target_speed,
speed_pid=PID(args.speed_kp, args.speed_ki, args.speed_kd),
lateral_pid=PID(args.lateral_kp, args.lateral_ki, args.lateral_kd),
heading_gain=args.heading_gain,
cruise_throttle=args.cruise_throttle,
curve_lookahead=args.curve_lookahead,
rolling_drag=args.rolling_drag,
aero_drag=args.aero_drag,
corner_drag=args.corner_drag,
brake_deadband=args.brake_deadband,
pedal_smoothing=args.pedal_smoothing,
)
dt = 1.0 / env.unwrapped.config["policy_frequency"]
obs, info = env.reset(seed=args.seed)
controller.reset()
total_reward = 0.0
for step in range(args.duration * env.unwrapped.config["policy_frequency"]):
params, reset_requested, quit_requested = shared.snapshot()
controller.update_params(params)
if reset_requested:
controller.reset()
if quit_requested:
break
command = controller.act(env, dt)
action = command.as_highway_env_action()
obs, reward, terminated, truncated, info = env.step(action)
total_reward += reward
vehicle = env.unwrapped.vehicle
_, lateral = vehicle.lane.local_coordinates(vehicle.position)
shared.update_metrics(
step=float(step),
speed=float(vehicle.speed),
speed_setpoint=controller.speed_setpoint,
curve_severity=controller.curve_severity,
virtual_drag=controller.virtual_drag,
lateral_error=float(lateral),
accelerator=command.accelerator,
brake=command.brake,
steering=command.steering,
)
if args.log_every and step % args.log_every == 0:
print(
f"step={step:04d} speed={vehicle.speed:5.2f}m/s "
f"setpoint={controller.speed_setpoint:5.2f}m/s "
f"curve={controller.curve_severity:.2f}rad "
f"drag={controller.virtual_drag:.3f} "
f"lateral_error={lateral:6.2f}m "
f"accelerator={command.accelerator:.3f} "
f"brake={command.brake:.3f} "
f"steering={command.steering:.3f}"
)
if render_mode == "human":
env.render()
time.sleep(dt * params["slowdown"])
if terminated or truncated:
break
print(f"finished: steps={step + 1}, total_reward={total_reward:.2f}")
env.close()
def parse_args() -> argparse.Namespace:
parser = argparse.ArgumentParser(
description="PID-controlled car on a highway-env racetrack circuit."
)
parser.add_argument(
"--env-id",
default="racetrack-v1",
choices=[
"racetrack-v0",
"racetrack-v1",
"racetrack-large-v0",
"racetrack-large-v1",
"racetrack-oval-v0",
"racetrack-oval-v1",
],
help="Racetrack variant.",
)
parser.add_argument("--target-speed", type=float, default=8.0, help="Target speed in m/s.")
parser.add_argument("--duration", type=int, default=90, help="Episode duration in seconds.")
parser.add_argument("--traffic", type=int, default=0, help="Number of other vehicles.")
parser.add_argument("--seed", type=int, default=7)
parser.add_argument("--no-render", action="store_true", help="Run headless.")
parser.add_argument("--gui", action=argparse.BooleanOptionalAction, default=True)
parser.add_argument("--slowdown", type=float, default=0.8, help="Rendering delay multiplier.")
parser.add_argument("--log-every", type=int, default=15, help="Print every N control steps; 0 disables.")
parser.add_argument("--speed-kp", type=float, default=0.35)
parser.add_argument("--speed-ki", type=float, default=0.03)
parser.add_argument("--speed-kd", type=float, default=0.02)
parser.add_argument("--lateral-kp", type=float, default=0.25)
parser.add_argument("--lateral-ki", type=float, default=0.00)
parser.add_argument("--lateral-kd", type=float, default=0.06)
parser.add_argument("--heading-gain", type=float, default=1.3)
parser.add_argument("--cruise-throttle", type=float, default=0.02)
parser.add_argument("--curve-lookahead", type=float, default=35.0)
parser.add_argument("--rolling-drag", type=float, default=0.06)
parser.add_argument("--aero-drag", type=float, default=0.08)
parser.add_argument("--corner-drag", type=float, default=0.35)
parser.add_argument("--brake-deadband", type=float, default=0.8)
parser.add_argument("--pedal-smoothing", type=float, default=0.45)
return parser.parse_args()
if __name__ == "__main__":
run(parse_args())
結果はこちら。
ハンドルと文字が被っていますが、だいぶいい感じになりましたね。