diff --git a/.gitignore b/.gitignore index f90a9cf..4a3f8ed 100644 --- a/.gitignore +++ b/.gitignore @@ -12,7 +12,6 @@ ighastar/src/Environments/analytical_bicycle.cpp ighastar/scripts/test_case_generation.py examples/ros_kinodynamic_example.py examples/ROS/ -examples/BeamNG/ # Python __pycache__/ *.py[cod] diff --git a/examples/BeamNG/Configs/example.yaml b/examples/BeamNG/Configs/example.yaml new file mode 100644 index 0000000..1ef6e07 --- /dev/null +++ b/examples/BeamNG/Configs/example.yaml @@ -0,0 +1,118 @@ +MPPI_config: + ROLLOUTS: 1024 # 8192 if using 80 timesteps + TIMESTEPS: &TIMESTEPS 30 # 80 + BINS: 1 + u_per_command: 1 + +Map_config: + map_name: "small_island" + map_size: &map_size 100 + map_res: &map_res 0.25 + map_res_hitl: 0.25 + elevation_range: 32.0 + layers: + color: 3 + elevation: 1 + semantics: 3 + costmap: 1 + +Dynamics_config: + wheelbase: &wheelbase 2.6 + throttle_to_wheelspeed: &throttle_to_wheelspeed 20.0 + steering_max: &steering_max 0.6 + dt: &delta_T 0.05 + D: 0.8 + B: 6.8 + C: 1.75 + lf: 1.3 + lr: 1.3 + Iz: 1.7 + LPF_tau: 0.5 + LPF_st: 0.5 + LPF_th: 0.5 + res_coeff: 0.00 + drag_coeff: 0.00 + car_length: *wheelbase + car_width: &car_width 1.5 + cg_height: &cg_height 0.75 + patch_size: &patch_size 3.0 + type: "slip3d" + +Sampling_config: + control_dim: 2 + noise_0: 1.0 # steering noise + noise_1: 1.0 + scaled_dt: 0.1 ## this is effectively the actuator bandwidth, larger values mean the actuator can move faster. + temperature: 0.5 + max_thr: 0.8 + min_thr: 0.0 + +Cost_config: + pos_w: 1.0 ## weight on the terminal goal cost + speed_w: 0.1 ## weight with which target speed will be tracked + heading_w: 0.1 + scaling_factor: 1.0 + critical_RI: &RI 0.8 + critical_vert_acc: &max_vert_acc 4.0 + roll_ditch_w: 50.0 + lethal_w: 50.0 + car_bb_length: 2.6 + car_bb_width: 1.6 + +RPS_config: + cg_height: *cg_height + wheelbase: *wheelbase + track_width: *car_width + max_speed: *throttle_to_wheelspeed + steering_max: *steering_max + accel_gain: 0.5 + roll_rate_gain: 0.5 + steer_slack: 1.0 + +Planner_config: + unstuck_expansions: 100000 # number of expansions to get the first plan + experiment_info_default: + resolution: 4.0 # starting resolution used for discretization. + epsilon: [5, 5, 3.14, 5.0] # ate, cte, heading, vel for goal region. + tolerance: 0.5 # Your perception (map) resolution should be at least this. + max_level: 5 # maximum level to which you expect the system to go; we use this to cache the hash values. + division_factor: 2 # factor by which the resolution increases every level. + max_expansions: 5000 # maximum number of expansions allowed. + hysteresis: 1000 # hysteresis threshold for for IGHA*-H + node_info: + node_type: "kinodynamic" + length: 2.6 + width: 1.6 # actual car width is 1.5 + map_res: *map_res + dt: *delta_T + timesteps: 10 + step_size: 1.0 + steering_list: [-15.0, -5.0, 0.0, 5.0, 15.0] # in degrees. + throttle_list: [-5.0, 0, 5.0] + del_theta: 90.0 # in degrees + del_vel: 2.5 + RI: *RI + max_vert_acc: *max_vert_acc + min_vel: 0.0 + max_vel: 15.0 + max_theta: 25.0 # in degrees + gear_switch_time: 1.0 + hash_theta: False + + +hysteresis: [100, -1] # -1 means HA*M +scenarios: ['0'] # , '1'] +time_limit: [120.0] +lookahead: [50.0] +wp_radius: [10.0] + +start_pos: [-67, 336, 34.5] +start_quat: [0, 0, 0.3826834, 0.9238795] + +run_lockstep: True +burn_time: 0.04 +smooth_map: False + +vehicle: + make: "sunburst" + model: "offroad" diff --git a/examples/BeamNG/Configs/offroad.yaml b/examples/BeamNG/Configs/offroad.yaml new file mode 100644 index 0000000..8f2def9 --- /dev/null +++ b/examples/BeamNG/Configs/offroad.yaml @@ -0,0 +1,54 @@ +# camera and lidar are very heavy from the rendering perspective especially if you have +# a low end gaming laptop (such as the author, 4 GB 1050Ti GPU with an 8th gen i7 CPU) +camera: + enable: False + width: 640 # Default width + height: 480 # Default height + fps: 30 # Default frames per second + fov: 87.0 # default fov of d400 series + pos: [1,0,0.7] ## use (0.15, 0.047, 0.02) for D455, (0.15, 0.025, 0.02) for d435 + dir: [0, -1, 0] + up: [0, 0, 1] + rot: [0, 0, 0, 1] ## technically, the up and forward dir should be derivable from this, however, I need to talk to the BnG devs to figure out the conversion from REP103 to BnG + color_optical_frame: "camera_color_optical_frame" + depth_optical_frame: "camera_depth_optical_frame" + depth_frame: "camera_depth_frame" + camera_color_topic: "/camera/color/image_raw" + camera_depth_topic: "/camera/depth/image_rect_raw" + camera_color_info_topic: "/camera/color/camera_info" + camera_depth_info_topic: "/camera/depth/camera_info" + monitor_topic: "/camera/depth/image_rect_raw" ## this is the topic HAL uses for monitoring purposes + annotation: False +lidar: + enable: False + rays_per_second_per_scan: 5000 # Default points + channels: 3 # Default height. in execution we cut the first and last channel if there are only 3 channels to simulate a 2D scanner + fps: 10 # Default frames per second + vertical_angle: 26.9 # this is only for simulation purposes + pos: [0.04, 0, 1.0] # default position of lidar wrt imu + rot: [0, 0, 0, 1] + dir: [0, -1, 0] + up: [0, 0, 1] + frame: "laser_frame" + max_distance: 10.0 + scan_topic: "/scan" + monitor_topic: "/scan" + pc_topic: "converted_pc" +vesc: + fps: 50.0 + erpm_gain: 3166 + topic: "sensors/core" + steering_degrees: 260 +mavros: + pos: [0,0,0.8] # default position on the hound + fps: 50 + monitor_topic: "/mavros/imu/data_raw" + pose_topic: "/mavros/local_position/pose" + odom_topic: "/mavros/local_position/odom" + state_topic: "/mavros/state" + gps_topic: "/mavros/gpsstatus/gps1/raw" + notification_topic: "/mavros/play_tune" + channel_topic: "mavros/rc/in" + raw_input_topic: '/mavros/manual_control/send' + frame: "base_link" + failure_action: "rosrun mavros mavsys rate --all 50" \ No newline at end of file diff --git a/examples/BeamNG/IGHAStarMP.py b/examples/BeamNG/IGHAStarMP.py new file mode 100644 index 0000000..4cf028d --- /dev/null +++ b/examples/BeamNG/IGHAStarMP.py @@ -0,0 +1,187 @@ +import torch.multiprocessing as mp +import numpy as np +import time +import traceback +from queue import Empty +from typing import Any, Optional, Tuple, Dict + + +class IGHAStarMP: + """ + Multiprocessing wrapper for the IGHA* planner. Runs the planner in a separate process and communicates via queues. + """ + + def __init__(self, configs: Dict[str, Any]) -> None: + mp.set_start_method("spawn", force=True) # Safe for CUDA + self.query_queue = mp.Queue(5) + self.result_queue = mp.Queue(5) + self.process = mp.Process( + target=self._planner_process, + args=(configs, self.query_queue, self.result_queue), + ) + self.process.start() + self.path = None + self.success = False + self.completed = True + self.expansion_counter = 0 + + def _planner_process( + self, configs: Dict[str, Any], query_queue: Any, result_queue: Any + ) -> None: + """ + Planner process: loads the CUDA/C++ kernel and runs the IGHA* planner in response to queries. + """ + import torch + import numpy as np + from ighastar.scripts.common_utils import create_planner + + planner = create_planner(configs["Planner_config"]) + print("[IGHAStarMP] Planner loaded.") + + map_res = configs["experiment_info_default"]["node_info"]["map_res"] + offset = None + avg_dt = 0.1 + stream = torch.cuda.Stream() + while True: + task = query_queue.get() + if task is None: + time.sleep(0.1) + continue + ( + map_center, + start_state, + goal_, + costmap, + heightmap, + hysteresis, + expansion_limit, + stop, + ) = task + try: + map_size = costmap.shape + bitmap = torch.ones((map_size[0], map_size[1], 2), dtype=torch.float32) + bitmap[..., 0] = torch.from_numpy(costmap) + bitmap[..., 1] = torch.from_numpy(heightmap) + if offset is None: + offset = map_res * np.array(bitmap.shape[:2]) * 0.5 + start = torch.zeros(4, dtype=torch.float32) + goal = torch.zeros(4, dtype=torch.float32) + start[:2] = torch.from_numpy(start_state[:2] + offset - map_center) + goal[:2] = torch.from_numpy(goal_[:2] + offset - map_center) + start[2] = start_state[5] + start[3] = float(np.linalg.norm(start_state[6:8])) + if start[3] > 1.0 and start_state[6] > 0.5: + V = start_state[6:8] + theta = start_state[5] + dx = V[0] * np.cos(theta) - V[1] * np.sin(theta) + dy = V[0] * np.sin(theta) + V[1] * np.cos(theta) + start[2] = np.arctan2(dy, dx) + dx = goal[0] - start[0] + dy = goal[1] - start[1] + goal[2] = torch.atan2(dy, dx) + goal[3] = 0.0 if stop else 10.0 + now = time.perf_counter() + with torch.cuda.stream(stream): + success = planner.search( + start, goal, bitmap, expansion_limit, hysteresis, True + ) # ignore goal validity + ( + avg_successor_time, + avg_goal_check_time, + avg_overhead_time, + avg_g_update_time, + switches, + max_level_profile, + Q_v_size, + expansion_counter, + expansion_list, + ) = planner.get_profiler_info() + + end = time.perf_counter() + avg_dt = 0.9 * avg_dt + 0.1 * (end - now) + print( + f"[IGHAStarMP] Avg_dt: {avg_dt:.3f}s, Expansions: {expansion_counter}, Success: {success}" + ) + if success: + path = planner.get_best_path().numpy() + path = np.flip(path, axis=0) + path[..., :2] -= offset + path[..., :2] += map_center + result_queue.put((True, True, path, expansion_counter)) + else: + result_queue.put((False, True, None, expansion_counter)) + except Exception as e: + print(f"[IGHAStarMP] Planner error: {e}") + traceback.print_exc() + result_queue.put((False, True, None, 3)) + + def set_query( + self, + map_center: np.ndarray, + start_state: np.ndarray, + goal_: np.ndarray, + costmap: np.ndarray, + heightmap: np.ndarray, + hysteresis: float, + expansion_limit: int, + stop: bool = False, + disable: bool = False, + ) -> None: + """ + Submit a new planning query. Returns immediately; results are available via update(). + """ + if disable: + return + if np.linalg.norm(start_state[:2] - goal_[:2]) < 5.0: + print("[IGHAStarMP] Start and goal are too close, skipping query.") + return + if not self.completed: + return + self.query_queue.put( + ( + map_center, + start_state, + goal_, + costmap, + heightmap, + hysteresis, + expansion_limit, + stop, + ) + ) + self.completed = False + self.success = False + + def reset(self) -> None: + """Reset planner state.""" + self.path = None + self.success = False + self.completed = True + self.expansion_counter = 0 + + def update(self) -> Tuple[bool, Optional[np.ndarray], int]: + """ + Call this periodically in your main loop to check for planner results. + Returns (success, path, expansion_counter). + """ + try: + ( + self.success, + self.completed, + path, + expansions, + ) = self.result_queue.get_nowait() + self.expansion_counter = expansions + if self.success: + self.path = path + return self.success, self.path, self.expansion_counter + else: + return False, self.path, self.expansion_counter + except Empty: + return False, self.path, self.expansion_counter + + def shutdown(self) -> None: + """Shut down the planner process.""" + self.query_queue.put(None) + self.process.terminate() + self.process.join() diff --git a/examples/BeamNG/README.md b/examples/BeamNG/README.md new file mode 100644 index 0000000..1644607 --- /dev/null +++ b/examples/BeamNG/README.md @@ -0,0 +1,82 @@ +# BeamNG Example Integration + +This folder contains an example integration of the IGHA* planner with the BeamNG simulator, using the [BeamNGRL](https://github.com/prl-mushr/BeamNGRL/tree/devel) framework. + +** disclaimer **: The actual performance of the MPC may vary on your system; you may need to tweak the parameters for the sampling scheme a little bit. The values used here are the values that worked on our system when running in lock-step with the simulator. + +## What does this example do? + +This example demonstrates how to use the IGHA* global planner and MPPI controller to drive a simulated vehicle in BeamNG. The system plans a path using a costmap generated from BEV (bird's-eye view) map data, then tracks that path in closed-loop with a learned or analytical vehicle model. + +## Prerequisites + +- **BeamNGRL** must be installed separately. You must use the `devel` branch, as the TCUDA dynamics model is only available there. +- **Follow the installation instructions on the [BeamNGRL repository](https://github.com/prl-mushr/BeamNGRL/tree/devel)** to set up BeamNGRL and its dependencies. +- Other dependencies are listed in the root `requirements.txt`. + +## Running the Example + +1. **Configure your test case:** + - Edit `examples/BeamNG/Configs/example.yaml` to set planner, vehicle, and scenario parameters. + - The `scenarios` field (a list of scenario names) determines which test case(s) will be run. Each scenario should have a corresponding waypoint file in `examples/BeamNG/Waypoints/` (e.g., `0.pkl`). + +2. **Run the example:** + ```bash + cd examples/BeamNG + python3 example.py --remote True --host_IP + ``` + - You can override the config or waypoint folder using command-line arguments. + - You can find the rest of the flags in the example.py's main function (for passing in a different config or waypoint folder). + +## Configuration File Structure + +The main configuration file is `examples/BeamNG/Configs/example.yaml`. Key sections: + +- **MPPI_config**: Parameters for the MPPI controller (number of rollouts, time steps, etc.). +- **Map_config**: Map size, resolution, and BEV map settings. +- **Dynamics_config**: Vehicle model parameters (wheelbase, steering limits, etc.). +- **Sampling_config**: Noise and sampling settings for the controller. +- **Planner_config**: Parameters for the IGHA* planner, including the main `experiment_info_default` block (resolution, epsilon, max_expansions, etc.). +- **Cost_config**: Weights and thresholds for the cost function used in planning and control. +- **RPS_config**: Parameters for the steering limiter and rollover prevention. +- **scenarios**: List of scenario names to run (each must have a corresponding waypoint file). +- **start_pos/start_quat**: Default vehicle start pose (can be overridden by scenario). +- **vehicle**: Vehicle make and model. + +## Adding or Changing a Scenario + +- To run a different scenario, add its name to the `scenarios` list in `example.yaml`. +- Each scenario name (e.g., `0`) must have a corresponding waypoint file (e.g., `0.pkl`) in the `Waypoints/` folder. +- **To generate a new waypoint file:** + - You can use a separate script or tool to create a list of waypoints (as a path) and save it as a `.pkl` file (Python pickle format). The file should contain a dictionary with a `"path"` key mapping to a list/array of waypoints. + - The format is typically: `{ "path": np.ndarray of shape (N, 3 or 4) }` where each row is `[x, y, heading, (optional speed)]`. +- The scenario name in the config should match the filename (without `.pkl`). + +## Code Overview (Planner Usage) + +- The main entry point is `example.py`. +- The script loads configuration files and initializes the BeamNG interface, planner, and controller. +- For each scenario and hysteresis value: + 1. Loads the scenario waypoints and resets the simulation environment. + 2. In a main loop: + - Polls the simulator for the current vehicle state. + - Updates the goal using the current position and the loaded waypoints. + - Generates a costmap from BEV map data. + - Calls the IGHA* planner (`IGHAStarMP`) to update the plan based on the current state, goal, and costmap. It is called IGHAStarMP because we run the planner in parallel using torch Multi-processing + - Transforms the planned path to robot-centric coordinates and feeds it to the MPPI controller. + - The controller computes control actions, which are sent to the simulator. + - Visualization is updated in real time. + - The loop continues until the goal is reached or the time limit is exceeded. +- Planner and controller parameters can be tuned in the YAML config. + +## Troubleshooting & Tips + +- **Missing waypoint file:** If you get an error about a missing waypoint file, make sure the scenario name matches a `.pkl` file in the `Waypoints/` folder. +- **BeamNG connection issues:** Ensure BeamNG is running and accessible at the IP address you provide. The `--remote` and `--host_IP` arguments must match your setup. +- **Vehicle drops/falls through map:** Make sure the Z value in `start_pos` or the waypoint file is correct for your map. The car should start just above the ground. +- **No visualization:** The script uses OpenCV for visualization. Make sure you have a display available, or run with X forwarding if using SSH. +- **Parameter tuning:** If the vehicle is not following the path well, try adjusting the cost weights, planner resolution, or controller noise parameters in the config. + +--- + +For more details on BeamNGRL, see the [BeamNGRL documentation](https://github.com/prl-mushr/BeamNGRL/tree/devel). \ No newline at end of file diff --git a/examples/BeamNG/TrackingCost.py b/examples/BeamNG/TrackingCost.py new file mode 100644 index 0000000..5652fec --- /dev/null +++ b/examples/BeamNG/TrackingCost.py @@ -0,0 +1,194 @@ +import torch +from typing import Any, Optional, Dict + + +class SimpleCarCost(torch.nn.Module): + """ + Cost function for simple car dynamics and tracking. + """ + + def __init__( + self, + Cost_config: Dict[str, Any], + Map_config: Dict[str, Any], + dtype: torch.dtype = torch.float32, + device: torch.device = torch.device("cuda"), + ) -> None: + super(SimpleCarCost, self).__init__() + self.dtype = dtype + self.d = device + self.critical_RI = torch.tensor( + Cost_config["critical_RI"], dtype=self.dtype, device=self.d + ) + self.lethal_w = torch.tensor( + Cost_config["lethal_w"], dtype=self.dtype, device=self.d + ) + self.critical_vert_acc = torch.tensor( + Cost_config["critical_vert_acc"], dtype=self.dtype, device=self.d + ) + self.pos_w = torch.tensor(Cost_config["pos_w"], dtype=self.dtype, device=self.d) + self.roll_ditch_w = torch.tensor( + Cost_config["roll_ditch_w"], dtype=self.dtype, device=self.d + ) + self.speed_w = torch.tensor( + Cost_config["speed_w"], dtype=self.dtype, device=self.d + ) + self.heading_w = torch.tensor( + Cost_config["heading_w"], dtype=self.dtype, device=self.d + ) + self.scaling_factor = torch.tensor( + Cost_config["scaling_factor"], dtype=self.dtype, device=self.d + ) + self.scaling = None + self.BEVmap_size = torch.tensor( + Map_config["map_size"], dtype=self.dtype, device=self.d + ) + self.BEVmap_res = torch.tensor( + Map_config["map_res"], dtype=self.dtype, device=self.d + ) + self.BEVmap_size_px = torch.tensor( + (self.BEVmap_size / self.BEVmap_res), device=self.d, dtype=torch.int32 + ) + size_px = int(self.BEVmap_size_px.item()) + self.BEVmap = torch.zeros((size_px, size_px), device=self.d) + self.BEVmap_height = torch.zeros_like(self.BEVmap) + self.BEVmap_normal = torch.zeros( + (size_px, size_px, 3), dtype=self.dtype, device=self.d + ) + self.BEVmap_center = torch.zeros(3, dtype=self.dtype, device=self.d) + self.BEVmap_cost = torch.zeros_like(self.BEVmap_height) + self.GRAVITY = torch.tensor(9.8, dtype=self.dtype, device=self.d) + self.goal_state = torch.zeros(2, device=self.d, dtype=self.dtype) + self.car_w2 = torch.tensor( + Cost_config["car_bb_width"] / 2, dtype=self.dtype, device=self.d + ) + self.car_l2 = torch.tensor( + Cost_config["car_bb_length"] / 2, dtype=self.dtype, device=self.d + ) + self.constraint_violation = False + + @torch.jit.export + def set_BEV( + self, + BEVmap_height: torch.Tensor, + BEVmap_normal: torch.Tensor, + BEVmap_cost: torch.Tensor, + ) -> None: + """ + Set BEV (bird's-eye view) map data for cost calculation. + """ + self.BEVmap_height = BEVmap_height + self.BEVmap_normal = BEVmap_normal + self.BEVmap_cost = (255 - BEVmap_cost) / 255 + + @torch.jit.export + def set_goal(self, goal_state: torch.Tensor) -> None: + self.goal_state = goal_state[:2] + + def set_path(self, path: torch.Tensor) -> None: + self.path = torch.tensor(path, dtype=self.dtype, device=self.d) + + @torch.jit.export + def set_speed_limit(self, speed_lim: float) -> None: + self.speed_target = torch.tensor(speed_lim, dtype=self.dtype, device=self.d) + + def meters_to_px(self, meters: torch.Tensor) -> torch.Tensor: + px = ((meters + self.BEVmap_size * 0.5) / self.BEVmap_res).to( + dtype=torch.long, device=self.d + ) + px = torch.maximum(px, torch.zeros_like(px)) + px = torch.minimum(px, self.BEVmap_size_px - 1) + return px + + def forward(self, state: torch.Tensor, controls: torch.Tensor) -> torch.Tensor: + # Unpack state + x = state[..., 0] + y = state[..., 1] + z = state[..., 2] + roll = state[..., 3] + pitch = state[..., 4] + yaw = state[..., 5] + vx = state[..., 6] + vy = state[..., 7] + ay = state[..., 10] + az = state[..., 11] + + beta = torch.atan2(vy, vx) + cy = torch.cos(yaw) + sy = torch.sin(yaw) + V = torch.sqrt(vx**2 + vy**2) * torch.sign(vx) + flx = x + self.car_l2 * cy - self.car_w2 * sy + fly = y + self.car_l2 * sy + self.car_w2 * cy + frx = x + self.car_l2 * cy + self.car_w2 * sy + fry = y + self.car_l2 * sy - self.car_w2 * cy + blx = x - self.car_l2 * cy - self.car_w2 * sy + bly = y - self.car_l2 * sy + self.car_w2 * cy + brx = x - self.car_l2 * cy + self.car_w2 * sy + bry = y - self.car_l2 * sy - self.car_w2 * cy + + flx_px = self.meters_to_px(flx) + fly_px = self.meters_to_px(fly) + frx_px = self.meters_to_px(frx) + fry_px = self.meters_to_px(fry) + blx_px = self.meters_to_px(blx) + bly_px = self.meters_to_px(bly) + brx_px = self.meters_to_px(brx) + bry_px = self.meters_to_px(bry) + + # State cost is the maximum cost at the car's footprint corners + state_cost = torch.zeros_like(x) + state_cost = torch.max( + state_cost, torch.square(self.BEVmap_cost[fly_px, flx_px]) + ) + state_cost = torch.max( + state_cost, torch.square(self.BEVmap_cost[fry_px, frx_px]) + ) + state_cost = torch.max( + state_cost, torch.square(self.BEVmap_cost[bly_px, blx_px]) + ) + state_cost = torch.max( + state_cost, torch.square(self.BEVmap_cost[bry_px, brx_px]) + ) + + cr = torch.cos(roll) + cp = torch.cos(pitch) + beta = torch.atan2(vy, vx) ** 2 + # Running cost as a weighted sum of position, heading, and velocity errors + roll_ditch_cost = ( + +torch.clamp( + torch.abs(az - self.GRAVITY * cr * cp) - self.critical_vert_acc, 0, 10.0 + ) + / 10.0 + + torch.clamp(torch.abs(ay / az) - self.critical_RI, 0, 1) + ) * self.roll_ditch_w + constraint_cost = self.lethal_w * state_cost + + x_err = x - self.path[:, 0] + y_err = y - self.path[:, 1] + cy_err = cy - torch.cos(self.path[:, 2]) + sy_err = sy - torch.sin(self.path[:, 2]) + yaw_err = cy_err**2 + sy_err**2 + vel_err = (V - self.path[:, 3]) ** 2 + pos_err = x_err**2 + y_err**2 + running_cost = ( + self.pos_w * pos_err + + self.heading_w * yaw_err + + self.speed_w * vel_err + + beta * 1.5 + ) + if self.scaling is None: + steps = running_cost.shape[-1] + self.scaling = torch.linspace( + 0.1, self.scaling_factor, steps, device=self.d + ) + running_cost = running_cost * self.scaling + roll_ditch_cost + constraint_cost[torch.where(pos_err < 1)] = 0 + + constraint_cost = constraint_cost.mean(dim=0).sum(dim=1) + if torch.all(constraint_cost > self.lethal_w): + self.constraint_violation = True + else: + self.constraint_violation = False + + # Mean over bins, sum over time, add constraint cost + return (running_cost.mean(dim=0)).sum(dim=1) + constraint_cost diff --git a/examples/BeamNG/Waypoints/0.png b/examples/BeamNG/Waypoints/0.png new file mode 100644 index 0000000..9d6be28 Binary files /dev/null and b/examples/BeamNG/Waypoints/0.png differ diff --git a/examples/BeamNG/Waypoints/1.png b/examples/BeamNG/Waypoints/1.png new file mode 100644 index 0000000..8fd3ea4 Binary files /dev/null and b/examples/BeamNG/Waypoints/1.png differ diff --git a/examples/BeamNG/example.py b/examples/BeamNG/example.py new file mode 100644 index 0000000..64a3d14 --- /dev/null +++ b/examples/BeamNG/example.py @@ -0,0 +1,404 @@ +#!/usr/bin/env python3 +import numpy as np +import torch +from BeamNGRL.BeamNG.beamng_interface import * +from BeamNGRL.control.UW_mppi.MPPI import MPPI +from BeamNGRL.control.UW_mppi.Dynamics.SimpleCarDynamicsTCUDA import SimpleCarDynamics +from TrackingCost import SimpleCarCost +from BeamNGRL.control.UW_mppi.Sampling.Delta_Sampling import Delta_Sampling +import yaml +import cv2 +import argparse +import sys +import pathlib +import os +from typing import Any, Optional, Dict, List + +BASE_DIR = pathlib.Path(__file__).resolve().parent.parent.parent +sys.path.append(str(BASE_DIR / "scripts")) +from utils import ( + generate_costmap_from_BEVmap, + convert_global_path_to_bng, + update_goal, + steering_limiter, + PlannerVis, +) +from IGHAStarMP import IGHAStarMP +import pickle + +torch.manual_seed(0) + + +def main( + config_path: Optional[str] = None, + hal_config_path: Optional[str] = None, + waypoint_folder: Optional[str] = None, + output_folder: Optional[str] = None, + args: Optional[Any] = None, +) -> None: + if config_path is None: + print("no config file provided!") + exit() + if hal_config_path is None: + print("no hal config file provided!") + exit() + + if waypoint_folder is None: + print("no waypoint folder provided!") + exit() + + with open(config_path) as f: + Config = yaml.safe_load(f) + with open(hal_config_path) as f: + hal_Config = yaml.safe_load(f) + + Dynamics_config = Config["Dynamics_config"] + Cost_config = Config["Cost_config"] + Sampling_config = Config["Sampling_config"] + MPPI_config = Config["MPPI_config"] + Map_config = Config["Map_config"] + vehicle = Config["vehicle"] + start_pos = np.array( + Config["start_pos"] + ) # Default start position, may be overwritten by scenario file + start_quat = np.array(Config["start_quat"]) + map_res = Map_config["map_res"] + map_size = Map_config["map_size"] + + # Check that all scenario waypoint files exist and load them + scenario_waypoints = {} + for scenario in Config["scenarios"]: + WP_file = waypoint_folder + scenario + ".pkl" + if not os.path.isfile(WP_file): + raise ValueError(f"Waypoint file for scenario {scenario} does not exist") + scenario_waypoints[scenario] = pickle.load(open(WP_file, "rb")) + + dtype = torch.float + device = torch.device("cuda") + # initialize the beamng interface + bng_interface = get_beamng_default( + car_model=vehicle["model"], + start_pos=start_pos, + start_quat=start_quat, + car_make=vehicle["make"], + map_config=Map_config, + host_IP=args.host_IP, + remote=args.remote, + camera_config=hal_Config["camera"], + lidar_config=hal_Config["lidar"], + accel_config=hal_Config["mavros"], + burn_time=Config["burn_time"], + run_lockstep=Config["run_lockstep"], + ) + bng_interface.smooth_map = Config["smooth_map"] + + # initialize the planner visualization -- this updates the visualization in a separate thread + plan_vis = PlannerVis(int(map_size / map_res), 1 / map_res) + + try: + for hysteresis in Config["hysteresis"]: + costs = SimpleCarCost(Cost_config, Map_config, device=device) + sampling = Delta_Sampling(Sampling_config, MPPI_config, device=device) + dynamics = SimpleCarDynamics( + Dynamics_config, Map_config, MPPI_config, device=device + ) + controller = MPPI(dynamics, costs, sampling, MPPI_config, device) + for scenario in Config["scenarios"]: + data = scenario_waypoints[scenario] + target_WP = convert_global_path_to_bng( + bng_interface=bng_interface, + path=data["path"], + Map_config=Map_config, + ) + # Convert loaded waypoints to BeamNG coordinates + + time_limit = Config["time_limit"][0] + lookahead = Config["lookahead"][0] + wp_radius = Config["wp_radius"][0] + + bng_interface.reset( + start_pos=target_WP[0, :3], start_quat=target_WP[0, 3:] + ) + current_wp_index = 0 + goal = None + action = np.zeros(2) + controller.reset() + success = False + + last_reset_time = bng_interface.timestamp + ts = bng_interface.timestamp - last_reset_time + + bng_interface.state_poll() + cooldown_timer = 0 + + first_path = False + expansion_limit = Config["Planner_config"]["experiment_info_default"][ + "max_expansions" + ] + planner = IGHAStarMP(Config["Planner_config"]) + time.sleep(2) + planner.reset() + + while ts < time_limit: + bng_interface.state_poll() + + state = np.copy(bng_interface.state) + ts = bng_interface.timestamp - last_reset_time + # Get car position in world frame + pos = np.copy(state[:2]) + + goal, success, current_wp_index = update_goal( + goal, + pos, + target_WP, + current_wp_index, + lookahead, + wp_radius=wp_radius, + ) + + BEV_heght = torch.from_numpy(bng_interface.BEV_heght).to( + dtype=dtype, device=device + ) + BEV_normal = torch.from_numpy(bng_interface.BEV_normal).to( + dtype=dtype, device=device + ) + costmap = generate_costmap_from_BEVmap( + bng_interface.BEV_lethal, bng_interface.BEV_normal + ) + + ## Planner code + _, path, expansions = planner.update() + planner.set_query( + pos, + state, + goal, + costmap, + bng_interface.BEV_heght, + hysteresis, + expansion_limit, + ) + if path is None: + if not first_path: + expansion_limit = min( + expansion_limit * 2, + Config["Planner_config"]["unstuck_expansions"], + ) + continue + else: + first_path = True + expansion_limit = Config["Planner_config"][ + "experiment_info_default" + ]["max_expansions"] + # Transform path to robot-centric coordinates + controller_path = np.copy(path) + controller_path[:, :2] -= np.copy(pos) + reference_index = np.argmin( + np.linalg.norm(controller_path[:, :2], axis=1) + ) + # handle the case where the path is too short + if ( + reference_index + < len(controller_path) - MPPI_config["TIMESTEPS"] + ): + reference_path = controller_path[ + reference_index : reference_index + + MPPI_config["TIMESTEPS"], + :4, + ] + else: + reference_path = np.zeros((MPPI_config["TIMESTEPS"], 4)) + available = controller_path[reference_index:, :4] + reference_path[: len(available)] = available + reference_path[len(available) :, :3] = reference_path[ + len(available) - 1, :3 + ] + reference_path[len(available) :, 3] = 0.0 + if reference_index >= len(controller_path) - 10: + print("stopping") + action *= 0 + bng_interface.send_ctrl( + action, + speed_ctrl=True, + speed_max=Dynamics_config["throttle_to_wheelspeed"], + Kp=2, + Ki=1, + Kd=0.0, + FF_gain=0.0, + ) + # convert the reference path to a tensor + reference_path = torch.from_numpy(reference_path).to( + device=device, dtype=dtype + ) + + ## Controller initialization + controller.Dynamics.set_BEV(BEV_heght, BEV_normal) + controller.Costs.set_BEV( + bng_interface.BEV_heght, + bng_interface.BEV_normal, + torch.from_numpy(costmap).to(dtype=dtype, device=device), + ) + controller.Costs.set_path(reference_path) + state_to_ctrl = np.copy(state) + state_to_ctrl[:3] = np.zeros(3) + # Previous control output is used as input for next cycle + state_to_ctrl[15:17] = action # Wheelspeed hack + ## Controller forward pass + action = np.array( + controller.forward( + torch.from_numpy(state_to_ctrl).to( + device=device, dtype=dtype + ) + ) + .cpu() + .numpy(), + dtype=np.float64, + )[0] + # Clamp actions for safety + action[1] = np.clip( + action[1], + Sampling_config["min_thr"], + Sampling_config["max_thr"], + ) + action[0] = steering_limiter(action[0], state, Config["RPS_config"]) + # the following code block is to back the car out of a bad state. + if ( + controller.Costs.constraint_violation + ): # check constraint violation. + action = np.zeros(2) + if state[6] > 0.5: + controller.reset() + cooldown_timer = int( + 0.5 / Config["burn_time"] + ) # set the car up to drive backwards for 0.5 seconds. + if cooldown_timer > 0: + action = np.zeros(2) + action[1] = -0.2 # Reverse to recover from bad state + cooldown_timer -= 1 + bng_interface.send_ctrl( + action, + speed_ctrl=True, + speed_max=Dynamics_config["throttle_to_wheelspeed"], + Kp=10, + Ki=1, + Kd=0.0, + FF_gain=np.fabs(np.sin(state[4])), + ) + continue + + if success: # halt the system + print("Reached goal!") + action *= 0 + bng_interface.send_ctrl( + action, + speed_ctrl=True, + speed_max=Dynamics_config["throttle_to_wheelspeed"], + Kp=2, + Ki=1, + Kd=0.0, + FF_gain=0.0, + ) + break + bng_interface.send_ctrl( + action, + speed_ctrl=True, + speed_max=Dynamics_config["throttle_to_wheelspeed"], + Kp=4, + Ki=0.5, + Kd=0.0, + FF_gain=np.fabs(np.sin(state[4])), + ) + + # Visualization update + _, indices = torch.topk( + controller.Sampling.cost_total, k=10, largest=False + ) + controller_states = ( + controller.Dynamics.states[:, indices, ...].cpu().numpy() + ) + plan_vis.update_vis( + controller_states, + controller_path, + costmap, + bng_interface.BEV_heght, + 1 / map_res, + goal - state[:2], + expansions, + hysteresis, + ) + + planner.shutdown() + del planner + + except KeyboardInterrupt: + pass + except Exception as e: + print(traceback.format_exc()) + if Config["run_lockstep"]: + bng_interface.bng.close() + cv2.destroyAllWindows() + os._exit(1) + if Config["run_lockstep"]: + bng_interface.bng.close() + cv2.destroyAllWindows() + os._exit(1) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument( + "--config_name", + type=str, + default="example.yaml", + help="name of the config file to use", + ) + parser.add_argument( + "--hal_config_name", + type=str, + default="offroad.yaml", + help="name of the config file to use", + ) + parser.add_argument( + "--remote", + type=bool, + default=True, + help="whether to connect to a remote beamng server", + ) + parser.add_argument( + "--host_IP", + type=str, + default="169.254.216.9", + help="host ip address if using remote beamng", + ) + parser.add_argument( + "--waypoint_folder", + type=str, + default="/examples/BeamNG/Waypoints/", + help="folder containing the waypoint files", + ) + parser.add_argument( + "--output_folder", + type=str, + default="/examples/BeamNG/Results/example/", + help="folder containing the output", + ) + + args = parser.parse_args() + config_name = args.config_name + config_path = str(Path(os.getcwd())) + "/examples/BeamNG/Configs/" + config_name + + hal_config_name = args.hal_config_name + hal_config_path = ( + str(Path(os.getcwd())) + "/examples/BeamNG/Configs/" + hal_config_name + ) + + waypoint_folder = str(Path(os.getcwd())) + args.waypoint_folder + output_folder = str(Path(os.getcwd())) + args.output_folder + + with torch.no_grad(): + main( + config_path=config_path, + hal_config_path=hal_config_path, + waypoint_folder=waypoint_folder, + output_folder=output_folder, + args=args, + ) diff --git a/examples/BeamNG/utils.py b/examples/BeamNG/utils.py new file mode 100644 index 0000000..ff48b61 --- /dev/null +++ b/examples/BeamNG/utils.py @@ -0,0 +1,348 @@ +import numpy as np +import cv2 +import time +import threading +import torch +from typing import Any, Optional, List, Tuple, Dict + + +def generate_costmap_from_BEVmap( + BEV_lethal: np.ndarray, + BEV_normal: np.ndarray, + costmap_cosine_thresh: float = np.cos(np.radians(60)), +) -> np.ndarray: + """Generate a costmap from BEV lethal and normal maps.""" + dot_product = BEV_normal[:, :, 2] + angle_cost = np.where(dot_product >= costmap_cosine_thresh, 255, 0).astype( + np.float32 + ) + costmap = 255.0 * (1 - BEV_lethal[..., 0]) + costmap = np.minimum(costmap, angle_cost) + costmap = cv2.GaussianBlur(costmap, (3, 3), 0) + costmap[costmap < 255.0] = 0.0 + return costmap + + +def convert_global_path_to_bng( + bng_interface: Optional[Any] = None, + path: Optional[np.ndarray] = None, + Map_config: Optional[Dict[str, Any]] = None, +) -> np.ndarray: + """Convert a global path to BeamNG waypoints with elevation and quaternion.""" + target_wp = [] + map_res = Map_config["map_res"] + full_map_size = int(9000 * 0.1 / map_res) # 900m at 0.1 m resolution + offset = full_map_size // 2 + for i in range(0, len(path), 10): + wp = np.zeros(7) + wp[:2] = path[i][:2] + # Query the heightmap at the xy location, given the map resolution + x = int((path[i][0] / map_res) + offset) + y = int((path[i][1] / map_res) + offset) + z = ( + bng_interface.elevation_map_full[y, x] + 0.5 + ) # 0.5 m offset so that we start above ground + wp[2] = z + rpy = np.array([0, 0, path[i][2]]) # yaw angle + quat = bng_interface.quat_from_rpy(rpy) + bng_quat = bng_interface.convert_REP103_to_beamng(quat) + wp[3:] = bng_quat + target_wp.append(wp) + return np.array(target_wp) + + +def update_goal( + goal: Optional[np.ndarray], + pos: np.ndarray, + target_WP: np.ndarray, + current_wp_index: int, + lookahead: float, + wp_radius: float = 1.0, +) -> Tuple[np.ndarray, bool, int]: + """Update the goal position based on lookahead and proximity to final waypoint.""" + final_wp = target_WP[-1] + dx = final_wp[0] - pos[0] + dy = final_wp[1] - pos[1] + dist_to_goal = np.hypot(dx, dy) + success = False + if dist_to_goal <= lookahead: + # Close enough to final goal + goal_x, goal_y = final_wp[0], final_wp[1] + if dist_to_goal < wp_radius: + success = True + else: + # Project onto lookahead circle in direction of global goal + angle = np.arctan2(dy, dx) + goal_x = pos[0] + lookahead * np.cos(angle) + goal_y = pos[1] + lookahead * np.sin(angle) + goal = [goal_x, goal_y] + return goal, success, current_wp_index + + +def steering_limiter( + steer: float, state: np.ndarray, RPS_config: Dict[str, Any] +) -> float: + """Limit steering to prevent rollovers and respect physical constraints.""" + steering_setpoint = steer * RPS_config["steering_max"] + whspd2 = max(1.0, np.linalg.norm(state[6:8])) ** 2 # speed squared in world frame + Aylim = ( + 0.5 + * RPS_config["track_width"] + * np.cos(state[3] + state[12] * 0.5) + / RPS_config["cg_height"] + ) * max(1.0, abs(state[11])) + steering_limit_max = ( + np.arctan2(RPS_config["wheelbase"] * (Aylim - 9.8 * np.sin(state[3])), whspd2) + + RPS_config["steer_slack"] * RPS_config["steering_max"] + ) + steering_limit_min = ( + -np.arctan2(RPS_config["wheelbase"] * (Aylim + 9.8 * np.sin(state[3])), whspd2) + - RPS_config["steer_slack"] * RPS_config["steering_max"] + ) + steering_setpoint = min( + steering_limit_max, max(steering_limit_min, steering_setpoint) + ) + delta_steering = 0 + Ay = state[10] + Ay_error = 0 + if abs(Ay) > Aylim: + if Ay >= 0: + Ay_error = min(Aylim - Ay, 0) + delta_steering = ( + 4.0 + * ( + Ay_error * RPS_config["accel_gain"] + - RPS_config["roll_rate_gain"] * abs(state[11]) * state[12] + ) + * (np.cos(steering_setpoint) ** 2) + * RPS_config["wheelbase"] + / whspd2 + ) + delta_steering = min(delta_steering, 0) + else: + Ay_error = max(-Aylim - Ay, 0) + delta_steering = ( + 4.0 + * ( + Ay_error * RPS_config["accel_gain"] + - RPS_config["roll_rate_gain"] * abs(state[11]) * state[12] + ) + * (np.cos(steering_setpoint) ** 2) + * RPS_config["wheelbase"] + / whspd2 + ) + delta_steering = max( + delta_steering, 0 + ) # prevents turning in the opposite direction and causing a rollover + steering_setpoint += delta_steering + steering_setpoint = steering_setpoint / RPS_config["steering_max"] + steering_setpoint = min(max(steering_setpoint, -1.0), 1.0) + return steering_setpoint + + +class PlannerVis: + def __init__(self, map_size: int, resolution_inv: float) -> None: + self.map_size = map_size + self.resolution_inv = resolution_inv + self.costmap = None + self.elevation_map = None + self.lock = threading.Lock() + self.path = None + self.states = None + self.cosine_thresh = np.cos(np.radians(45)) + self.vis_thread = threading.Thread(target=self.costmap_vis) + self.vis_thread.daemon = True + self.vis_thread.start() + + def update_vis( + self, + states: np.ndarray, + path: Optional[np.ndarray], + costmap: np.ndarray, + elevation_map: np.ndarray, + resolution_inv: float, + goal: np.ndarray, + expansion_counter: int, + hysteresis: float, + ) -> None: + with self.lock: + if isinstance(states, torch.Tensor): + self.states = states.cpu().numpy() + else: + self.states = np.copy(states) + self.path = path + self.costmap = costmap + self.elevation_map = elevation_map + self.resolution_inv = resolution_inv + self.goal = goal + self.hysteresis = hysteresis + self.expansion_counter = expansion_counter + + def generate_costmap_from_BEVmap(self, normal: np.ndarray) -> np.ndarray: + dot_product = normal[:, :, 2] + costmap = np.where(dot_product >= self.cosine_thresh, 255, 0).astype(np.float32) + return costmap + + def costmap_vis(self) -> None: + while True: + if ( + self.states is not None + and self.costmap is not None + and self.elevation_map is not None + ): + # Normalize costmap to 8-bit grayscale (0-255) + if len(self.costmap.shape) == 3: + costmap_color = self.generate_costmap_from_BEVmap(self.costmap) + costmap_color = np.clip(costmap_color, 0, 255).astype(np.uint8) + else: + costmap_color = np.clip(self.costmap, 0, 255).astype(np.uint8) + pink = np.array([255, 105, 180], dtype=np.uint8) # BGR format + white = np.array([255, 255, 255], dtype=np.uint8) + color_map = np.zeros( + (costmap_color.shape[0], costmap_color.shape[1], 3), dtype=np.uint8 + ) + mask_white = costmap_color == 255 + mask_pink = ~mask_white + color_map[mask_white] = white + color_map[mask_pink] = pink + costmap_color = color_map + # Normalize elevation map to 8-bit and apply colormap + elev_norm = np.clip((self.elevation_map + 4) / 8, 0, 1) + elev_uint8 = (elev_norm * 255).astype(np.uint8) + elev_color = np.stack([elev_uint8] * 3, axis=-1) + # Blend the two images + costmap = costmap_color + costmap[mask_white] = elev_color[mask_white] + # Draw goal + goal_x = int( + np.clip( + (self.goal[0] * self.resolution_inv) + self.map_size // 2, + 0, + self.map_size - 1, + ) + ) + goal_y = int( + np.clip( + (self.goal[1] * self.resolution_inv) + self.map_size // 2, + 0, + self.map_size - 1, + ) + ) + radius = int(5 * self.resolution_inv) + cv2.circle(costmap, (goal_x, goal_y), radius, (0, 0, 255), 1) + # Draw path + if self.path is not None: + path_X = np.array( + np.clip( + (self.path[..., 0] * self.resolution_inv) + + self.map_size // 2, + 0, + self.map_size - 1, + ), + dtype=int, + ) + path_Y = np.array( + np.clip( + (self.path[..., 1] * self.resolution_inv) + + self.map_size // 2, + 0, + self.map_size - 1, + ), + dtype=int, + ) + car_width_px = int(2.0 * self.resolution_inv) + velocity = self.path[..., 3] + velocity_norm = (velocity - np.min(velocity)) / ( + np.max(velocity) - np.min(velocity) + ) + velocity_color = np.array( + np.clip((velocity_norm * 255), 0, 255), dtype=int + ) + for i in range(len(path_X) - 1): + cv2.line( + costmap, + (path_X[i], path_Y[i]), + (path_X[i + 1], path_Y[i + 1]), + (0, int(velocity_color[i]), 0), + car_width_px, + ) + # Draw states + if self.states is not None: + if len(self.costmap.shape) < 3: + print_states = self.states + x = print_states[:, :, :, 0].flatten() + y = print_states[:, :, :, 1].flatten() + X = np.clip( + np.array( + (x * self.resolution_inv) + self.map_size // 2, + dtype=np.int32, + ), + 0, + self.map_size - 1, + ) + Y = np.clip( + np.array( + (y * self.resolution_inv) + self.map_size // 2, + dtype=np.int32, + ), + 0, + self.map_size - 1, + ) + costmap[Y, X] = 0 + else: + print_states = self.states + x = print_states[:, :, :, 0].flatten() + y = print_states[:, :, :, 1].flatten() + X = np.clip( + np.array( + (x * self.resolution_inv) + self.map_size // 2, + dtype=np.int32, + ), + 0, + self.map_size - 1, + ) + Y = np.clip( + np.array( + (y * self.resolution_inv) + self.map_size // 2, + dtype=np.int32, + ), + 0, + self.map_size - 1, + ) + costmap[Y, X] = np.array([0, 0, 0]) + # resize and flip the costmap for visualization + costmap = cv2.resize(costmap, (500, 500), interpolation=cv2.INTER_AREA) + costmap = cv2.flip(costmap, 0) + if self.hysteresis == -1: + var = "HA*M" + else: + var = f"IGHA*-{self.hysteresis}" + cv2.putText( + costmap, + f"Var: {var}", + (10, 40), + cv2.FONT_HERSHEY_TRIPLEX, + 1, + (0, 255, 0), + 1, + ) + if self.expansion_counter == 1: + self.expansion_counter = "FAILED" + cv2.putText( + costmap, + f"Exp: {self.expansion_counter}", + (10, 70), + cv2.FONT_HERSHEY_TRIPLEX, + 1, + (0, 255, 0), + 1, + ) + cv2.imshow("map", costmap) + cv2.waitKey(1) + # Reset for next update + self.path = None + self.states = None + self.costmap = None + self.elevation_map = None + else: + time.sleep(0.02)