Spaces:
Sleeping
Sleeping
| """2D kinematic bicycle simulator with ray-cast lidar and collision checks.""" | |
| from __future__ import annotations | |
| from dataclasses import dataclass, field | |
| from typing import List, Tuple | |
| import math | |
| import numpy as np | |
| class Car: | |
| x: float = 0.0 | |
| y: float = 0.0 | |
| heading: float = 0.0 # radians | |
| speed: float = 0.0 # m/s | |
| length: float = 4.5 | |
| width: float = 1.8 | |
| class Obstacle: | |
| x: float | |
| y: float | |
| w: float | |
| h: float # half-extents aligned to world axes | |
| def contains(self, px: float, py: float) -> bool: | |
| return (abs(px - self.x) <= self.w) and (abs(py - self.y) <= self.h) | |
| def ray_hit(self, ox: float, oy: float, dx: float, dy: float, max_dist: float) -> float: | |
| """Axis-aligned box slab test; returns distance along ray or max_dist.""" | |
| tmin = 0.0 | |
| tmax = max_dist | |
| for (o, d, c, e) in ((ox, dx, self.x, self.w), (oy, dy, self.y, self.h)): | |
| if abs(d) < 1e-9: | |
| if o < c - e or o > c + e: | |
| return max_dist | |
| else: | |
| t1 = (c - e - o) / d | |
| t2 = (c + e - o) / d | |
| lo, hi = min(t1, t2), max(t1, t2) | |
| tmin = max(tmin, lo) | |
| tmax = min(tmax, hi) | |
| if tmin > tmax: | |
| return max_dist | |
| return tmin if tmin > 0 else max_dist | |
| class World: | |
| """Straight-road world with optional obstacles and a goal position.""" | |
| road_half_width: float = 4.0 # lane from y=-4 to y=+4 (two 4m half-lanes) | |
| road_length: float = 200.0 # x in [0, 200] | |
| target_speed: float = 10.0 # m/s (~36 km/h) | |
| max_steer: float = 0.6 # radians | |
| max_accel: float = 3.0 # m/s^2 | |
| max_speed: float = 20.0 | |
| dt: float = 0.1 | |
| obstacles: List[Obstacle] = field(default_factory=list) | |
| goal: Tuple[float, float] = (380.0, 0.0) | |
| goal_heading: float = 0.0 | |
| goal_radius: float = 2.0 | |
| LIDAR_RAYS = 12 | |
| LIDAR_MAX = 30.0 | |
| def lidar_scan(car: Car, world: World) -> np.ndarray: | |
| """Cast LIDAR_RAYS evenly in front-biased fan around the car heading.""" | |
| angles = np.linspace(-math.pi / 2, math.pi / 2, LIDAR_RAYS) | |
| scans = np.full(LIDAR_RAYS, LIDAR_MAX, dtype=np.float32) | |
| for i, a in enumerate(angles): | |
| theta = car.heading + a | |
| dx, dy = math.cos(theta), math.sin(theta) | |
| dist = LIDAR_MAX | |
| # Road boundary walls (y = ±road_half_width) as virtual obstacles | |
| # Solve for t where car.y + t*dy = ±road_half_width | |
| for wall_y in (world.road_half_width, -world.road_half_width): | |
| if abs(dy) > 1e-6: | |
| t = (wall_y - car.y) / dy | |
| if 0 < t < dist: | |
| dist = t | |
| # Front/back walls | |
| for wall_x in (0.0, world.road_length): | |
| if abs(dx) > 1e-6: | |
| t = (wall_x - car.x) / dx | |
| if 0 < t < dist: | |
| dist = t | |
| # Obstacles | |
| for obs in world.obstacles: | |
| t = obs.ray_hit(car.x, car.y, dx, dy, dist) | |
| if t < dist: | |
| dist = t | |
| scans[i] = dist | |
| return np.clip(scans / LIDAR_MAX, 0.0, 1.0) | |
| def step_dynamics(car: Car, throttle: float, steering: float, world: World) -> Car: | |
| """Kinematic bicycle update (rear-axle reference).""" | |
| throttle = float(np.clip(throttle, -1.0, 1.0)) | |
| steering = float(np.clip(steering, -1.0, 1.0)) | |
| steer_angle = steering * world.max_steer | |
| accel = throttle * world.max_accel | |
| new_speed = car.speed + accel * world.dt | |
| new_speed = float(np.clip(new_speed, -0.3 * world.max_speed, world.max_speed)) | |
| # Bicycle model | |
| beta = 0.0 # rear-axle ref => slip angle ~ 0 | |
| dx = new_speed * math.cos(car.heading + beta) * world.dt | |
| dy = new_speed * math.sin(car.heading + beta) * world.dt | |
| dtheta = (new_speed / car.length) * math.tan(steer_angle) * world.dt | |
| return Car( | |
| x=car.x + dx, | |
| y=car.y + dy, | |
| heading=_wrap_angle(car.heading + dtheta), | |
| speed=new_speed, | |
| length=car.length, | |
| width=car.width, | |
| ) | |
| def _wrap_angle(a: float) -> float: | |
| return (a + math.pi) % (2 * math.pi) - math.pi | |
| def car_collides(car: Car, world: World) -> bool: | |
| # Sample 4 corners of the car AABB (approx, ignores heading rotation for speed) | |
| hl, hw = car.length / 2, car.width / 2 | |
| corners = [] | |
| cos_h, sin_h = math.cos(car.heading), math.sin(car.heading) | |
| for sx, sy in ((hl, hw), (hl, -hw), (-hl, hw), (-hl, -hw)): | |
| cx = car.x + sx * cos_h - sy * sin_h | |
| cy = car.y + sx * sin_h + sy * cos_h | |
| corners.append((cx, cy)) | |
| for (cx, cy) in corners: | |
| if cy > world.road_half_width or cy < -world.road_half_width: | |
| return True | |
| if cx < 0 or cx > world.road_length: | |
| return True | |
| for obs in world.obstacles: | |
| if obs.contains(cx, cy): | |
| return True | |
| return False | |