diff --git a/README.md b/README.md index d1cf7e6..2639441 100644 --- a/README.md +++ b/README.md @@ -48,40 +48,55 @@ AdaptiveRL is an educational reinforcement-learning project in which a Proximal AdaptiveRL is an educational reinforcement-learning project in which a PPO agent learns to navigate a simulated 3D drone through an obstacle-filled environment toward a target. > [!IMPORTANT] -> **THIS IS A SIMULATION.** +> **THIS IS AN EDUCATIONAL SIMULATION.** > -> This project is an academic computer-science demonstration running a simplified 3-DOF kinematic point-mass simulation. +> This project is an academic computer-science demonstration featuring: +> 1. A fast **3-DOF kinematic point-mass simulation** (`DroneNavigation3DEnv`) with 3 acceleration actions. +> 2. A physics-rich **6-DOF rigid-body quadrotor simulation** (`Drone6DOFEnv`) featuring unit quaternion attitude, Euler rotational equations, four individual motor thrusts, rotor torque mapping, Runge-Kutta 4th-order (RK4) integration, and full 3D quadrotor airframe visualization. > > It is **NOT**: > - a real drone autopilot > - a real-world flight controller -> - PX4 Autopilot -> - ArduPilot -> - a realistic 6-DOF quadrotor aerodynamics simulator +> - PX4 Autopilot or ArduPilot +> - calibrated against physical drone hardware telemetry > - production hardware software --- ## Features -- **Custom 3D Drone Navigation Environment**: Farama Gymnasium-compliant continuous 3D translation simulation with aerodynamic drag damping. -- **PPO Reinforcement Learning**: On-policy actor-critic learning using Stable-Baselines3. -- **Continuous 3D Acceleration Actions**: Commanded acceleration controls in X, Y, and Z axes ($[-1.0, 1.0]^3$). -- **29-Dimensional Observation Space**: Normalized drone position, velocity, target position, relative target vector, target distance, and LiDAR range readings. -- **16-Ray Simulated LiDAR**: Spherical rangefinder calculating analytical distances to obstacles and arena boundaries. +- **Dual Simulation Dynamics**: + - **3-DOF Kinematic Drone** (`drone`, `drone_3d`): Fast point-mass kinematics with semi-implicit Euler integration, continuous 3D acceleration actions ($[-1, 1]^3$), and 29-dimensional observation space. + - **6-DOF Quadrotor** (`drone-6dof`, `drone_6dof`): Theoretical rigid-body flight dynamics featuring unit quaternion attitude, Euler rotational dynamics, gyroscopic terms, 4-motor thrust control ($[-1, 1]^4 \to [0, F_{\max}]^4$), Runge-Kutta 4th-order (RK4) integration, continuous swept collision checking, and 36-dimensional observation space. + +### 3-DOF Kinematic vs. 6-DOF Rigid-Body Simulation Comparison + +| Property | 3-DOF | 6-DOF | +|---|---|---| +| Observation dimensions | 29 | 36 | +| Action dimensions | 3 | 4 | +| Control | Acceleration | Motor thrust | +| Attitude | Not modeled | Quaternion | +| Angular velocity | Not modeled | 3-axis | +- **Proper 3D Quadrotor Geometry & Visualization**: + - Full geometric airframe with central fuselage, 4 arms in X configuration, 4 rotor hub assemblies, and circular propeller blade paths. + - Distinct front (red) and rear (blue) arm/rotor visual identification plus forward heading nosecone pointer (+X_B direction). + - Exact body-to-world rigid-body coordinate transformation: $\mathbf{p}_W = \mathbf{p}_{\text{drone}} + R(\mathbf{q}) \mathbf{p}_B$. +- **Reinforcement Learning (PPO & SAC)**: + - On-policy PPO and off-policy SAC using Stable-Baselines3. + - Unified configuration pipelines (`configs/ppo_drone_6dof.yaml`, `configs/sac_drone_6dof.yaml`, `configs/drone_6dof.yaml`). +- **Interactive Streamlit Flight Deck (`app.py`)**: + - Dynamics model selector (3-DOF Kinematic Drone vs. 6-DOF Quadrotor). + - Trajectory playback scrubber updating quadrotor position, attitude, arm orientations, LiDAR, velocity, and telemetry (altitude, speed, Euler roll/pitch/yaw in degrees, angular velocity, goal distance). + - Explicit simulation playback mode indicator distinguishing educational simulation playback from live physical hardware. +- **16-Ray Simulated LiDAR**: Spherical rangefinder calculating analytical distances to obstacles and arena boundaries with configurable noise and dropout. - **Procedurally Generated Spherical Obstacles**: Placed with guaranteed clearance from launch and goal points. -- **Collision Detection**: Analytical spherical and bounding plane boundary collision detection. +- **Continuous Swept Collision Detection**: Spherical segment and bounding box collision checks preventing high-speed obstacle tunneling. - **Target Detection**: Automatic goal-arrival termination within a calibrated target radius. -- **Reward Shaping**: Multi-component reward encouraging progress toward the goal while penalizing collisions and excessive step duration. +- **Reward Shaping & Decomposed Inspection**: Multi-component reward encouraging progress while penalizing collisions, excessive control effort, angular velocity, and tilt. - **Deterministic Evaluation**: Reusable evaluation pipeline with reproducible seed control. -- **Trajectory & Safety Metrics**: Rigorous trajectory evaluation including path length, straight-line distance, path efficiency, obstacle surface clearance, maximum velocity/acceleration, and separated obstacle vs. boundary collisions. -- **Untrained Random Policy Baseline**: Built-in non-learning baseline to scientifically validate policy improvement. -- **Obstacle-Density Experiment**: Controlled testing across 4, 6, and 8 obstacles to demonstrate environmental difficulty scaling. -- **PPO Learning-Curve Benchmark**: Train fresh PPO models across configurable timestep budgets and export evaluation metrics as JSON/CSV with optional Matplotlib plots (`--plot`, `--plot-x-axis`). -- **Command-Line Interface (CLI)**: Typer-based CLI for training, evaluation, learning-curve benchmarking, environment inspection, and trajectory demonstration. -- **Streamlit + Plotly 3D GUI**: Interactive browser-based presentation flight deck with a trajectory playback scrubber and live sensor visualization. -- **Training Checkpoints**: Automatic model weight checkpointing (`.zip`) and JSON metadata export. -- **Automated Test Suite**: Unit and integration tests verify kinematics, environment spaces, training lifecycle, benchmark outputs, and GUI charts. +- **Command-Line Interface (CLI)**: Typer-based CLI for training (`train`), evaluation (`evaluate`), and deterministic demonstration flights (`demo-drone`). +- **Automated Test Suite**: Unit and integration tests verify kinematics, 6-DOF physics invariants (hover, free fall, uncoupled torques, drag decay, quaternion stability up to 50k/100k steps), training lifecycle, and GUI visualization. --- @@ -893,9 +908,10 @@ python -m pip install -e ".[all]" ## Limitations -- **Kinematic Point Mass**: The drone is modeled as a 3-DOF translational point-mass with linear aerodynamic drag. It does not model 6-DOF rotational attitude dynamics, motor RPM lag, or gyroscopic effects. -- **Analytical Rangefinder**: LiDAR sensing calculates exact geometric ray-sphere and ray-box intersections without simulated sensor noise, beam divergence, or surface reflection scattering. -- **Simulation Only**: This project is built strictly as an academic educational demonstration and cannot be directly deployed onto real drone flight hardware without a low-level attitude controller. +- **Simulation Only**: Designed strictly as an academic computer science and reinforcement learning demonstration project. +- **No Hardware Validation**: This is a theoretical simulation model and has not been tuned or validated against physical UAV flight hardware telemetry. +- **Analytical Rangefinder**: LiDAR sensing calculates geometric ray-sphere and ray-box intersections. +- **No Real Drone Autopilot**: Not intended for direct deployment onto real drone flight hardware without a low-level attitude controller. --- diff --git a/app.py b/app.py index dd4f00e..685296b 100644 --- a/app.py +++ b/app.py @@ -7,16 +7,23 @@ from __future__ import annotations +import math import time from pathlib import Path from typing import Any, Dict, Optional +import numpy as np import streamlit as st from stable_baselines3 import PPO from adaptive_rl.algorithms.ppo import PPOAlgorithm +from adaptive_rl.algorithms.registry import ( + inspect_model_checkpoint, + load_algorithm_from_pretrained, +) from adaptive_rl.config import AlgorithmConfig, EnvironmentConfig, ExperimentConfig, TrainingConfig from adaptive_rl.environments.drone import DroneNavigation3DEnv +from adaptive_rl.environments.drone_6dof import Drone6DOFEnv, quaternion_to_euler_angles from adaptive_rl.evaluation.evaluator import ( Evaluator, evaluate_ppo_policy, @@ -47,11 +54,12 @@ def main() -> None: st.sidebar.title("đ AdaptiveRL") st.sidebar.markdown( "**Autonomous 3D Drone Navigation**\n\n" - "College Demonstration Prototype using Proximal Policy Optimization (PPO).\n\n" - "*(Point-mass kinematic simulation for academic evaluation)*" + "Demonstration Platform for 3-DOF Kinematic and 6-DOF Rigid-Body Simulation using PPO & SAC.\n\n" + "*(Educational simulation for academic evaluation)*" ) available_models = get_available_models() + model_info_map = {m.name: inspect_model_checkpoint(m) for m in available_models} model_names = [m.name for m in available_models] tabs = st.tabs( @@ -68,27 +76,48 @@ def main() -> None: # TAB 1: LIVE 3D DEMO # ========================================== with tabs[0]: - st.subheader("Live 3D Drone Navigation Simulation") + st.subheader("Interactive 3D Drone Simulation Playback") st.caption( - "Observe the drone navigate from its launch position toward the green target diamond " - "while avoiding spherical obstacles using continuous 3D acceleration actions." + "Observe the simulated drone navigate toward the target waypoint while avoiding spherical " + "obstacles in a 3D arena. Playback telemetry visualizes rigid-body dynamics, attitude, and LiDAR." + ) + + st.info( + "âšī¸ **Simulation Playback Mode:** You are viewing recorded step playback from numerical " + "physics integration (RK4). This is an educational theoretical flight simulation, not " + "live hardware telemetry or real-time physical control.", + icon="âšī¸", ) - ctrl_col1, ctrl_col2, ctrl_col3, ctrl_col4 = st.columns([2, 1, 1, 1]) + ctrl_col1, ctrl_col2, ctrl_col3, ctrl_col4, ctrl_col5 = st.columns([1.5, 2, 1, 1, 1]) with ctrl_col1: + env_choice = st.selectbox( + "Dynamics Model:", + options=["6-DOF Quadrotor", "3-DOF Kinematic Drone"], + index=0, + help="Choose 6-DOF rigid-body dynamics (quaternion attitude & 4 rotors) or fast 3-DOF point-mass kinematics.", + ) + + with ctrl_col2: policy_options = ["Random Action Baseline"] - if model_names: - policy_options = [f"Trained PPO: {m}" for m in model_names] + policy_options + for m in available_models: + info = model_info_map[m.name] + if info["valid"] and info["algorithm"]: + algo_label = info["algorithm"].upper() + env_label = f" ({info['compatible_env']})" if info["compatible_env"] else "" + policy_options.append(f"Trained {algo_label}{env_label}: {m.name}") + else: + policy_options.append(f"Unsupported: {m.name}") selected_policy = st.selectbox( "Select Policy:", options=policy_options, index=0, - help="Choose a trained PPO model checkpoint or test an untrained random baseline.", + help="Choose a trained model checkpoint or test an untrained random baseline.", ) - with ctrl_col2: + with ctrl_col3: demo_seed = st.number_input( "Environment Seed:", min_value=0, @@ -98,7 +127,7 @@ def main() -> None: help="Locks procedural obstacle placement and target location for reproducible evaluation.", ) - with ctrl_col3: + with ctrl_col4: num_obs = st.slider( "Obstacles:", min_value=0, @@ -108,7 +137,7 @@ def main() -> None: help="Number of procedural spherical obstacles generated in the arena.", ) - with ctrl_col4: + with ctrl_col5: show_lidar_rays = st.checkbox("Show 16-Ray LiDAR", value=True) run_col1, _ = st.columns([1, 4]) @@ -117,28 +146,64 @@ def main() -> None: "đ Run Flight Simulation", type="primary", use_container_width=True ) - if launch_clicked or "flight_data" not in st.session_state: - with st.spinner("Executing simulation dynamics..."): - env = DroneNavigation3DEnv(num_obstacles=int(num_obs)) - model_to_use: Optional[PPO] = None + is_6dof = "6-DOF" in env_choice + sim_key = f"flight_data_{'6dof' if is_6dof else '3dof'}" - if selected_policy.startswith("Trained PPO:") and available_models: - raw_name = selected_policy.replace("Trained PPO: ", "").strip() - matching = [p for p in available_models if p.name == raw_name] - if matching: - try: - model_to_use = PPO.load(str(matching[0])) - except Exception as exc: - st.warning(f"Could not load checkpoint ({exc}); using random actions.") - - st.session_state["flight_data"] = run_drone_simulation_episode( - env=env, - model=model_to_use, - seed=int(demo_seed), - ) - env.close() + if launch_clicked or sim_key not in st.session_state: + with st.spinner(f"Executing {env_choice} simulation dynamics..."): + if is_6dof: + env = Drone6DOFEnv(num_obstacles=int(num_obs)) + else: + env = DroneNavigation3DEnv(num_obstacles=int(num_obs)) - flight_data = st.session_state["flight_data"] + try: + model_to_use: Optional[Any] = None + + if not selected_policy.startswith("Random Action Baseline") and available_models: + raw_name = selected_policy.split(": ", 1)[-1].strip() + matching = [p for p in available_models if p.name == raw_name] + if matching: + target_p = matching[0] + info = model_info_map[target_p.name] + if not info["valid"]: + st.warning( + f"Checkpoint '{raw_name}' is unsupported: {info['error']}. " + "Falling back to Random Baseline." + ) + elif ( + info["obs_shape"] != env.observation_space.shape + or info["action_shape"] != env.action_space.shape + ): + st.warning( + f"Model '{raw_name}' expects observation shape {info['obs_shape']} and " + f"action shape {info['action_shape']}, but {env_choice} environment has " + f"observation shape {env.observation_space.shape} and action shape " + f"{env.action_space.shape}. Falling back to Random Baseline." + ) + else: + try: + if info["algorithm"] == "sac": + from stable_baselines3 import SAC + + model_to_use = SAC.load(str(target_p)) + else: + from stable_baselines3 import PPO + + model_to_use = PPO.load(str(target_p)) + except Exception as exc: + st.warning( + f"Could not load checkpoint ({exc}); using random actions." + ) + + st.session_state[sim_key] = run_drone_simulation_episode( + env=env, + model=model_to_use, + seed=int(demo_seed), + ) + finally: + env.close() + + flight_data = st.session_state[sim_key] steps = flight_data["steps"] total_steps = len(steps) - 1 @@ -147,7 +212,7 @@ def main() -> None: st.markdown( f"""
Completed in {flight_data["total_steps"]} timesteps with cumulative reward {flight_data["total_reward"]:.2f} (Seed: {flight_data["seed"]}).
@@ -165,13 +230,14 @@ def main() -> None: max_value=total_steps, value=total_steps, step=1, - help="Scrub through flight time to inspect position, speed, and sensor range.", + help="Scrub through recorded flight time to inspect 3D drone attitude, position, speed, and sensor range.", ) step_info = steps[selected_step] cur_pos = step_info["position"] cur_vel = step_info["velocity"] cur_lidar = step_info["lidar_ranges"] + cur_quat = step_info.get("quaternion") # Build Interactive 3D Plotly Arena fig = build_arena_3d_figure( @@ -182,22 +248,42 @@ def main() -> None: trajectory=[s["position"] for s in steps[: selected_step + 1]], current_pos=cur_pos, current_vel=cur_vel, + attitude_quaternion=cur_quat, lidar_rays=flight_data["lidar_rays"], lidar_ranges=cur_lidar, show_lidar=show_lidar_rays, - title=f"3D Flight Arena â Step {selected_step}/{total_steps} (Distance to Goal: {step_info['distance_to_goal']:.2f}m)", + title=f"3D Flight Arena ({env_choice}) â Step {selected_step}/{total_steps} (Distance to Goal: {step_info['distance_to_goal']:.2f}m)", ) st.plotly_chart(fig, use_container_width=True) - # Step Metrics Row - m1, m2, m3, m4, m5, m6 = st.columns(6) - m1.metric("Altitude (Z)", f"{cur_pos[2]:.2f} m") - m2.metric("Ground Speed", f"{step_info['speed']:.2f} m/s") - m3.metric("Range to Goal", f"{step_info['distance_to_goal']:.2f} m") - m4.metric("Nearest Obstacle", f"{step_info['min_obstacle_distance']:.2f} m") - m5.metric("Step Reward", f"{step_info['reward']:+.2f}") - m6.metric("Cumulative Reward", f"{step_info['cumulative_reward']:+.2f}") + # Telemetry Metrics Rows + if cur_quat is not None and len(cur_quat) == 4: + roll_rad, pitch_rad, yaw_rad = quaternion_to_euler_angles(np.asarray(cur_quat)) + roll_deg = math.degrees(roll_rad) + pitch_deg = math.degrees(pitch_rad) + yaw_deg = math.degrees(yaw_rad) + else: + roll_deg, pitch_deg, yaw_deg = 0.0, 0.0, 0.0 + + omega_mag = step_info.get("angular_speed", 0.0) + + # Attitude & Velocity Telemetry + st.markdown("##### đĄ Flight Deck Telemetry") + t1, t2, t3, t4, t5, t6 = st.columns(6) + t1.metric("Altitude (Z)", f"{cur_pos[2]:.2f} m") + t2.metric("Linear Speed", f"{step_info['speed']:.2f} m/s") + t3.metric("Roll (Ī)", f"{roll_deg:+.1f}°") + t4.metric("Pitch (θ)", f"{pitch_deg:+.1f}°") + t5.metric("Yaw (Ī)", f"{yaw_deg:+.1f}°") + t6.metric("Angular Speed", f"{omega_mag:.2f} rad/s") + + # Navigation & Rewards Telemetry + m1, m2, m3, m4 = st.columns(4) + m1.metric("Range to Goal", f"{step_info['distance_to_goal']:.2f} m") + m2.metric("Nearest Obstacle", f"{step_info['min_obstacle_distance']:.2f} m") + m3.metric("Step Reward", f"{step_info['reward']:+.2f}") + m4.metric("Cumulative Reward", f"{step_info['cumulative_reward']:+.2f}") # ========================================== # TAB 2: TRAIN PPO @@ -329,23 +415,46 @@ def main() -> None: try: metrics_to_plot: Dict[str, Any] = {} - # Evaluate PPO + # Evaluate Model if eval_mode in ["Compare Both (PPO vs Random)", "PPO Only"]: - if available_models: + if available_models and eval_model_name in model_info_map: target_path = next( p for p in available_models if p.name == eval_model_name ) - ppo_algo = PPOAlgorithm.from_pretrained(target_path, env=eval_env) - ppo_res = evaluate_ppo_policy( - algorithm=ppo_algo, - env=eval_env, - num_episodes=int(eval_episodes), - base_seed=42, - ) - metrics_to_plot["PPO Policy"] = ppo_res - Evaluator.save_report(ppo_res, Path("artifacts/evaluation.json")) + info = model_info_map[target_path.name] + if not info["valid"]: + st.warning( + f"Checkpoint '{eval_model_name}' is unsupported: {info['error']}" + ) + elif ( + info["obs_shape"] != eval_env.observation_space.shape + or info["action_shape"] != eval_env.action_space.shape + ): + st.warning( + f"Checkpoint '{eval_model_name}' expects observation shape {info['obs_shape']} " + f"and action shape {info['action_shape']}, but 3-DOF benchmark environment requires " + f"{eval_env.observation_space.shape} and {eval_env.action_space.shape}. " + "Cannot evaluate incompatible model." + ) + else: + trained_algo = load_algorithm_from_pretrained( + target_path, env=eval_env + ) + trained_res = evaluate_ppo_policy( + algorithm=trained_algo, + env=eval_env, + num_episodes=int(eval_episodes), + base_seed=42, + ) + algo_title = ( + f"{info['algorithm'].upper()} Policy" + if info["algorithm"] + else "Trained Policy" + ) + metrics_to_plot[algo_title] = trained_res + Evaluator.save_report(trained_res, Path("artifacts/evaluation.json")) else: - st.warning("No saved model found. Please train a PPO agent first.") + st.warning("No saved model found. Please train a compatible agent first.") # Evaluate Random Policy if eval_mode in ["Compare Both (PPO vs Random)", "Random Only"]: @@ -390,7 +499,7 @@ def main() -> None: # Side-by-Side Comparison Chart fig_comp = build_comparison_bar_chart( metrics_map=bench_results, - title="PPO Policy vs Random Baseline (Success & Collision Rates)", + title="Trained Policy vs Random Baseline (Success & Collision Rates)", ) st.plotly_chart(fig_comp, use_container_width=True) st.info("Evaluation report exported to `artifacts/evaluation.json`") @@ -426,24 +535,34 @@ def main() -> None: run_diff_btn = st.button("đ¯ Run Obstacle-Density Experiment", type="primary") if run_diff_btn: - if available_models: - with st.spinner("Evaluating across 4, 6, and 8 obstacles..."): - target_path = next(p for p in available_models if p.name == diff_model_name) - dummy_env = DroneNavigation3DEnv() - try: - algo = PPOAlgorithm.from_pretrained(target_path, env=dummy_env) - density_results = run_obstacle_density_experiment( - algorithm=algo, - obstacle_counts=(4, 6, 8), - episodes_per_density=int(diff_episodes), - base_seed=42, - output_path=Path("artifacts/obstacle_density_experiment.json"), - ) - st.session_state["density_results"] = density_results - finally: - dummy_env.close() + if available_models and diff_model_name in model_info_map: + target_path = next(p for p in available_models if p.name == diff_model_name) + info = model_info_map[target_path.name] + if not info["valid"]: + st.warning(f"Checkpoint '{diff_model_name}' is unsupported: {info['error']}") + elif info["obs_shape"] != (29,) or info["action_shape"] != (3,): + st.warning( + f"Checkpoint '{diff_model_name}' expects observation shape {info['obs_shape']} and " + f"action shape {info['action_shape']}, which is incompatible with the 3-DOF obstacle-density " + "experiment (requires observation shape (29,) and action shape (3,))." + ) + else: + with st.spinner("Evaluating across 4, 6, and 8 obstacles..."): + dummy_env = DroneNavigation3DEnv() + try: + algo = load_algorithm_from_pretrained(target_path, env=dummy_env) + density_results = run_obstacle_density_experiment( + algorithm=algo, + obstacle_counts=(4, 6, 8), + episodes_per_density=int(diff_episodes), + base_seed=42, + output_path=Path("artifacts/obstacle_density_experiment.json"), + ) + st.session_state["density_results"] = density_results + finally: + dummy_env.close() else: - st.warning("No saved model found. Please train a PPO agent first.") + st.warning("No saved model found. Please train a compatible agent first.") density_data = st.session_state.get("density_results", None) if density_data: @@ -479,18 +598,27 @@ def main() -> None: # TAB 5: ABOUT & ARCHITECTURE # ========================================== with tabs[4]: - st.subheader("System Architecture & College Demonstration Specs") + st.subheader("System Architecture & Specifications") st.markdown( r""" + ### Simulation Environments: 3-DOF vs. 6-DOF Comparison + + | Property | 3-DOF | 6-DOF | + |---|---|---| + | Observation dimensions | 29 | 36 | + | Action dimensions | 3 | 4 | + | Control | Acceleration | Motor thrust | + | Attitude | Not modeled | Quaternion | + | Angular velocity | Not modeled | 3-axis | + ### How It Works (Step-by-Step) - 1. **Drone Observes**: The drone receives a **29-dimensional continuous state vector** at each time step. - 2. **PPO Predicts**: The neural network (MlpPolicy) computes continuous 3D acceleration commands. - 3. **Action Executed**: Commanded accelerations $(a_x, a_y, a_z) \in [-1.0, 1.0]^3$ are scaled to $\pm 4.0\text{ m/s}^2$. - 4. **Physics Integration**: 3D kinematics updates velocity with aerodynamic drag and integrates position. - 5. **Reward Feedback**: The environment rewards closing distance to the target and penalizes collisions. - 6. **Policy Learning**: During training, PPO updates actor-critic weights using generalized advantage estimation. - 7. **Evaluation**: The trained policy is tested deterministically on unseen random seeds. + 1. **Observation**: State vector received from 3-DOF point-mass (29 dims) or 6-DOF rigid-body (36 dims). + 2. **Policy Prediction**: Trained neural network (PPO or SAC) predicts continuous control actions. + 3. **Action Execution**: Commanded accelerations (3-DOF) or individual motor thrusts (6-DOF) applied to dynamics. + 4. **Physics Integration**: Semi-implicit Euler (3-DOF) or Runge-Kutta 4th-order (RK4) integration with quaternion kinematics (6-DOF). + 5. **Reward Feedback**: Progress towards goal rewarded; collisions, control effort, and instability penalized. + 6. **Policy Learning**: Agents trained with Stable-Baselines3 PPO or SAC on parameterized Gymnasium environments. """ ) @@ -499,7 +627,7 @@ def main() -> None: with arch_col1: st.markdown( r""" - ### Specifications + ### 3-DOF Kinematic Drone Specs - **Observation Space (29 Dims)**: - Drone position $\mathbf{p} = [x, y, z]$ (3 dims) - Velocity vector $\mathbf{v} = [v_x, v_y, v_z]$ (3 dims) @@ -509,22 +637,23 @@ def main() -> None: - 16-ray spherical LiDAR rangefinder readings (16 dims) - **Action Space (3 Dims)**: - Continuous 3D acceleration commands in $[-1.0, 1.0]^3$ - - **Agent**: Stable-Baselines3 PPO (`MlpPolicy`, 2 layers of 64 units) + - **Integration**: Semi-implicit Euler ($\Delta t = 0.05\text{ s}$) """ ) with arch_col2: st.markdown( r""" - ### Kinematic Flight Model - $$\mathbf{a}_t = \text{clip}(\mathbf{u}_t, -1, 1) \cdot a_{\max}$$ - $$\mathbf{v}_{t+1} = \mathbf{v}_t (1 - c_{\text{drag}} \Delta t) + \mathbf{a}_t \Delta t$$ - $$\mathbf{p}_{t+1} = \mathbf{p}_t + \mathbf{v}_{t+1} \Delta t$$ - - **Time Step**: $\Delta t = 0.1\text{ s}$ - - **Max Acceleration**: $a_{\max} = 4.0\text{ m/s}^2$ - - **Max Velocity**: $v_{\max} = 8.0\text{ m/s}$ - - **Linear Drag**: $c_{\text{drag}} = 0.05$ - - **Arena Bounds**: $30\text{m} \times 30\text{m} \times 15\text{m}$ + ### 6-DOF Rigid-Body Quadrotor Specs + - **Observation Space (36 Dims)**: + - Position (3 dims), Velocity (3 dims) + - Attitude quaternion $\mathbf{q} = [q_w, q_x, q_y, q_z]$ (4 dims) + - Body angular velocity $\boldsymbol{\omega} = [p, q, r]$ (3 dims) + - Target position (3 dims), Relative offset (3 dims), Distance ratio (1 dim) + - 16-ray spherical LiDAR rangefinder readings (16 dims) + - **Action Space (4 Dims)**: + - 4 continuous rotor thrust commands in $[-1.0, 1.0]^4 \to [0, F_{\max}]^4$ + - **Integration**: RK4 with 5 substeps ($\Delta t_{\text{sub}} = 0.01\text{ s}$) and quaternion re-normalization """ ) @@ -532,9 +661,9 @@ def main() -> None: st.markdown( """ ### Educational Scope & Honest Technical Limitations - - **Simulation Only**: Designed as a college computer science/engineering demonstration project. - - **Point-Mass Kinematics**: Uses a translational 3-DOF model with aerodynamic drag; does not model 6-DOF rotor aerodynamics, blade stall, or motor ESC latency. - - **Analytical Raycasts**: The 16-ray spherical LiDAR uses geometric ray-sphere and ray-box intersections without sensor noise or atmospheric scattering. + - **Educational Platform**: Designed for algorithmic benchmarking and educational demonstration. + - **Theoretical Simulation**: Canonical rigid-body quadrotor and point-mass physics without hardware validation claims. + - **Analytical Raycasts**: The 16-ray spherical LiDAR calculates geometric ray-sphere and ray-box intersections. - **No Real Drone Autopilot**: Not intended for direct deployment on physical UAV flight controllers (PX4/ArduPilot). """ ) diff --git a/configs/ppo_drone_6dof.yaml b/configs/ppo_drone_6dof.yaml new file mode 100644 index 0000000..2d7502f --- /dev/null +++ b/configs/ppo_drone_6dof.yaml @@ -0,0 +1,41 @@ +name: "ppo_drone_6dof" +seed: 42 + +algorithm: + name: "ppo" + learning_rate: 0.0003 + gamma: 0.99 + batch_size: 64 + parameters: + n_steps: 1024 + n_epochs: 10 + clip_range: 0.2 + ent_coef: 0.01 + +environment: + name: "drone-6dof" + max_steps: 400 + parameters: + bounds: [30.0, 30.0, 15.0] + start_pos: [5.0, 5.0, 5.0] + goal_pos: [25.0, 25.0, 10.0] + num_obstacles: 4 + obstacle_radius: 2.0 + target_radius: 1.5 + collision_radius: 0.8 + progress_weight: 2.0 + action_penalty_weight: 0.01 + angular_rate_penalty_weight: 0.05 + tilt_penalty_weight: 0.1 + +training: + total_timesteps: 50000 + checkpoint_freq: 10000 + log_interval: 10 + +evaluation: + eval_episodes: 20 + deterministic: true + +output_dir: "artifacts" +log_dir: "artifacts/logs" diff --git a/configs/sac_drone_6dof.yaml b/configs/sac_drone_6dof.yaml new file mode 100644 index 0000000..af988f7 --- /dev/null +++ b/configs/sac_drone_6dof.yaml @@ -0,0 +1,41 @@ +name: "sac_drone_6dof" +seed: 42 + +algorithm: + name: "sac" + learning_rate: 0.0003 + gamma: 0.99 + batch_size: 256 + parameters: + buffer_size: 100000 + tau: 0.005 + learning_starts: 100 + ent_coef: "auto" + +environment: + name: "drone-6dof" + max_steps: 400 + parameters: + bounds: [30.0, 30.0, 15.0] + start_pos: [5.0, 5.0, 5.0] + goal_pos: [25.0, 25.0, 10.0] + num_obstacles: 4 + obstacle_radius: 2.0 + target_radius: 1.5 + collision_radius: 0.8 + progress_weight: 2.0 + action_penalty_weight: 0.01 + angular_rate_penalty_weight: 0.05 + tilt_penalty_weight: 0.1 + +training: + total_timesteps: 50000 + checkpoint_freq: 10000 + log_interval: 10 + +evaluation: + eval_episodes: 20 + deterministic: true + +output_dir: "artifacts" +log_dir: "artifacts/logs" diff --git a/docs/ARCHITECTURE.md b/docs/ARCHITECTURE.md index 83aab9a..9340d36 100644 --- a/docs/ARCHITECTURE.md +++ b/docs/ARCHITECTURE.md @@ -19,17 +19,36 @@ src/adaptive_rl/ ## Component Details ### 1. Environments (`adaptive_rl.environments`) -- **`DroneNavigation3DEnv` (`drone`, `drone_3d`)**: Point-mass 3-DOF kinematic translation with semi-implicit Euler stepping and linear aerodynamic drag ($c_{\text{drag}} = 0.05$). -- **`Drone6DOFEnv` (`drone-6dof`, `drone_6dof`)**: Rigid-body 6-DOF quadrotor dynamics with quaternion attitude kinematics, Euler rotational equations, 4-rotor "X" configuration thrusts, and RK4 multi-substep numerical integration. + +| Property | 3-DOF | 6-DOF | +|---|---|---| +| Observation dimensions | 29 | 36 | +| Action dimensions | 3 | 4 | +| Control | Acceleration | Motor thrust | +| Attitude | Not modeled | Quaternion | +| Angular velocity | Not modeled | 3-axis | + +- **`DroneNavigation3DEnv` (`drone`, `drone_3d`)**: Point-mass 3-DOF kinematic translation with semi-implicit Euler stepping and linear aerodynamic drag ($c_{\text{drag}} = 0.05$). Continuous 3D acceleration commands $a \in [-1.0, 1.0]^3$ and 29-dimensional observation space. +- **`Drone6DOFEnv` (`drone-6dof`, `drone_6dof`)**: Rigid-body 6-DOF quadrotor flight dynamics featuring: + - Unit quaternion attitude kinematics ($\mathbf{q} \in \mathbb{R}^4, \|\mathbf{q}\|=1.0$). + - Euler rigid-body rotational equations with gyroscopic cross-coupling and rotational damping. + - 4-rotor "X" configuration mapping motor thrusts to collective thrust and body roll, pitch, and yaw torques. + - Runge-Kutta 4th-order (RK4) integration with 5 internal substeps and per-substep quaternion re-normalization. + - Continuous swept-sphere and swept-box collision checking preventing high-speed obstacle tunneling. + - 4 continuous motor thrust actions $a \in [-1.0, 1.0]^4 \to [0, F_{\max}]^4$ and 36-dimensional observation space. + - Fully inspectable decomposed reward terms (progress, step, effort, angular rate, tilt, terminal). - **`DroneDisturbed3DEnv` (`drone_disturbed`, `drone_disturbance`)**: 3D drone navigation subject to steady prevailing wind, stochastic Ornstein-Uhlenbeck (OU) gusts, moving dynamic obstacles with boundary reflection, and step-indexed disturbance recovery tracking. -- **Observations (Standard 29 Dims)**: - - Relative target vector (3 dims) - - Drone velocity (3 dims) - - Normalized drone position (3 dims) - - Normalized target position (3 dims) - - Target distance scalar (1 dim) - - 16-ray spherical LiDAR rangefinder readings (16 dims) with configurable Gaussian noise and dropout. -- **Actions**: Continuous 3D acceleration commands $a \in [-1.0, 1.0]^3$ (or 4-motor thrusts in 6-DOF). + +### 2. Visualization & Flight Deck (`adaptive_rl.gui`) +- **Interactive 3D Arena Plotly Visualizer**: + - Full 3D quadrotor geometric model: central fuselage hub, four arms in X configuration, four rotor assemblies, and circular propeller blade paths. + - Distinct front/rear visual identification (front red arms/rotors, rear blue arms/rotors, and forward heading nosecone pointer along $+X_B$). + - Exact body-to-world rigid-body coordinate transformation: $\mathbf{p}_W = \mathbf{p}_{\text{drone}} + R(\mathbf{q}) \mathbf{p}_B$. + - 16-ray spherical LiDAR beams and contact points. + - Trajectory playback scrubber slider with telemetry (altitude, speed, Euler roll/pitch/yaw, angular velocity, goal distance, obstacle clearance). +- **Streamlit Application (`app.py`)**: + - Dynamics model selector supporting both 3-DOF Kinematic Drone and 6-DOF Quadrotor. + - Playback mode distinction with educational simulation disclaimer. ### 2. Algorithms & Training (`adaptive_rl.algorithms`, `adaptive_rl.training`) - **PPO (`PPOAlgorithm`)**: On-policy actor-critic algorithm wrapped from Stable-Baselines3. diff --git a/docs/ENVIRONMENT_6DOF.md b/docs/ENVIRONMENT_6DOF.md index a28ff0f..05851f2 100644 --- a/docs/ENVIRONMENT_6DOF.md +++ b/docs/ENVIRONMENT_6DOF.md @@ -10,23 +10,38 @@ The `drone-6dof` environment simulates the rigid-body flight dynamics of a quadr --- -## 2. Coordinate Frames & Conventions +## 2. Comparison: 3-DOF Kinematic vs. 6-DOF Rigid-Body Simulation + +| Feature / Property | 3-DOF Model (`drone`, `drone_3d`) | 6-DOF Model (`drone-6dof`, `drone_6dof`) | +| :--- | :--- | :--- | +| **Physical Model** | Point-mass kinematic translation | Rigid-body translation & rotational dynamics | +| **Attitude State** | None (assumed upright point mass) | Unit quaternion $\mathbf{q} = [q_w, q_x, q_y, q_z]$ | +| **Angular Rates** | None | Body angular velocity $\boldsymbol{\omega} = [p, q, r]$ (rad/s) | +| **Actuation** | 3 continuous accelerations $a \in [-1, 1]^3$ | 4 individual motor thrusts $F_i \in [0, F_{\max}]$ | +| **Torque Generation** | None | Rotor-generated roll, pitch, and yaw torques | +| **Numerical Integration** | Semi-implicit Euler ($\Delta t = 0.05$ s) | Runge-Kutta 4th-Order (RK4, 5 substeps, $\Delta t_{\text{sub}} = 0.01$ s) | +| **Quaternion Normalization** | Not applicable | Enforced after every substep ($\|\mathbf{q}\| = 1.0$) | +| **Observation Dimensions** | 29 dimensions | 36 dimensions (includes quat, angular velocity) | +| **Collision Detection** | Point sphere / box check | Continuous swept-sphere & swept-box collision checking | +| **Computational Cost** | Ultra-fast (~100,000 steps/sec) | Physics-rich (~2,000â5,000 steps/sec) | -To prevent ambiguity, the coordinate systems and conventions are defined as follows: +--- + +## 3. Coordinate Frames & Conventions -### 2.1 World Frame: ENU (East-North-Up) +### 3.1 World Frame: ENU (East-North-Up) * $+X_W$: East * $+Y_W$: North * $+Z_W$: Upward against gravity. -* Gravitational acceleration: $\mathbf{g} = [0, 0, -9.81]^T\text{ m/s}^2$. +* Gravitational acceleration: $\mathbf{g} = [0, 0, -9.81]^T\text{ m/s}^2$ (or configurable non-negative scalar). -### 2.2 Body Frame +### 3.2 Body Frame Fixed to the quadrotor center of mass (CoM): * $+X_B$: Forward along the longitudinal axis. * $+Y_B$: Left along the lateral axis. -* $+Z_B$: Upward along the normal axis (aligned with the collective rotor thrust vector). +* $+Z_B$: Upward along the normal axis (aligned with collective thrust vector). -### 2.3 Attitude Representation: Unit Quaternion +### 3.3 Attitude Representation: Unit Quaternion Attitude is represented by a unit quaternion $\mathbf{q} = [q_w, q_x, q_y, q_z]^T \in \mathbb{R}^4$ with $\|\mathbf{q}\| = 1$, parameterizing the rotation from body frame to world frame: $$\mathbf{v}_W = R(\mathbf{q}) \mathbf{v}_B$$ @@ -37,9 +52,16 @@ $$R(\mathbf{q}) = \begin{bmatrix} 2(q_x q_z - q_w q_y) & 2(q_y q_z + q_w q_x) & 1 - 2(q_x^2 + q_y^2) \end{bmatrix}$$ +Euler angles (roll $\phi$, pitch $\theta$, yaw $\psi$) are converted using numerically safe trigonometry: +$$\begin{aligned} +\phi &= \text{atan2}(2(q_w q_x + q_y q_z), 1 - 2(q_x^2 + q_y^2)) \\ +\theta &= \arcsin(\text{clamp}(2(q_w q_y - q_z q_x), -1, 1)) \\ +\psi &= \text{atan2}(2(q_w q_z + q_x q_y), 1 - 2(q_y^2 + q_z^2)) +\end{aligned}$$ + --- -## 3. Rotor Layout & Actuation +## 4. Rotor Layout & Actuation The quadrotor operates in an "X" configuration with arm length $L = 0.2\text{ m}$. With $d = \frac{L}{\sqrt{2}} \approx 0.1414\text{ m}$: @@ -57,59 +79,58 @@ Each rotor generates thrust $F_i \in [0, F_{\max}]$ directed along $+Z_B$: 2. **Body Torques** $\boldsymbol{\tau} = [\tau_x, \tau_y, \tau_z]^T$: $$\tau_x = d (-F_1 + F_2 + F_3 - F_4) \quad \text{(Roll torque about } +X_B\text{)}$$ $$\tau_y = d (-F_1 - F_2 + F_3 + F_4) \quad \text{(Pitch torque about } +Y_B\text{)}$$ - $$\tau_z = c_\tau (F_1 - F_2 + F_3 - F_4) \quad \text{(Yaw torque about } +Z_B\text{)}$$ + $$\tau_z = c_\tau (+F_1 - F_2 + F_3 - F_4) \quad \text{(Yaw torque about } +Z_B\text{)}$$ where $c_\tau = 0.01\text{ m}$ is the aerodynamic torque-to-thrust ratio. --- -## 4. Equations of Motion +## 5. Equations of Motion -### 4.1 Translational Dynamics (World Frame) +### 5.1 Translational Dynamics (World Frame) $$m \ddot{\mathbf{p}} = \begin{bmatrix} 0 \\ 0 \\ -mg \end{bmatrix} + R(\mathbf{q}) \begin{bmatrix} 0 \\ 0 \\ T \end{bmatrix} - D_v \mathbf{v}$$ where: * $m = 1.0\text{ kg}$ is total mass. -* $D_v = 0.1\text{ N}\cdot\text{s/m}$ is the linear aerodynamic translational drag coefficient. +* $D_v = 0.1\text{ N}\cdot\text{s/m}$ is linear aerodynamic drag. * $\mathbf{v} = \dot{\mathbf{p}}$ is world-frame linear velocity. -### 4.2 Rotational Dynamics (Euler Rigid-Body Equations, Body Frame) +### 5.2 Rotational Dynamics (Euler Rigid-Body Equations, Body Frame) $$I \dot{\boldsymbol{\omega}} = \boldsymbol{\tau} - \boldsymbol{\omega} \times (I \boldsymbol{\omega}) - D_\omega \boldsymbol{\omega}$$ where: * $\boldsymbol{\omega} = [p, q, r]^T$ is the body angular velocity. -* $I = \text{diag}(I_{xx}, I_{yy}, I_{zz}) = \text{diag}(0.01, 0.01, 0.02)\text{ kg}\cdot\text{m}^2$ is the moment of inertia tensor. -* $D_\omega = 0.02\text{ N}\cdot\text{m}\cdot\text{s/rad}$ is the rotational aerodynamic damping. +* $I = \text{diag}(0.01, 0.01, 0.02)\text{ kg}\cdot\text{m}^2$ is the moment of inertia tensor. +* $D_\omega = 0.02\text{ N}\cdot\text{m}\cdot\text{s/rad}$ is rotational aerodynamic damping. -### 4.3 Quaternion Kinematics +### 5.3 Quaternion Kinematics $$\dot{\mathbf{q}} = \frac{1}{2} \mathbf{q} \otimes \begin{bmatrix} 0 \\ \boldsymbol{\omega} \end{bmatrix}$$ -$$\begin{aligned} -\dot{q}_w &= -\frac{1}{2} (q_x p + q_y q + q_z r) \\ -\dot{q}_x &= \frac{1}{2} (q_w p + q_y r - q_z q) \\ -\dot{q}_y &= \frac{1}{2} (q_w q - q_x r + q_z p) \\ -\dot{q}_z &= \frac{1}{2} (q_w r + q_x q - q_y p) -\end{aligned}$$ --- -## 5. Numerical Integration & Stability +## 6. Numerical Integration & Validation Tolerances + +Stepping employs **Runge-Kutta 4th-order (RK4)** integration: +* Environment step $\Delta t = 0.05\text{ s}$ with 5 substeps ($\Delta t_{\text{sub}} = 0.01\text{ s}$). +* Re-normalization enforced after every substep ($\|\mathbf{q}\| \leftarrow 1.0$). +* Continuous swept-sphere and swept-box collision checking between steps prevents tunneling. -To avoid numerical drift and instability: -* The environment step is $\Delta t = 0.05\text{ s}$. -* Stepping employs **Runge-Kutta 4th-order (RK4)** numerical integration with $5$ internal substeps ($\Delta t_{\text{sub}} = 0.01\text{ s}$). -* The quaternion $\mathbf{q}$ is explicitly re-normalized after every RK4 substep: - $$\mathbf{q} \leftarrow \frac{\mathbf{q}}{\|\mathbf{q}\|}$$ -* Tests verify zero explosion and strict finite bounds over 1,000 continuous random steps. +### Verified Test Tolerances (`tests/test_physics_validation_6dof.py`): +1. **Hover Equilibrium**: Acceleration $\|d\mathbf{v}\| \le 10^{-9}\text{ m/s}^2$; vertical drift $< 10^{-4}$ m over 100 steps. +2. **Free Fall**: Acceleration equals $[0, 0, -g]$ within $10^{-6}\text{ m/s}^2$. +3. **Pure Torques**: Roll, pitch, and yaw torques uncoupled within $10^{-9}\text{ N}\cdot\text{m}$. +4. **Aerodynamic Drag**: Velocity decay matches analytical exponential $v_0 e^{-(D_v/m)t}$ within $0.1\%$. +5. **Orthogonality**: $R R^T = I$ and $\det(R) = 1.0$ within $10^{-6}$. +6. **Quaternion Stability**: Maintained $\|\mathbf{q}\| = 1.0 \pm 10^{-5}$ over 50,000 steps and 100,000 steps. --- -## 6. Action and Observation Spaces +## 7. Action and Observation Spaces -### 6.1 Action Space -Continuous 4-dimensional box: `Box(-1.0, 1.0, shape=(4,), dtype=float32)`. -Mapped linearly to motor thrusts: +### 7.1 Action Space +`Box(-1.0, 1.0, shape=(4,), dtype=float32)`: $$F_i = \frac{a_i + 1}{2} F_{\max} \in [0, 5.0]\text{ N}$$ -Hover corresponds to $F_i = \frac{mg}{4} = 2.4525\text{ N}$ ($a_i \approx -0.019$). +Hover thrust corresponds to $F_i = \frac{mg}{4} = 2.4525\text{ N}$ ($a_i \approx -0.019$). -### 6.2 Observation Space -Continuous 36-dimensional box: `Box(-1.0, 1.0, shape=(36,), dtype=float32)`: +### 7.2 Observation Space +`Box(-1.0, 1.0, shape=(36,), dtype=float32)`: * `[0:3]`: Normalized position $\mathbf{p} / \text{bounds}$ in $[0, 1]^3$ * `[3:6]`: Normalized velocity $\mathbf{v} / v_{\max}$ in $[-1, 1]^3$ * `[6:10]`: Attitude quaternion $[q_w, q_x, q_y, q_z]$ in $[-1, 1]^4$ @@ -121,9 +142,18 @@ Continuous 36-dimensional box: `Box(-1.0, 1.0, shape=(36,), dtype=float32)`: --- -## 7. Assumptions & Limitations +## 8. Reward Function & Decomposition + +The reward function guides the agent toward the goal while penalizing collisions, excessive control effort, angular velocity, and unlevel attitude: + +$$R_t = R_{\text{terminal}} + w_{\text{progress}} \Delta d + r_{\text{step}} - w_{\text{effort}} \|\mathbf{a}\|^2 - w_{\text{rate}} \|\boldsymbol{\omega}\|^2 - w_{\text{tilt}} (1 - R_{33})$$ + +Where: +* $R_{\text{terminal}} = +100.0$ (goal reached) or $-100.0$ (collision). +* $w_{\text{progress}} = 2.0$ (reward for closing distance to goal). +* $r_{\text{step}} = -0.05$ (step penalty). +* $w_{\text{effort}} = 0.01$ (penalizes excessive motor commands). +* $w_{\text{rate}} = 0.05$ (penalizes high body angular rates). +* $w_{\text{tilt}} = 0.1$ (penalizes tilt deviation from level flight, where $R_{33} = \hat{z}_B \cdot \hat{z}_W$). -1. **Rigid Airframe**: The airframe is assumed perfectly rigid with constant moments of inertia. -2. **Simplified Aerodynamics**: Drag is modeled as linear translational and rotational damping. Complex fluid dynamics, such as ground effect, blade flapping, vortex ring state, and aerodynamic downwash are omitted. -3. **Instantaneous Motor Dynamics**: Rotor thrust response is modeled without motor electrical inductance or ESC lag. -4. **Validation**: This model should be treated as an educational simulation platform rather than a physical digital twin. +All components are exposed in `info["reward_components"]` for evaluation and analysis. diff --git a/docs/TRAINING_6DOF.md b/docs/TRAINING_6DOF.md new file mode 100644 index 0000000..2c69b85 --- /dev/null +++ b/docs/TRAINING_6DOF.md @@ -0,0 +1,172 @@ +# 6-DOF Quadrotor Reinforcement Learning Training Guide + +This guide documents training, evaluating, and demonstrating reinforcement learning agents on the rigid-body 6-DOF quadrotor simulation environment (`Drone6DOFEnv`, registered as `drone-6dof`). + +> [!NOTE] +> **Educational Research Platform**: +> This quadrotor dynamics model is an educational theoretical simulation. It is designed for algorithmic benchmarking and reinforcement learning experiments and is not tuned to specific physical drone hardware telemetry. + +--- + +## 1. Quick Start Commands + +### 1.1 Training PPO on 6-DOF +Train an on-policy PPO agent using the preconfigured YAML configuration: +```bash +adaptive-rl train --config configs/ppo_drone_6dof.yaml --timesteps 50000 +``` +Or with custom budget and seed: +```bash +adaptive-rl train --config configs/ppo_drone_6dof.yaml --timesteps 25000 --seed 42 +``` + +### 1.2 Training SAC on 6-DOF +Train an off-policy SAC agent: +```bash +adaptive-rl train --config configs/sac_drone_6dof.yaml --timesteps 50000 +``` + +### 1.3 Evaluating Trained Checkpoints +Evaluate the trained policy over 20 deterministic benchmark episodes: +```bash +adaptive-rl evaluate --model artifacts/models/ppo_drone_6dof_final.zip --config configs/ppo_drone_6dof.yaml --episodes 20 +``` + +### 1.4 Command-Line 3D Demonstration +Run a deterministic flight demo rendering ASCII status and real-time telemetry: +```bash +adaptive-rl demo-drone --model artifacts/models/ppo_drone_6dof_final.zip --config configs/ppo_drone_6dof.yaml --seed 42 --max-steps 150 +``` + +### 1.5 Interactive Streamlit 3D Flight Deck +Launch the interactive web-based 3D simulator: +```bash +adaptive-rl gui +``` +Select **6-DOF Quadrotor** from the dynamics selector dropdown in the browser. + +--- + +## 2. Configuration Files + +Three configuration files are provided in `configs/`: + +| Config File | Algorithm | Environment | Action Dim | Obs Dim | +| :--- | :--- | :--- | :--- | :--- | +| `configs/drone_6dof.yaml` | PPO | `drone-6dof` | 4 | 36 | +| `configs/ppo_drone_6dof.yaml` | PPO | `drone-6dof` | 4 | 36 | +| `configs/sac_drone_6dof.yaml` | SAC | `drone-6dof` | 4 | 36 | + +### 2.1 PPO Configuration (`configs/ppo_drone_6dof.yaml`) +```yaml +name: "ppo_drone_6dof" +seed: 42 + +algorithm: + name: "ppo" + learning_rate: 0.0003 + gamma: 0.99 + batch_size: 64 + parameters: + n_steps: 1024 + n_epochs: 10 + clip_range: 0.2 + ent_coef: 0.01 + +environment: + name: "drone-6dof" + max_steps: 400 + parameters: + bounds: [30.0, 30.0, 15.0] + start_pos: [5.0, 5.0, 5.0] + goal_pos: [25.0, 25.0, 10.0] + num_obstacles: 4 + obstacle_radius: 2.0 + target_radius: 1.5 + collision_radius: 0.8 + progress_weight: 2.0 + action_penalty_weight: 0.01 + angular_rate_penalty_weight: 0.05 + tilt_penalty_weight: 0.1 + +training: + total_timesteps: 50000 + checkpoint_freq: 10000 + log_interval: 10 + +evaluation: + eval_episodes: 20 + deterministic: true + +output_dir: "artifacts" +log_dir: "artifacts/logs" +``` + +### 2.2 SAC Configuration (`configs/sac_drone_6dof.yaml`) +```yaml +name: "sac_drone_6dof" +seed: 42 + +algorithm: + name: "sac" + learning_rate: 0.0003 + gamma: 0.99 + batch_size: 256 + parameters: + buffer_size: 100000 + tau: 0.005 + learning_starts: 100 + ent_coef: "auto" + +environment: + name: "drone-6dof" + max_steps: 400 + parameters: + bounds: [30.0, 30.0, 15.0] + num_obstacles: 4 +``` + +--- + +## 3. Python API Usage + +```python +from adaptive_rl.environments.registry import make_env +from adaptive_rl.algorithms.ppo import PPOAlgorithm +from adaptive_rl.evaluation.evaluator import Evaluator + +# 1. Instantiate the 6-DOF environment +env = make_env("drone-6dof", num_obstacles=4, max_steps=400) + +# 2. Instantiate and train PPO +algo = PPOAlgorithm(env=env, n_steps=1024, batch_size=64) +algo.train(total_timesteps=50000) + +# 3. Save checkpoint +algo.save("artifacts/models/my_6dof_ppo.zip") + +# 4. Evaluate deterministic performance +evaluator = Evaluator(algorithm=algo, env=env) +metrics = evaluator.evaluate(num_episodes=20, deterministic=True, base_seed=42) +print(f"Success Rate: {metrics.success_rate * 100:.1f}%, Mean Reward: {metrics.mean_reward:.2f}") + +env.close() +``` + +--- + +## 4. Decomposed Reward Inspection + +During stepping, the environment returns detailed reward components inside the `info` dictionary: + +```python +obs, reward, terminated, truncated, info = env.step(action) + +components = info["reward_components"] +print("Progress Reward:", components["progress_reward"]) +print("Step Penalty:", components["step_penalty"]) +print("Control Effort Penalty:", components["effort_penalty"]) +print("Angular Rate Penalty:", components["rate_penalty"]) +print("Tilt Angle Penalty:", components["tilt_penalty"]) +print("Terminal Reward:", components["terminal_reward"]) +``` diff --git a/pyproject.toml b/pyproject.toml index 683904c..6e73140 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -80,6 +80,9 @@ where = ["src"] [tool.pytest.ini_options] testpaths = ["tests"] pythonpath = ["src"] +markers = [ + "slow: marks tests as slow (deselect with '-m \"not slow\"')", +] filterwarnings = [ "ignore::DeprecationWarning", ] diff --git a/src/adaptive_rl/algorithms/registry.py b/src/adaptive_rl/algorithms/registry.py index 3019aba..be0e78a 100644 --- a/src/adaptive_rl/algorithms/registry.py +++ b/src/adaptive_rl/algorithms/registry.py @@ -4,7 +4,7 @@ from dataclasses import dataclass, field from pathlib import Path -from typing import TYPE_CHECKING, Any, Callable, Dict, List, Optional, cast +from typing import TYPE_CHECKING, Any, Callable, Dict, List, Optional, Tuple, cast if TYPE_CHECKING: from adaptive_rl.algorithms.base import BaseAlgorithm @@ -167,6 +167,152 @@ def list_all_algorithm_metadata() -> Dict[str, AlgorithmMetadata]: return algorithm_registry.list_all_metadata() +def inspect_model_checkpoint(path: str | Path) -> Dict[str, Any]: + """Inspect a model checkpoint archive to extract algorithm and space metadata. + + Args: + path: Path to model checkpoint file (.zip). + + Returns: + Dict with keys: + - 'algorithm': 'ppo' | 'sac' | None + - 'obs_shape': Tuple[int, ...] or None + - 'action_shape': Tuple[int, ...] or None + - 'compatible_env': '3-DOF' | '6-DOF' | None + - 'valid': bool + - 'error': Optional[str] + """ + model_path = Path(path) + if not model_path.exists() and model_path.with_suffix(".zip").exists(): + model_path = model_path.with_suffix(".zip") + + if not model_path.is_file(): + return { + "algorithm": None, + "obs_shape": None, + "action_shape": None, + "compatible_env": None, + "valid": False, + "error": f"Model file not found: {path}", + } + + import zipfile + + if not zipfile.is_zipfile(model_path): + return { + "algorithm": None, + "obs_shape": None, + "action_shape": None, + "compatible_env": None, + "valid": False, + "error": f"Checkpoint '{model_path.name}' is not a valid zip archive", + } + + algo: Optional[str] = None + obs_shape: Optional[Tuple[int, ...]] = None + act_shape: Optional[Tuple[int, ...]] = None + + # Check sidecar metadata JSON if present + candidate_metadata_paths = [ + model_path.with_name(f"{model_path.stem}_metadata.json"), + model_path.parent / f"{model_path.stem.replace('_final', '')}_metadata.json", + model_path.parent.parent / "metadata" / f"{model_path.stem}_metadata.json", + model_path.parent.parent + / "metadata" + / f"{model_path.stem.replace('_final', '')}_metadata.json", + ] + for meta_p in candidate_metadata_paths: + if meta_p.is_file(): + try: + import json + + with open(meta_p, "r", encoding="utf-8") as f: + sidecar_data = json.load(f) + sidecar_algo = sidecar_data.get("algorithm") + if sidecar_algo: + algo = str(sidecar_algo).lower().strip() + break + except Exception: + pass + + try: + with zipfile.ZipFile(model_path, "r") as z: + if "data" not in z.namelist(): + return { + "algorithm": None, + "obs_shape": None, + "action_shape": None, + "compatible_env": None, + "valid": False, + "error": f"Archive '{model_path.name}' is missing Stable-Baselines3 'data' entry", + } + + import json + + raw_bytes = z.read("data") + raw_str = raw_bytes.decode("utf-8", errors="ignore") + data = json.loads(raw_str) + + if obs_shape is None: + o_space = data.get("observation_space") + if isinstance(o_space, dict) and "_shape" in o_space: + obs_shape = tuple(int(x) for x in o_space["_shape"]) + + if act_shape is None: + a_space = data.get("action_space") + if isinstance(a_space, dict) and "_shape" in a_space: + act_shape = tuple(int(x) for x in a_space["_shape"]) + + if not algo: + policy_mod = str(data.get("policy_class", {}).get("__module__", "")) + if ( + "sac" in policy_mod + or "SACPolicy" in raw_str + or any(k in data for k in ["buffer_size", "tau", "target_entropy"]) + ): + algo = "sac" + elif ( + "common.policies" in policy_mod + or "ActorCriticPolicy" in raw_str + or any(k in data for k in ["n_steps", "n_epochs", "clip_range"]) + ): + algo = "ppo" + except Exception as exc: + return { + "algorithm": None, + "obs_shape": None, + "action_shape": None, + "compatible_env": None, + "valid": False, + "error": f"Corrupt checkpoint archive '{model_path.name}': {exc}", + } + + if not algo or algo not in ("ppo", "sac"): + return { + "algorithm": algo, + "obs_shape": obs_shape, + "action_shape": act_shape, + "compatible_env": None, + "valid": False, + "error": f"Unsupported or unrecognized algorithm type: '{algo or 'unknown'}'", + } + + compatible_env: Optional[str] = None + if obs_shape == (29,) and act_shape == (3,): + compatible_env = "3-DOF" + elif obs_shape == (36,) and act_shape == (4,): + compatible_env = "6-DOF" + + return { + "algorithm": algo, + "obs_shape": obs_shape, + "action_shape": act_shape, + "compatible_env": compatible_env, + "valid": True, + "error": None, + } + + def load_algorithm_from_pretrained( path: str | Path, env: Optional[Any] = None, @@ -189,20 +335,46 @@ def load_algorithm_from_pretrained( if not model_path.exists() and model_path.with_suffix(".zip").exists(): model_path = model_path.with_suffix(".zip") + if not model_path.is_file(): + raise FileNotFoundError(f"Model file not found at: {path}") + detected = algorithm_name.strip().lower() if algorithm_name else None - if detected is None and model_path.exists(): - try: - import zipfile - - with zipfile.ZipFile(model_path, "r") as z: - if "data" in z.namelist(): - data_str = z.read("data").decode("utf-8", errors="ignore") - if '"SAC"' in data_str or "SACAlgorithm" in data_str: - detected = "sac" - elif '"PPO"' in data_str or "PPOAlgorithm" in data_str: - detected = "ppo" - except Exception: - pass + + # Inspect checkpoint archive + info = inspect_model_checkpoint(model_path) + + if detected is None: + if not info.get("valid"): + raise AlgorithmRegistryError( + info.get("error") + or f"Unsupported or unrecognized checkpoint at '{model_path}'. Expected a valid PPO or SAC model checkpoint." + ) + detected = info.get("algorithm") + + # If archive is a valid checkpoint, verify space compatibility and algorithm match + if info.get("valid"): + archive_algo = info.get("algorithm") + # Never load SAC through a PPO loader + if archive_algo == "sac" and detected == "ppo": + raise AlgorithmRegistryError( + f"Cannot load SAC model '{model_path.name}' as PPO algorithm." + ) + if archive_algo == "ppo" and detected == "sac": + raise AlgorithmRegistryError( + f"Cannot load PPO model '{model_path.name}' as SAC algorithm." + ) + + if env is not None: + if info.get("obs_shape") is not None and hasattr(env, "observation_space"): + if info["obs_shape"] != env.observation_space.shape: + raise ValueError( + f"Model observation space {info['obs_shape']} does not match environment observation space {env.observation_space.shape}" + ) + if info.get("action_shape") is not None and hasattr(env, "action_space"): + if info["action_shape"] != env.action_space.shape: + raise ValueError( + f"Model action space {info['action_shape']} does not match environment action space {env.action_space.shape}" + ) if detected == "sac": from adaptive_rl.algorithms.sac import SACAlgorithm @@ -218,15 +390,9 @@ def load_algorithm_from_pretrained( return cast("BaseAlgorithm", factory.from_pretrained(model_path, env=env)) return cast("BaseAlgorithm", factory(env=env)) - # Fallback heuristic: try PPO first, then SAC - try: - from adaptive_rl.algorithms.ppo import PPOAlgorithm - - return PPOAlgorithm.from_pretrained(model_path, env=env) - except Exception: - from adaptive_rl.algorithms.sac import SACAlgorithm - - return SACAlgorithm.from_pretrained(model_path, env=env) + raise AlgorithmRegistryError( + f"Unsupported algorithm '{detected}' for model checkpoint at '{model_path}'." + ) __all__ = [ @@ -236,6 +402,7 @@ def load_algorithm_from_pretrained( "algorithm_registry", "get_algorithm_factory", "get_algorithm_metadata", + "inspect_model_checkpoint", "list_algorithms", "list_all_algorithm_metadata", "load_algorithm_from_pretrained", diff --git a/src/adaptive_rl/cli.py b/src/adaptive_rl/cli.py index ac8797b..4fd0d1d 100644 --- a/src/adaptive_rl/cli.py +++ b/src/adaptive_rl/cli.py @@ -1677,75 +1677,107 @@ def demo_drone( max_steps: int = typer.Option(150, "--max-steps", help="Maximum steps for demo episode"), ) -> None: """Run a deterministic, visual demonstration of the trained drone agent.""" - if not model.exists(): - console.print(f"[bold red]Model file not found:[/bold red] {model}") + if max_steps <= 0: + console.print( + f"[bold red]Invalid max_steps:[/bold red] max_steps must be > 0, got {max_steps}" + ) + raise typer.Exit(code=1) + + if not model.is_file(): + console.print(f"[bold red]Model file not found:[/bold red] {model} is not a valid file") raise typer.Exit(code=1) + env_name = "drone" env_kwargs = {} - if config is not None and config.exists(): + cfg = None + if config is not None: + if not config.is_file(): + console.print(f"[bold red]Configuration file not found:[/bold red] {config}") + raise typer.Exit(code=1) cfg = load_config(config) env_kwargs = cfg.environment.parameters + if hasattr(cfg.environment, "name") and cfg.environment.name: + env_name = cfg.environment.name from adaptive_rl.algorithms.registry import load_algorithm_from_pretrained - env = make_env("drone", **env_kwargs) - algo_hint = cfg.algorithm.name if "cfg" in locals() and cfg is not None else None - algo = load_algorithm_from_pretrained(model, env=env, algorithm_name=algo_hint) - - obs, info = env.reset(seed=seed) - console.print( - Panel.fit( - f"[bold green]Starting Autonomous Drone 3D Navigation Demo[/bold green]\n\n" - f"âĸ [bold]Model:[/bold] {model}\n" - f"âĸ [bold]Seed:[/bold] {seed}\n" - f"âĸ [bold]Start Position:[/bold] {info.get('position')}\n" - f"âĸ [bold]Target Waypoint:[/bold] {info.get('goal')}\n" - f"âĸ [bold]Obstacles in Arena:[/bold] {info.get('num_obstacles')}\n" - f"âĸ [bold]Initial Distance to Goal:[/bold] {info.get('distance_to_goal', 0):.2f}m", - title="Demonstration Flight", - border_style="cyan", - ) - ) - - if hasattr(env, "render"): - rendered = env.render() - if rendered: - console.print(rendered) - - total_reward = 0.0 + env = make_env(env_name, **env_kwargs) + step_info: Dict[str, Any] = {} outcome = "UNKNOWN" + total_reward = 0.0 step_num = 0 + try: + algo_hint = cfg.algorithm.name if cfg is not None else None + algo = load_algorithm_from_pretrained(model, env=env, algorithm_name=algo_hint) + + # Verify model compatibility with environment spaces + if hasattr(algo, "model") and algo.model is not None: + m = algo.model + if hasattr(m, "observation_space") and m.observation_space is not None: + if m.observation_space.shape != env.observation_space.shape: + console.print( + f"[bold red]Model observation space mismatch:[/bold red] " + f"model expects {m.observation_space.shape}, environment has {env.observation_space.shape}" + ) + raise typer.Exit(code=1) + if hasattr(m, "action_space") and m.action_space is not None: + if m.action_space.shape != env.action_space.shape: + console.print( + f"[bold red]Model action space mismatch:[/bold red] " + f"model expects {m.action_space.shape}, environment has {env.action_space.shape}" + ) + raise typer.Exit(code=1) - for step_num in range(1, max_steps + 1): - action, _ = algo.predict(obs, deterministic=True) - obs, reward, terminated, truncated, step_info = env.step(action) - total_reward += float(reward) - - if step_num % 10 == 0 or terminated or truncated: - dist = step_info.get("distance_to_goal", 0.0) - alt = step_info.get("altitude", 0.0) - spd = step_info.get("speed", 0.0) - min_obs = step_info.get("min_obstacle_distance", 0.0) - console.print( - f"Step {step_num:03d} | Alt: {alt:4.1f}m | Spd: {spd:4.1f}m/s | " - f"Dist: {dist:5.1f}m | Min Obs: {min_obs:4.1f}m | Reward: {reward:+6.2f}" + obs, info = env.reset(seed=seed) + console.print( + Panel.fit( + f"[bold green]Starting Autonomous Drone 3D Navigation Demo ({env_name})[/bold green]\n\n" + f"âĸ [bold]Model:[/bold] {model}\n" + f"âĸ [bold]Seed:[/bold] {seed}\n" + f"âĸ [bold]Start Position:[/bold] {info.get('position')}\n" + f"âĸ [bold]Target Waypoint:[/bold] {info.get('goal')}\n" + f"âĸ [bold]Obstacles in Arena:[/bold] {info.get('num_obstacles')}\n" + f"âĸ [bold]Initial Distance to Goal:[/bold] {info.get('distance_to_goal', 0):.2f}m", + title="Demonstration Flight", + border_style="cyan", ) + ) - if terminated or truncated: - if step_info.get("success"): - outcome = "SUCCESS" - elif step_info.get("collision"): - outcome = f"FAILED / COLLISION ({step_info.get('collision_type', 'obstacle')})" - else: - outcome = "FAILED / MAX STEPS REACHED" - break + if hasattr(env, "render"): + rendered = env.render() + if rendered: + console.print(rendered) + + for step_num in range(1, max_steps + 1): + action, _ = algo.predict(obs, deterministic=True) + obs, reward, terminated, truncated, step_info = env.step(action) + total_reward += float(reward) + + if step_num % 10 == 0 or terminated or truncated: + dist = step_info.get("distance_to_goal", 0.0) + alt = step_info.get("altitude", 0.0) + spd = step_info.get("speed", 0.0) + min_obs = step_info.get("min_obstacle_distance", 0.0) + console.print( + f"Step {step_num:03d} | Alt: {alt:4.1f}m | Spd: {spd:4.1f}m/s | " + f"Dist: {dist:5.1f}m | Min Obs: {min_obs:4.1f}m | Reward: {reward:+6.2f}" + ) - if hasattr(env, "render"): - rendered = env.render() - if rendered: - console.print(rendered) + if terminated or truncated: + if step_info.get("success"): + outcome = "SUCCESS" + elif step_info.get("collision"): + outcome = f"FAILED / COLLISION ({step_info.get('collision_type', 'obstacle')})" + else: + outcome = "FAILED / MAX STEPS REACHED" + break - env.close() + if hasattr(env, "render"): + rendered = env.render() + if rendered: + console.print(rendered) + finally: + env.close() style = "bold green" if outcome == "SUCCESS" else "bold red" console.print( diff --git a/src/adaptive_rl/environments/drone_6dof.py b/src/adaptive_rl/environments/drone_6dof.py index 04a57f8..af18f8c 100644 --- a/src/adaptive_rl/environments/drone_6dof.py +++ b/src/adaptive_rl/environments/drone_6dof.py @@ -61,7 +61,7 @@ import math from dataclasses import dataclass -from typing import Any, Dict, List, Optional, Tuple +from typing import Any, Dict, List, Optional, Sequence, Tuple import numpy as np from gymnasium import spaces @@ -75,18 +75,21 @@ ) -def quaternion_to_rotation_matrix(q: np.ndarray) -> np.ndarray: +def quaternion_to_rotation_matrix(q: np.ndarray | Sequence[float]) -> np.ndarray: """Convert a unit quaternion [qw, qx, qy, qz] to a 3x3 rotation matrix (body to world).""" - qw, qx, qy, qz = float(q[0]), float(q[1]), float(q[2]), float(q[3]) - # Normalize for safety + q_arr = np.asarray(q, dtype=np.float64) + if q_arr.shape != (4,): + raise ValueError(f"quaternion must have shape (4,), got {q_arr.shape}") + if not np.all(np.isfinite(q_arr)): + raise ValueError(f"quaternion must have finite coordinates, got {q}") + qw, qx, qy, qz = float(q_arr[0]), float(q_arr[1]), float(q_arr[2]), float(q_arr[3]) norm = math.sqrt(qw * qw + qx * qx + qy * qy + qz * qz) - if norm > 1e-12: - qw /= norm - qx /= norm - qy /= norm - qz /= norm - else: - qw, qx, qy, qz = 1.0, 0.0, 0.0, 0.0 + if norm < 1e-12: + raise ValueError("quaternion cannot have zero norm") + qw /= norm + qx /= norm + qy /= norm + qz /= norm return np.array( [ @@ -110,6 +113,45 @@ def quaternion_to_rotation_matrix(q: np.ndarray) -> np.ndarray: ) +def quaternion_to_euler_angles(q: np.ndarray) -> Tuple[float, float, float]: + """Convert a unit quaternion [qw, qx, qy, qz] to Euler angles (roll, pitch, yaw) in radians. + + Conventions: + - Roll (phi): rotation about body +X (East/Forward) + - Pitch (theta): rotation about body +Y (North/Left) + - Yaw (psi): rotation about body +Z (Upward) + + Returns: + (roll, pitch, yaw) in radians + """ + qw, qx, qy, qz = float(q[0]), float(q[1]), float(q[2]), float(q[3]) + norm = math.sqrt(qw * qw + qx * qx + qy * qy + qz * qz) + if norm > 1e-12: + qw /= norm + qx /= norm + qy /= norm + qz /= norm + else: + qw, qx, qy, qz = 1.0, 0.0, 0.0, 0.0 + + # Roll (x-axis rotation) + sinr_cosp = 2.0 * (qw * qx + qy * qz) + cosr_cosp = 1.0 - 2.0 * (qx * qx + qy * qy) + roll = math.atan2(sinr_cosp, cosr_cosp) + + # Pitch (y-axis rotation) + sinp = 2.0 * (qw * qy - qz * qx) + sinp = max(-1.0, min(1.0, sinp)) + pitch = math.asin(sinp) + + # Yaw (z-axis rotation) + siny_cosp = 2.0 * (qw * qz + qx * qy) + cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz) + yaw = math.atan2(siny_cosp, cosy_cosp) + + return roll, pitch, yaw + + def quaternion_derivative(q: np.ndarray, omega: np.ndarray) -> np.ndarray: """Compute time derivative of quaternion given body angular velocity omega [p, q, r].""" qw, qx, qy, qz = float(q[0]), float(q[1]), float(q[2]), float(q[3]) @@ -162,14 +204,24 @@ def __init__( dt: float = 0.05, substeps: int = 5, ) -> None: - if mass <= 0.0: - raise ValueError(f"Mass must be positive, got {mass}") - if arm_length <= 0.0: - raise ValueError(f"Arm length must be positive, got {arm_length}") - if max_thrust_per_motor <= 0.0: - raise ValueError(f"max_thrust_per_motor must be positive, got {max_thrust_per_motor}") - if dt <= 0.0: - raise ValueError(f"dt must be positive, got {dt}") + for name, val in [ + ("mass", mass), + ("arm_length", arm_length), + ("max_thrust_per_motor", max_thrust_per_motor), + ("dt", dt), + ]: + if not math.isfinite(val) or val <= 0.0: + raise ValueError(f"{name} must be a positive finite float, got {val}") + + for name, val in [ + ("gravity", gravity), + ("torque_to_thrust_ratio", torque_to_thrust_ratio), + ("linear_damping", linear_damping), + ("angular_damping", angular_damping), + ]: + if not math.isfinite(val) or val < 0.0: + raise ValueError(f"{name} must be a non-negative finite float, got {val}") + if substeps < 1: raise ValueError(f"substeps must be >= 1, got {substeps}") @@ -189,11 +241,17 @@ def __init__( if inertia is None: self.inertia_diag = np.array([0.01, 0.01, 0.02], dtype=np.float64) else: + if not isinstance(inertia, (tuple, list, np.ndarray)) or len(inertia) != 3: + raise ValueError(f"Inertia must have exactly three components, got {inertia}") self.inertia_diag = np.array( [float(inertia[0]), float(inertia[1]), float(inertia[2])], dtype=np.float64 ) - if any(val <= 0.0 for val in self.inertia_diag): - raise ValueError(f"Inertia components must be strictly positive, got {inertia}") + if not np.all(np.isfinite(self.inertia_diag)) or any( + val <= 0.0 for val in self.inertia_diag + ): + raise ValueError( + f"Inertia components must be strictly positive and finite, got {inertia}" + ) self.inv_inertia = 1.0 / self.inertia_diag self.hover_thrust_per_motor = (self.mass * self.gravity) / 4.0 @@ -213,30 +271,42 @@ def reset( angular_velocity: Optional[np.ndarray] = None, ) -> None: """Reset the internal physical state.""" - self.state.position = ( - np.asarray(position, dtype=np.float64).copy() - if position is not None - else np.zeros(3, dtype=np.float64) - ) - self.state.velocity = ( - np.asarray(velocity, dtype=np.float64).copy() - if velocity is not None - else np.zeros(3, dtype=np.float64) - ) + if position is not None: + pos = np.asarray(position, dtype=np.float64) + if pos.shape != (3,) or not np.all(np.isfinite(pos)): + raise ValueError(f"Position must be a finite 3-element vector, got {position}") + self.state.position = pos.copy() + else: + self.state.position = np.zeros(3, dtype=np.float64) + + if velocity is not None: + vel = np.asarray(velocity, dtype=np.float64) + if vel.shape != (3,) or not np.all(np.isfinite(vel)): + raise ValueError(f"Velocity must be a finite 3-element vector, got {velocity}") + self.state.velocity = vel.copy() + else: + self.state.velocity = np.zeros(3, dtype=np.float64) + if quaternion is not None: q = np.asarray(quaternion, dtype=np.float64) - q_norm = np.linalg.norm(q) - self.state.quaternion = ( - (q / q_norm).copy() if q_norm > 1e-12 else np.array([1.0, 0.0, 0.0, 0.0]) - ) + if q.shape != (4,) or not np.all(np.isfinite(q)): + raise ValueError(f"Quaternion must be a finite 4-element vector, got {quaternion}") + q_norm = float(np.linalg.norm(q)) + if q_norm < 1e-12: + raise ValueError("Quaternion cannot have zero norm") + self.state.quaternion = (q / q_norm).copy() else: self.state.quaternion = np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64) - self.state.angular_velocity = ( - np.asarray(angular_velocity, dtype=np.float64).copy() - if angular_velocity is not None - else np.zeros(3, dtype=np.float64) - ) + if angular_velocity is not None: + omega = np.asarray(angular_velocity, dtype=np.float64) + if omega.shape != (3,) or not np.all(np.isfinite(omega)): + raise ValueError( + f"Angular velocity must be a finite 3-element vector, got {angular_velocity}" + ) + self.state.angular_velocity = omega.copy() + else: + self.state.angular_velocity = np.zeros(3, dtype=np.float64) def compute_forces_and_torques(self, motor_thrusts: np.ndarray) -> Tuple[float, np.ndarray]: """Compute collective thrust and body torques from 4 individual rotor thrusts. @@ -250,7 +320,12 @@ def compute_forces_and_torques(self, motor_thrusts: np.ndarray) -> Tuple[float, Returns: (total_thrust, torques_vector [tau_x, tau_y, tau_z]) """ - f = np.clip(np.asarray(motor_thrusts, dtype=np.float64), 0.0, self.max_thrust_per_motor) + f_arr = np.asarray(motor_thrusts, dtype=np.float64) + if f_arr.shape != (4,): + raise ValueError(f"Motor thrusts must have exactly 4 elements, got shape {f_arr.shape}") + # Sanitize non-finite values safely + f_sanitized = np.nan_to_num(f_arr, nan=0.0, posinf=self.max_thrust_per_motor, neginf=0.0) + f = np.clip(f_sanitized, 0.0, self.max_thrust_per_motor) f1, f2, f3, f4 = f[0], f[1], f[2], f[3] total_thrust = float(f1 + f2 + f3 + f4) @@ -407,39 +482,112 @@ def __init__( ) -> None: super().__init__() - if any(b <= 0.0 for b in bounds): - raise ValueError(f"Bounds must be strictly positive, got {bounds}") - if max_steps < 1: - raise ValueError(f"max_steps must be >= 1, got {max_steps}") - if target_radius <= 0.0: - raise ValueError(f"target_radius must be positive, got {target_radius}") - if collision_radius <= 0.0: - raise ValueError(f"collision_radius must be positive, got {collision_radius}") - if lidar_noise_std < 0.0: - raise ValueError(f"lidar_noise_std cannot be negative, got {lidar_noise_std}") - if not (0.0 <= lidar_dropout_prob <= 1.0): + # Bounds: three finite positive dimensions + if ( + not isinstance(bounds, (tuple, list, np.ndarray)) + or len(bounds) != 3 + or not all(math.isfinite(float(b)) and float(b) > 0.0 for b in bounds) + ): + raise ValueError(f"Bounds must be three finite positive numbers, got {bounds}") + self.bounds = (float(bounds[0]), float(bounds[1]), float(bounds[2])) + + # Start and Goal positions: three finite coordinates inside valid arena limits + if start_pos is not None: + s_arr = np.asarray(start_pos, dtype=np.float64) + if s_arr.shape != (3,) or not np.all(np.isfinite(s_arr)): + raise ValueError(f"start_pos must be a finite 3-element vector, got {start_pos}") + if any(s_arr[i] < 0.0 or s_arr[i] > self.bounds[i] for i in range(3)): + raise ValueError(f"start_pos {s_arr} is outside valid arena bounds {self.bounds}") + self.default_start = s_arr + else: + self.default_start = np.array([5.0, 5.0, 5.0], dtype=np.float64) + + if goal_pos is not None: + g_arr = np.asarray(goal_pos, dtype=np.float64) + if g_arr.shape != (3,) or not np.all(np.isfinite(g_arr)): + raise ValueError(f"goal_pos must be a finite 3-element vector, got {goal_pos}") + if any(g_arr[i] < 0.0 or g_arr[i] > self.bounds[i] for i in range(3)): + raise ValueError(f"goal_pos {g_arr} is outside valid arena bounds {self.bounds}") + self.default_goal = g_arr + else: + self.default_goal = np.array( + [self.bounds[0] - 5.0, self.bounds[1] - 5.0, self.bounds[2] - 5.0], + dtype=np.float64, + ) + + # Obstacle count: non-negative integer + if ( + isinstance(num_obstacles, bool) + or not isinstance(num_obstacles, (int, np.integer)) + or num_obstacles < 0 + ): + raise ValueError(f"Obstacle count must be a non-negative integer, got {num_obstacles}") + + # Obstacle radius: positive finite value + if not math.isfinite(obstacle_radius) or obstacle_radius <= 0.0: + raise ValueError( + f"Obstacle radius must be a positive finite float, got {obstacle_radius}" + ) + + # Target and collision radius: positive finite values + if not math.isfinite(target_radius) or target_radius <= 0.0: + raise ValueError(f"target_radius must be a positive finite float, got {target_radius}") + if not math.isfinite(collision_radius) or collision_radius <= 0.0: + raise ValueError( + f"collision_radius must be a positive finite float, got {collision_radius}" + ) + + # LiDAR ray count: positive integer + if ( + isinstance(num_lidar_rays, bool) + or not isinstance(num_lidar_rays, (int, np.integer)) + or num_lidar_rays < 1 + ): + raise ValueError(f"LiDAR ray count must be a positive integer, got {num_lidar_rays}") + + # LiDAR range: positive finite value + if not math.isfinite(lidar_range) or lidar_range <= 0.0: + raise ValueError(f"LiDAR range must be a positive finite float, got {lidar_range}") + + # Sensor noise and dropout: finite valid values + if not math.isfinite(lidar_noise_std) or lidar_noise_std < 0.0: + raise ValueError( + f"lidar_noise_std must be a non-negative finite float, got {lidar_noise_std}" + ) + if not math.isfinite(lidar_dropout_prob) or not (0.0 <= lidar_dropout_prob <= 1.0): raise ValueError( f"lidar_dropout_prob must be between 0.0 and 1.0, got {lidar_dropout_prob}" ) - if not (0.0 <= lidar_min_range < lidar_range): + if not math.isfinite(lidar_min_range) or not (0.0 <= lidar_min_range < lidar_range): raise ValueError( f"lidar_min_range must be >= 0.0 and < lidar_range ({lidar_range}), got {lidar_min_range}" ) - self.bounds = (float(bounds[0]), float(bounds[1]), float(bounds[2])) - self.default_start = ( - np.asarray(start_pos, dtype=np.float64) - if start_pos is not None - else np.array([5.0, 5.0, 5.0], dtype=np.float64) - ) - self.default_goal = ( - np.asarray(goal_pos, dtype=np.float64) - if goal_pos is not None - else np.array( - [self.bounds[0] - 5.0, self.bounds[1] - 5.0, self.bounds[2] - 5.0], - dtype=np.float64, - ) - ) + # Episode limits: positive integer + if ( + isinstance(max_steps, bool) + or not isinstance(max_steps, (int, np.integer)) + or max_steps < 1 + ): + raise ValueError(f"max_steps must be a positive integer, got {max_steps}") + + # Reward coefficients: finite and mathematically valid + for r_name, r_val in [ + ("step_penalty", step_penalty), + ("goal_reward", goal_reward), + ("collision_reward", collision_reward), + ("progress_weight", progress_weight), + ]: + if not math.isfinite(r_val): + raise ValueError(f"{r_name} must be a finite float, got {r_val}") + + for p_name, p_val in [ + ("action_penalty_weight", action_penalty_weight), + ("angular_rate_penalty_weight", angular_rate_penalty_weight), + ("tilt_penalty_weight", tilt_penalty_weight), + ]: + if not math.isfinite(p_val) or p_val < 0.0: + raise ValueError(f"{p_name} must be a non-negative finite float, got {p_val}") self.num_obstacles = int(num_obstacles) self.obstacle_radius = float(obstacle_radius) @@ -563,19 +711,54 @@ def _get_obs(self) -> np.ndarray: ) return np.asarray(np.clip(raw, -1.0, 1.0), dtype=np.float32) - def _check_collision(self, pos: np.ndarray) -> Tuple[bool, str]: + def _check_collision( + self, pos: np.ndarray, prev_pos: Optional[np.ndarray] = None + ) -> Tuple[bool, str]: r = self.collision_radius bx, by, bz = self.bounds - if pos[0] - r <= 0.0 or pos[0] + r >= bx: + if prev_pos is None: + if pos[0] - r <= 0.0 or pos[0] + r >= bx: + return True, "boundary_x" + if pos[1] - r <= 0.0 or pos[1] + r >= by: + return True, "boundary_y" + if pos[2] - r <= 0.0 or pos[2] + r >= bz: + return True, "boundary_z" + + for obs in self._obstacles: + if obs.collides_with(pos, r): + return True, "obstacle" + return False, "none" + + # Continuous swept collision check between prev_pos and pos + p0 = np.asarray(prev_pos, dtype=np.float64) + p1 = np.asarray(pos, dtype=np.float64) + + # 1. Swept boundary check + min_p = np.minimum(p0, p1) + max_p = np.maximum(p0, p1) + if min_p[0] - r <= 0.0 or max_p[0] + r >= bx: return True, "boundary_x" - if pos[1] - r <= 0.0 or pos[1] + r >= by: + if min_p[1] - r <= 0.0 or max_p[1] + r >= by: return True, "boundary_y" - if pos[2] - r <= 0.0 or pos[2] + r >= bz: + if min_p[2] - r <= 0.0 or max_p[2] + r >= bz: return True, "boundary_z" + # 2. Swept obstacle spheres check + seg = p1 - p0 + seg_len_sq = float(np.dot(seg, seg)) + for obs in self._obstacles: - if obs.collides_with(pos, r): + c = obs.center + threshold = obs.radius + r + if seg_len_sq < 1e-12: + dist = float(np.linalg.norm(p0 - c)) + else: + t = float(np.dot(c - p0, seg) / seg_len_sq) + t_clamped = max(0.0, min(1.0, t)) + closest_point = p0 + t_clamped * seg + dist = float(np.linalg.norm(closest_point - c)) + if dist <= threshold: return True, "obstacle" return False, "none" @@ -676,21 +859,30 @@ def reset( num_obs = self.num_obstacles if options and "num_obstacles" in options: - num_obs = int(options["num_obstacles"]) + raw_obs = options["num_obstacles"] + if ( + isinstance(raw_obs, bool) + or not isinstance(raw_obs, (int, np.integer)) + or raw_obs < 0 + ): + raise ValueError( + f"num_obstacles in options must be a non-negative integer, got {raw_obs}" + ) + num_obs = int(raw_obs) if options and "lidar_noise_std" in options: n_std = float(options["lidar_noise_std"]) - if n_std < 0.0: - raise ValueError(f"lidar_noise_std cannot be negative, got {n_std}") + if not math.isfinite(n_std) or n_std < 0.0: + raise ValueError(f"lidar_noise_std cannot be negative or non-finite, got {n_std}") self.lidar_noise_std = n_std if options and "lidar_dropout_prob" in options: d_prob = float(options["lidar_dropout_prob"]) - if not (0.0 <= d_prob <= 1.0): + if not math.isfinite(d_prob) or not (0.0 <= d_prob <= 1.0): raise ValueError(f"lidar_dropout_prob must be between 0.0 and 1.0, got {d_prob}") self.lidar_dropout_prob = d_prob if options and "lidar_min_range" in options: m_range = float(options["lidar_min_range"]) - if not (0.0 <= m_range < self.lidar_range): + if not math.isfinite(m_range) or not (0.0 <= m_range < self.lidar_range): raise ValueError( f"lidar_min_range must be >= 0.0 and < lidar_range ({self.lidar_range}), got {m_range}" ) @@ -723,20 +915,25 @@ def step( action: np.ndarray, ) -> Tuple[np.ndarray, float, bool, bool, Dict[str, Any]]: act_arr = np.asarray(action, dtype=np.float32) + if act_arr.shape != (4,): + raise ValueError(f"Action must have shape (4,), got {act_arr.shape}") + if not np.all(np.isfinite(act_arr)): + act_arr = np.nan_to_num(act_arr, nan=0.0, posinf=1.0, neginf=-1.0) if not self.action_space.contains(act_arr): act_arr = np.clip(act_arr, -1.0, 1.0) self._current_step += 1 + old_pos = self._position.copy() thrusts = self._action_to_motor_thrusts(act_arr) new_state = self.dynamics.step_rk4(thrusts) - self._position = new_state.position + self._position = new_state.position.copy() curr_distance = float(np.linalg.norm(self._goal - self._position)) dist_delta = self._prev_distance_to_goal - curr_distance self._prev_distance_to_goal = curr_distance - is_collision, collision_type = self._check_collision(self._position) + is_collision, collision_type = self._check_collision(self._position, prev_pos=old_pos) is_success = curr_distance <= self.target_radius terminated = False @@ -751,12 +948,38 @@ def step( reward = self.collision_reward info["success"] = False info["is_success"] = False + info["reward_components"] = { + "progress_reward": 0.0, + "step_penalty": 0.0, + "effort_penalty": 0.0, + "rate_penalty": 0.0, + "tilt_penalty": 0.0, + "terminal_reward": self.collision_reward, + } + info["progress_reward"] = 0.0 + info["effort_penalty"] = 0.0 + info["rate_penalty"] = 0.0 + info["tilt_penalty"] = 0.0 + info["terminal_reward"] = self.collision_reward if self.terminate_on_collision: terminated = True elif is_success: reward = self.goal_reward info["success"] = True info["is_success"] = True + info["reward_components"] = { + "progress_reward": 0.0, + "step_penalty": 0.0, + "effort_penalty": 0.0, + "rate_penalty": 0.0, + "tilt_penalty": 0.0, + "terminal_reward": self.goal_reward, + } + info["progress_reward"] = 0.0 + info["effort_penalty"] = 0.0 + info["rate_penalty"] = 0.0 + info["tilt_penalty"] = 0.0 + info["terminal_reward"] = self.goal_reward terminated = True else: action_effort = float(np.sum(np.square(act_arr))) @@ -775,6 +998,19 @@ def step( reward = float( progress_reward + self.step_penalty - effort_penalty - rate_penalty - tilt_pen ) + info["reward_components"] = { + "progress_reward": progress_reward, + "step_penalty": self.step_penalty, + "effort_penalty": -effort_penalty, + "rate_penalty": -rate_penalty, + "tilt_penalty": -tilt_pen, + "terminal_reward": 0.0, + } + info["progress_reward"] = progress_reward + info["effort_penalty"] = effort_penalty + info["rate_penalty"] = rate_penalty + info["tilt_penalty"] = tilt_pen + info["terminal_reward"] = 0.0 if self._current_step >= self.max_steps and not terminated: truncated = True diff --git a/src/adaptive_rl/gui/__init__.py b/src/adaptive_rl/gui/__init__.py index 3775df0..cceb290 100644 --- a/src/adaptive_rl/gui/__init__.py +++ b/src/adaptive_rl/gui/__init__.py @@ -8,6 +8,7 @@ build_recovery_curve_figure, build_training_curve_figure, get_available_models, + inspect_model_checkpoint, run_drone_simulation_episode, ) @@ -19,5 +20,6 @@ "build_recovery_curve_figure", "build_training_curve_figure", "get_available_models", + "inspect_model_checkpoint", "run_drone_simulation_episode", ] diff --git a/src/adaptive_rl/gui/visualizer.py b/src/adaptive_rl/gui/visualizer.py index 26ba648..941057f 100644 --- a/src/adaptive_rl/gui/visualizer.py +++ b/src/adaptive_rl/gui/visualizer.py @@ -1,12 +1,108 @@ """Plotly-based 3D visual arena and sensor visualizer for AdaptiveRL.""" +import math from pathlib import Path from typing import Any, Dict, List, Optional, Sequence, Tuple import numpy as np import plotly.graph_objects as go +from adaptive_rl.algorithms.registry import inspect_model_checkpoint from adaptive_rl.environments.drone import ObstacleSphere3D +from adaptive_rl.environments.drone_6dof import ( + quaternion_to_rotation_matrix, +) + + +def compute_quadrotor_3d_geometry( + center_pos: np.ndarray | Sequence[float], + quaternion: Optional[np.ndarray | Sequence[float]] = None, + arm_span: float = 1.2, + rotor_radius: float = 0.28, + n_propeller_points: int = 16, +) -> Dict[str, Any]: + """Compute 3D world-coordinate geometry for an X-configuration quadrotor. + + Geometry components: + - Central body fuselage and forward heading nose pointer (+X_B direction) + - Front arms (connecting center to Rotor 1 [FR] and Rotor 2 [FL]) + - Rear arms (connecting center to Rotor 3 [RL] and Rotor 4 [RR]) + - 4 rotor hubs (R1: Front-Right, R2: Front-Left, R3: Rear-Left, R4: Rear-Right) + - 4 circular propeller blade paths + + Transformation: + P_W = center_pos + R(q) @ P_B + """ + c_pos = np.asarray(center_pos, dtype=np.float64) + if c_pos.shape != (3,): + raise ValueError(f"center_pos must have shape (3,), got {c_pos.shape}") + if not np.all(np.isfinite(c_pos)): + raise ValueError(f"center_pos must have finite coordinates, got {center_pos}") + + if quaternion is not None: + q_arr = np.asarray(quaternion, dtype=np.float64) + if q_arr.shape != (4,): + raise ValueError(f"quaternion must have shape (4,), got {q_arr.shape}") + if not np.all(np.isfinite(q_arr)): + raise ValueError(f"quaternion must have finite coordinates, got {quaternion}") + rot_mat = quaternion_to_rotation_matrix(q_arr) + else: + rot_mat = np.eye(3, dtype=np.float64) + + # Arm offset in local body frame (+X forward, +Y left, +Z up) + # Rotor 1: (+d, -d, 0) [Front-Right] + # Rotor 2: (+d, +d, 0) [Front-Left] + # Rotor 3: (-d, +d, 0) [Rear-Left] + # Rotor 4: (-d, -d, 0) [Rear-Right] + d = float(arm_span) / (2.0 * math.sqrt(2.0)) + + body_rotors = [ + np.array([d, -d, 0.0], dtype=np.float64), # R1 Front-Right + np.array([d, d, 0.0], dtype=np.float64), # R2 Front-Left + np.array([-d, d, 0.0], dtype=np.float64), # R3 Rear-Left + np.array([-d, -d, 0.0], dtype=np.float64), # R4 Rear-Right + ] + + # Transform rotors to world coordinates + world_rotors = [c_pos + rot_mat @ r_b for r_b in body_rotors] + + # Forward heading indicator: nose pointer along +X_B + nose_body = np.array([d * 0.95, 0.0, 0.0], dtype=np.float64) + nose_world = c_pos + rot_mat @ nose_body + + # Central fuselage ring / diamond in body frame + fuselage_pts_b = [ + np.array([d * 0.35, 0.0, 0.0], dtype=np.float64), + np.array([0.0, d * 0.35, 0.0], dtype=np.float64), + np.array([-d * 0.35, 0.0, 0.0], dtype=np.float64), + np.array([0.0, -d * 0.35, 0.0], dtype=np.float64), + np.array([d * 0.35, 0.0, 0.0], dtype=np.float64), + ] + fuselage_pts_w = [c_pos + rot_mat @ p for p in fuselage_pts_b] + + # Propeller rings in body frame (circles in XY plane at z = +0.03) + theta = np.linspace(0.0, 2.0 * math.pi, n_propeller_points) + propellers_w: List[np.ndarray] = [] + for r_b in body_rotors: + ring_pts_b = np.column_stack( + [ + r_b[0] + rotor_radius * np.cos(theta), + r_b[1] + rotor_radius * np.sin(theta), + np.full_like(theta, r_b[2] + 0.03), + ] + ) + ring_pts_w = np.array([c_pos + rot_mat @ pt for pt in ring_pts_b]) + propellers_w.append(ring_pts_w) + + return { + "center": c_pos, + "rotors": world_rotors, + "nose": nose_world, + "fuselage": fuselage_pts_w, + "propellers": propellers_w, + "front_arms": (world_rotors[0], c_pos, world_rotors[1]), + "rear_arms": (world_rotors[2], c_pos, world_rotors[3]), + } def _generate_sphere_surface( @@ -86,6 +182,8 @@ def build_arena_3d_figure( lidar_ranges: Optional[Sequence[float] | np.ndarray] = None, show_lidar: bool = True, title: str = "3D Drone Flight Arena", + arm_span: float = 1.2, + rotor_radius: float = 0.28, ) -> go.Figure: """Build an interactive 3D Plotly flight arena visualizer. @@ -226,8 +324,121 @@ def build_arena_3d_figure( ) ) - # 7. Current Drone Position & Velocity + # 7. Current Drone Quadrotor Geometry, Attitude & Velocity if current_pos is not None: + geom = compute_quadrotor_3d_geometry( + current_pos, + quaternion=attitude_quaternion, + arm_span=arm_span, + rotor_radius=rotor_radius, + ) + + # Front Arms (connecting Center to R1 and R2 in X configuration) + fa = geom["front_arms"] + fig.add_trace( + go.Scatter3d( + x=[fa[0][0], fa[1][0], fa[2][0]], + y=[fa[0][1], fa[1][1], fa[2][1]], + z=[fa[0][2], fa[1][2], fa[2][2]], + mode="lines", + line=dict(color="#ef4444", width=6), + name="Front Arms", + hoverinfo="skip", + ) + ) + + # Rear Arms (connecting Center to R3 and R4 in X configuration) + ra = geom["rear_arms"] + fig.add_trace( + go.Scatter3d( + x=[ra[0][0], ra[1][0], ra[2][0]], + y=[ra[0][1], ra[1][1], ra[2][1]], + z=[ra[0][2], ra[1][2], ra[2][2]], + mode="lines", + line=dict(color="#3b82f6", width=6), + name="Rear Arms", + hoverinfo="skip", + ) + ) + + # Front Propellers (R1 and R2 circular blade paths) + p1, p2 = geom["propellers"][0], geom["propellers"][1] + fig.add_trace( + go.Scatter3d( + x=list(p1[:, 0]) + [None] + list(p2[:, 0]), + y=list(p1[:, 1]) + [None] + list(p2[:, 1]), + z=list(p1[:, 2]) + [None] + list(p2[:, 2]), + mode="lines", + line=dict(color="#f87171", width=3), + name="Front Propellers", + hoverinfo="skip", + ) + ) + + # Rear Propellers (R3 and R4 circular blade paths) + p3, p4 = geom["propellers"][2], geom["propellers"][3] + fig.add_trace( + go.Scatter3d( + x=list(p3[:, 0]) + [None] + list(p4[:, 0]), + y=list(p3[:, 1]) + [None] + list(p4[:, 1]), + z=list(p3[:, 2]) + [None] + list(p4[:, 2]), + mode="lines", + line=dict(color="#60a5fa", width=3), + name="Rear Propellers", + hoverinfo="skip", + ) + ) + + # 4 Rotor Hub Assemblies + rotors = geom["rotors"] + fig.add_trace( + go.Scatter3d( + x=[r[0] for r in rotors], + y=[r[1] for r in rotors], + z=[r[2] for r in rotors], + mode="markers", + marker=dict( + symbol="circle", + size=6, + color=["#ef4444", "#ef4444", "#3b82f6", "#3b82f6"], + ), + name="Rotor Assemblies", + hoverinfo="text", + text=["R1 (Front-Right)", "R2 (Front-Left)", "R3 (Rear-Left)", "R4 (Rear-Right)"], + ) + ) + + # Forward Heading Nose Pointer (+X_B) + c_pt = geom["center"] + n_pt = geom["nose"] + fig.add_trace( + go.Scatter3d( + x=[c_pt[0], n_pt[0]], + y=[c_pt[1], n_pt[1]], + z=[c_pt[2], n_pt[2]], + mode="lines+markers", + line=dict(color="#fbbf24", width=5), + marker=dict(size=[0, 6], color="#fbbf24"), + name="Forward Heading (+X_B)", + hoverinfo="text", + text=["Center", "Forward Heading (+X_B)"], + ) + ) + + # Central Body Fuselage & Center Marker + fuse = geom["fuselage"] + fig.add_trace( + go.Scatter3d( + x=[p[0] for p in fuse], + y=[p[1] for p in fuse], + z=[p[2] for p in fuse], + mode="lines", + line=dict(color="#cbd5e0", width=3), + name="Fuselage", + hoverinfo="skip", + ) + ) + fig.add_trace( go.Scatter3d( x=[current_pos[0]], @@ -235,7 +446,7 @@ def build_arena_3d_figure( z=[current_pos[2]], mode="markers+text", marker=dict( - symbol="cross", + symbol="diamond", size=10, color="#ed8936", line=dict(color="#ffffff", width=2), @@ -269,15 +480,8 @@ def build_arena_3d_figure( ) # 6-DOF Rigid-body Attitude Coordinate Frame (Roll/Pitch/Yaw) - if attitude_quaternion is not None and len(attitude_quaternion) == 4: - qw, qx, qy, qz = [float(v) for v in attitude_quaternion] - rot_mat = np.array( - [ - [1 - 2 * (qy**2 + qz**2), 2 * (qx * qy - qz * qw), 2 * (qx * qz + qy * qw)], - [2 * (qx * qy + qz * qw), 1 - 2 * (qx**2 + qz**2), 2 * (qy * qz - qx * qw)], - [2 * (qx * qz - qy * qw), 2 * (qy * qz + qx * qw), 1 - 2 * (qx**2 + qy**2)], - ] - ) + if attitude_quaternion is not None: + rot_mat = quaternion_to_rotation_matrix(attitude_quaternion) axis_len = 1.2 axes_colors = [ ("#ef4444", "Body X (Roll)"), @@ -391,22 +595,36 @@ def run_drone_simulation_episode( truncated = False step_count = 0 + num_rays = int(getattr(env, "num_lidar_rays", 16)) + lidar_range = float(getattr(env, "lidar_range", 20.0)) + act_shape = env.action_space.shape if hasattr(env, "action_space") else (3,) + + raw_lidar_init = obs[-num_rays:] if len(obs) >= num_rays else np.zeros(num_rays) + lidar_ranges_init = (raw_lidar_init * lidar_range).copy() + steps_data.append( { "step": 0, "position": info["position"].copy(), "velocity": info["velocity"].copy(), "speed": float(info.get("speed", 0.0)), + "angular_velocity": ( + info["angular_velocity"].copy() if "angular_velocity" in info else None + ), + "angular_speed": float(info.get("angular_speed", 0.0)), + "altitude": float(info.get("altitude", info["position"][2])), "distance_to_goal": float(info.get("distance_to_goal", 0.0)), "min_obstacle_distance": float(info.get("min_obstacle_distance", 0.0)), "reward": 0.0, "cumulative_reward": 0.0, - "lidar_ranges": (obs[13 : 13 + env.num_lidar_rays] * env.lidar_range).copy(), - "action": np.zeros(3, dtype=np.float32), + "lidar_ranges": lidar_ranges_init, + "action": np.zeros(act_shape, dtype=np.float32), "collision": False, + "collision_type": "none", "is_success": False, - "quaternion": info.get("quaternion"), + "quaternion": info.get("quaternion").copy() if "quaternion" in info else None, "wind_vector": info.get("wind_vector"), + "reward_components": info.get("reward_components"), } ) @@ -424,8 +642,8 @@ def run_drone_simulation_episode( vel = info["velocity"].copy() trajectory.append(pos) - raw_lidar_norm = obs[13 : 13 + env.num_lidar_rays] - lidar_ranges = (raw_lidar_norm * env.lidar_range).copy() + raw_lidar_norm = obs[-num_rays:] if len(obs) >= num_rays else np.zeros(num_rays) + lidar_ranges = (raw_lidar_norm * lidar_range).copy() steps_data.append( { @@ -433,6 +651,11 @@ def run_drone_simulation_episode( "position": pos, "velocity": vel, "speed": float(info.get("speed", 0.0)), + "angular_velocity": ( + info["angular_velocity"].copy() if "angular_velocity" in info else None + ), + "angular_speed": float(info.get("angular_speed", 0.0)), + "altitude": float(info.get("altitude", pos[2])), "distance_to_goal": float(info.get("distance_to_goal", 0.0)), "min_obstacle_distance": float(info.get("min_obstacle_distance", 0.0)), "reward": float(reward), @@ -440,9 +663,11 @@ def run_drone_simulation_episode( "lidar_ranges": lidar_ranges, "action": np.asarray(action, dtype=np.float32).copy(), "collision": bool(info.get("collision", False)), + "collision_type": str(info.get("collision_type", "none")), "is_success": bool(info.get("is_success", False) or info.get("success", False)), - "quaternion": info.get("quaternion"), + "quaternion": info.get("quaternion").copy() if "quaternion" in info else None, "wind_vector": info.get("wind_vector"), + "reward_components": info.get("reward_components"), } ) @@ -786,3 +1011,17 @@ def build_recovery_curve_figure( ) return fig + + +__all__ = [ + "build_arena_3d_figure", + "build_comparison_bar_chart", + "build_density_experiment_figure", + "build_distribution_shift_trajectory_figure", + "build_recovery_curve_figure", + "build_training_curve_figure", + "compute_quadrotor_3d_geometry", + "get_available_models", + "inspect_model_checkpoint", + "run_drone_simulation_episode", +] diff --git a/tests/test_cli.py b/tests/test_cli.py index f5ef7b0..4ff3c28 100644 --- a/tests/test_cli.py +++ b/tests/test_cli.py @@ -687,3 +687,52 @@ def test_cli_demo_help() -> None: assert result.exit_code == 0 clean_output = re.sub(r"\x1b\[[0-9;]*[a-zA-Z]", "", result.output) assert "--seed" in clean_output + + +def test_cli_demo_drone_edge_cases(tmp_path: Path) -> None: + """Verify demo-drone rejects max_steps <= 0, missing config, and non-file models.""" + model_file = tmp_path / "dummy_model.zip" + model_file.write_bytes(b"dummy") + + # 1. max_steps <= 0 + res_zero_steps = runner.invoke( + app, + ["demo-drone", "--model", str(model_file), "--max-steps", "0"], + ) + assert res_zero_steps.exit_code == 1 + clean_z = re.sub(r"\x1b\[[0-9;]*[a-zA-Z]", "", res_zero_steps.output) + assert "Invalid max_steps" in clean_z + + res_neg_steps = runner.invoke( + app, + ["demo-drone", "--model", str(model_file), "--max-steps", "-5"], + ) + assert res_neg_steps.exit_code == 1 + clean_n = re.sub(r"\x1b\[[0-9;]*[a-zA-Z]", "", res_neg_steps.output) + assert "Invalid max_steps" in clean_n + + # 2. Nonexistent --config + res_missing_config = runner.invoke( + app, + [ + "demo-drone", + "--model", + str(model_file), + "--config", + str(tmp_path / "nonexistent.yaml"), + ], + ) + assert res_missing_config.exit_code == 1 + clean_c = re.sub(r"\x1b\[[0-9;]*[a-zA-Z]", "", res_missing_config.output) + assert "Configuration file not found" in clean_c + + # 3. Model is a directory (not an actual file) + dummy_dir = tmp_path / "dir_model.zip" + dummy_dir.mkdir() + res_dir_model = runner.invoke( + app, + ["demo-drone", "--model", str(dummy_dir)], + ) + assert res_dir_model.exit_code == 1 + clean_m = re.sub(r"\x1b\[[0-9;]*[a-zA-Z]", "", res_dir_model.output) + assert "Model file not found" in clean_m diff --git a/tests/test_drone_6dof.py b/tests/test_drone_6dof.py index a1bfebe..ffddde5 100644 --- a/tests/test_drone_6dof.py +++ b/tests/test_drone_6dof.py @@ -5,6 +5,7 @@ import math import numpy as np +import pytest from gymnasium.utils.env_checker import check_env from adaptive_rl.environments import get, make_env @@ -238,3 +239,58 @@ def test_drone_6dof_deterministic_reset() -> None: ) env1.close() env2.close() + + +@pytest.mark.parametrize( + "invalid_kwargs,match_str", + [ + ({"bounds": (30.0, 30.0)}, "Bounds"), + ({"bounds": (30.0, 30.0, -5.0)}, "Bounds"), + ({"bounds": (float("nan"), 30.0, 15.0)}, "Bounds"), + ({"bounds": (float("inf"), 30.0, 15.0)}, "Bounds"), + ({"start_pos": (5.0, 5.0)}, "start_pos"), + ({"start_pos": (float("nan"), 5.0, 5.0)}, "start_pos"), + ({"start_pos": (-1.0, 5.0, 5.0)}, "outside valid arena bounds"), + ({"start_pos": (50.0, 5.0, 5.0)}, "outside valid arena bounds"), + ({"goal_pos": (5.0, 5.0)}, "goal_pos"), + ({"goal_pos": (5.0, float("inf"), 5.0)}, "goal_pos"), + ({"goal_pos": (5.0, -2.0, 5.0)}, "outside valid arena bounds"), + ({"num_obstacles": -1}, "Obstacle count"), + ({"num_obstacles": True}, "Obstacle count"), + ({"num_obstacles": 2.5}, "Obstacle count"), + ({"obstacle_radius": 0.0}, "Obstacle radius"), + ({"obstacle_radius": float("nan")}, "Obstacle radius"), + ({"obstacle_radius": -1.0}, "Obstacle radius"), + ({"target_radius": 0.0}, "target_radius"), + ({"collision_radius": -1.0}, "collision_radius"), + ({"num_lidar_rays": 0}, "LiDAR ray count"), + ({"num_lidar_rays": True}, "LiDAR ray count"), + ({"num_lidar_rays": -5}, "LiDAR ray count"), + ({"lidar_range": 0.0}, "LiDAR range"), + ({"lidar_range": float("inf")}, "LiDAR range"), + ({"lidar_noise_std": -0.1}, "lidar_noise_std"), + ({"lidar_noise_std": float("nan")}, "lidar_noise_std"), + ({"lidar_dropout_prob": 1.5}, "lidar_dropout_prob"), + ({"lidar_dropout_prob": float("nan")}, "lidar_dropout_prob"), + ({"lidar_min_range": -0.1}, "lidar_min_range"), + ({"lidar_min_range": 25.0}, "lidar_min_range"), + ({"max_steps": 0}, "max_steps"), + ({"max_steps": True}, "max_steps"), + ({"max_steps": -10}, "max_steps"), + ({"step_penalty": float("nan")}, "step_penalty"), + ({"goal_reward": float("inf")}, "goal_reward"), + ({"collision_reward": float("nan")}, "collision_reward"), + ({"progress_weight": float("nan")}, "progress_weight"), + ({"action_penalty_weight": -0.01}, "action_penalty_weight"), + ({"angular_rate_penalty_weight": float("nan")}, "angular_rate_penalty_weight"), + ({"tilt_penalty_weight": -0.5}, "tilt_penalty_weight"), + ({"inertia": (0.01, 0.01)}, "Inertia"), + ({"inertia": (0.01, 0.01, 0.02, 0.03)}, "Inertia"), + ({"inertia": (0.01, -0.01, 0.02)}, "Inertia"), + ({"inertia": (0.01, float("nan"), 0.02)}, "Inertia"), + ], +) +def test_drone_6dof_invalid_input_validation(invalid_kwargs: dict, match_str: str) -> None: + """Verify that invalid shapes, non-finite values, and out-of-range inputs raise ValueError.""" + with pytest.raises(ValueError, match=match_str): + Drone6DOFEnv(**invalid_kwargs) diff --git a/tests/test_gui.py b/tests/test_gui.py index 6bf2780..ff82d7e 100644 --- a/tests/test_gui.py +++ b/tests/test_gui.py @@ -11,6 +11,7 @@ import plotly.graph_objects as go from adaptive_rl.environments.drone import DroneNavigation3DEnv, ObstacleSphere3D +from adaptive_rl.environments.drone_6dof import Drone6DOFEnv from adaptive_rl.gui.visualizer import ( _generate_box_wireframe, _generate_sphere_surface, @@ -18,6 +19,7 @@ build_comparison_bar_chart, build_density_experiment_figure, build_training_curve_figure, + compute_quadrotor_3d_geometry, get_available_models, run_drone_simulation_episode, ) @@ -201,3 +203,295 @@ def test_build_recovery_curve_figure() -> None: assert any("Fixed Arm" in n for n in names) assert any("Adaptive Arm" in n for n in names) assert any("90% Recovery Level" in n for n in names) + + +def test_quadrotor_geometry_identity_and_translation() -> None: + """Verify quadrotor geometry at origin and with translation under identity quaternion.""" + center = np.array([10.0, 20.0, 5.0]) + arm_span = 1.0 + d = arm_span / (2.0 * np.sqrt(2.0)) + q_ident = np.array([1.0, 0.0, 0.0, 0.0]) + + geom = compute_quadrotor_3d_geometry(center_pos=center, quaternion=q_ident, arm_span=arm_span) + + assert np.allclose(geom["center"], center) + rotors = geom["rotors"] + assert len(rotors) == 4 + + # R1: Front-Right (+d, -d) + assert np.allclose(rotors[0], [10.0 + d, 20.0 - d, 5.0], atol=1e-5) + # R2: Front-Left (+d, +d) + assert np.allclose(rotors[1], [10.0 + d, 20.0 + d, 5.0], atol=1e-5) + # R3: Rear-Left (-d, +d) + assert np.allclose(rotors[2], [10.0 - d, 20.0 + d, 5.0], atol=1e-5) + # R4: Rear-Right (-d, -d) + assert np.allclose(rotors[3], [10.0 - d, 20.0 - d, 5.0], atol=1e-5) + + # All rotor distances from center must be exactly arm_span / 2 + for r in rotors: + dist = float(np.linalg.norm(r - center)) + assert np.isclose(dist, arm_span / 2.0, atol=1e-5) + + # Nose heading indicator is along +X direction + nose = geom["nose"] + assert nose[0] > center[0] + assert np.isclose(nose[1], center[1], atol=1e-5) + assert np.isclose(nose[2], center[2], atol=1e-5) + + +def test_quadrotor_geometry_yaw_90_rotation() -> None: + """Verify 90-degree yaw rotation (+Z) rotates body +X to world +Y.""" + center = np.array([0.0, 0.0, 0.0]) + arm_span = 1.0 + d = arm_span / (2.0 * np.sqrt(2.0)) + # 90 deg yaw: q = [cos(pi/4), 0, 0, sin(pi/4)] + q_yaw90 = np.array([np.cos(np.pi / 4), 0.0, 0.0, np.sin(np.pi / 4)]) + + geom = compute_quadrotor_3d_geometry(center_pos=center, quaternion=q_yaw90, arm_span=arm_span) + nose = geom["nose"] + # Nose should point along +Y + assert np.isclose(nose[0], 0.0, atol=1e-5) + assert nose[1] > 0.0 + assert np.isclose(nose[2], 0.0, atol=1e-5) + + rotors = geom["rotors"] + # R1 was (+d, -d), rotated by +90 yaw -> (+d, +d) + assert np.allclose(rotors[0], [d, d, 0.0], atol=1e-5) + # R2 was (+d, +d), rotated by +90 yaw -> (-d, +d) + assert np.allclose(rotors[1], [-d, d, 0.0], atol=1e-5) + + +def test_quadrotor_geometry_pitch_and_roll_transformations() -> None: + """Verify pitch and roll transformations tilt the drone arms and nose appropriately.""" + center = np.array([5.0, 5.0, 5.0]) + # 90 deg pitch (nose down): q = [cos(pi/4), 0, sin(pi/4), 0] + q_pitch90 = np.array([np.cos(np.pi / 4), 0.0, np.sin(np.pi / 4), 0.0]) + geom_pitch = compute_quadrotor_3d_geometry(center_pos=center, quaternion=q_pitch90) + nose = geom_pitch["nose"] + # Body +X rotated by pitch +90 points down (-Z) + assert np.isclose(nose[0], center[0], atol=1e-5) + assert np.isclose(nose[1], center[1], atol=1e-5) + assert nose[2] < center[2] + + # 90 deg roll (left wing up): q = [cos(pi/4), sin(pi/4), 0, 0] + q_roll90 = np.array([np.cos(np.pi / 4), np.sin(np.pi / 4), 0.0, 0.0]) + geom_roll = compute_quadrotor_3d_geometry(center_pos=center, quaternion=q_roll90) + # Rotors distance from center is preserved + for r in geom_roll["rotors"]: + assert np.all(np.isfinite(r)) + + +def test_quadrotor_geometry_finite_and_figure_traces() -> None: + """Verify full arena Plotly figure includes all quadrotor geometry traces.""" + pos = np.array([15.0, 15.0, 8.0]) + quat = np.array([0.92388, 0.38268, 0.0, 0.0]) # ~45 deg roll + fig = build_arena_3d_figure( + bounds=(30.0, 30.0, 15.0), + current_pos=pos, + attitude_quaternion=quat, + ) + trace_names = [trace.name for trace in fig.data if hasattr(trace, "name")] + assert "Front Arms" in trace_names + assert "Rear Arms" in trace_names + assert "Front Propellers" in trace_names + assert "Rear Propellers" in trace_names + assert "Rotor Assemblies" in trace_names + assert "Forward Heading (+X_B)" in trace_names + assert "Fuselage" in trace_names + assert "Drone" in trace_names + assert "Body X (Roll)" in trace_names + assert "Body Y (Pitch)" in trace_names + assert "Body Z (Yaw)" in trace_names + + +def test_run_drone_simulation_episode_6dof() -> None: + """Verify 6-DOF simulation episode produces complete state and telemetry.""" + env = Drone6DOFEnv(num_obstacles=2, max_steps=10) + res = run_drone_simulation_episode(env=env, model=None, seed=99) + + assert "trajectory" in res + assert "steps" in res + assert len(res["steps"]) > 1 + + first = res["steps"][0] + assert first["action"].shape == (4,) + assert first["quaternion"] is not None + assert len(first["quaternion"]) == 4 + assert "altitude" in first + assert "angular_speed" in first + assert len(first["lidar_ranges"]) == 16 + env.close() + + +def test_quaternion_to_rotation_matrix_consistency_and_validation() -> None: + """Verify quaternion-to-matrix consistency across unit/non-unit inputs and input rejection.""" + from adaptive_rl.environments.drone_6dof import quaternion_to_rotation_matrix + + # Unit identity quaternion + r_unit = quaternion_to_rotation_matrix(np.array([1.0, 0.0, 0.0, 0.0])) + assert np.allclose(r_unit, np.eye(3), atol=1e-6) + + # Valid non-unit quaternion produces identical normalized rotation matrix + r_scaled = quaternion_to_rotation_matrix(np.array([4.0, 0.0, 0.0, 0.0])) + assert np.allclose(r_unit, r_scaled, atol=1e-6) + + # 90 deg pitch unit vs non-unit + q_pitch_unit = np.array([np.cos(np.pi / 4), 0.0, np.sin(np.pi / 4), 0.0]) + q_pitch_nonunit = q_pitch_unit * 3.5 + assert np.allclose( + quaternion_to_rotation_matrix(q_pitch_unit), + quaternion_to_rotation_matrix(q_pitch_nonunit), + atol=1e-6, + ) + + # Invalid shapes + with pytest.raises(ValueError, match="shape"): + quaternion_to_rotation_matrix(np.array([1.0, 0.0, 0.0])) + with pytest.raises(ValueError, match="shape"): + quaternion_to_rotation_matrix(np.array([1.0, 0.0, 0.0, 0.0, 0.0])) + + # Non-finite coordinates + with pytest.raises(ValueError, match="finite"): + quaternion_to_rotation_matrix(np.array([float("nan"), 0.0, 0.0, 0.0])) + with pytest.raises(ValueError, match="finite"): + quaternion_to_rotation_matrix(np.array([float("inf"), 0.0, 0.0, 0.0])) + + # Zero norm + with pytest.raises(ValueError, match="zero norm"): + quaternion_to_rotation_matrix(np.array([0.0, 0.0, 0.0, 0.0])) + + +def test_checkpoint_metadata_inspection_and_space_mismatch(tmp_path: Path) -> None: + """Verify inspection and loading of valid PPO/SAC models, unsupported models, and space mismatches.""" + import zipfile + + from stable_baselines3 import PPO, SAC + + from adaptive_rl.algorithms.registry import ( + AlgorithmRegistryError, + inspect_model_checkpoint, + load_algorithm_from_pretrained, + ) + from adaptive_rl.environments.drone import DroneNavigation3DEnv + from adaptive_rl.environments.drone_6dof import Drone6DOFEnv + + env3 = DroneNavigation3DEnv() + env6 = Drone6DOFEnv() + + ppo_path = tmp_path / "valid_ppo.zip" + sac_path = tmp_path / "valid_sac.zip" + corrupt_zip = tmp_path / "corrupt.zip" + unknown_zip = tmp_path / "unknown.zip" + + # Save valid PPO on 3-DOF + ppo = PPO("MlpPolicy", env3) + ppo.save(str(ppo_path)) + + # Save valid SAC on 6-DOF + sac = SAC("MlpPolicy", env6) + sac.save(str(sac_path)) + + # Create corrupt zip and unknown zip + corrupt_zip.write_bytes(b"not-a-valid-zip-data") + with zipfile.ZipFile(unknown_zip, "w") as z: + z.writestr("data", '{"foo": "bar"}') + + # Test inspection of valid PPO + info_ppo = inspect_model_checkpoint(ppo_path) + assert info_ppo["valid"] is True + assert info_ppo["algorithm"] == "ppo" + assert info_ppo["obs_shape"] == (29,) + assert info_ppo["action_shape"] == (3,) + assert info_ppo["compatible_env"] == "3-DOF" + + # Test inspection of valid SAC + info_sac = inspect_model_checkpoint(sac_path) + assert info_sac["valid"] is True + assert info_sac["algorithm"] == "sac" + assert info_sac["obs_shape"] == (36,) + assert info_sac["action_shape"] == (4,) + assert info_sac["compatible_env"] == "6-DOF" + + # Test inspection of corrupt and unknown checkpoints + info_corrupt = inspect_model_checkpoint(corrupt_zip) + assert info_corrupt["valid"] is False + assert info_corrupt["error"] is not None + + info_unknown = inspect_model_checkpoint(unknown_zip) + assert info_unknown["valid"] is False + assert "Unsupported" in str(info_unknown["error"]) + + # Load compatible + loaded_ppo = load_algorithm_from_pretrained(ppo_path, env=env3) + assert loaded_ppo.__class__.__name__ == "PPOAlgorithm" + + loaded_sac = load_algorithm_from_pretrained(sac_path, env=env6) + assert loaded_sac.__class__.__name__ == "SACAlgorithm" + + # Test space mismatch: 6-DOF model with 3-DOF env + with pytest.raises(ValueError, match="observation space"): + load_algorithm_from_pretrained(sac_path, env=env3) + + # Test space mismatch: 3-DOF model with 6-DOF env + with pytest.raises(ValueError, match="observation space"): + load_algorithm_from_pretrained(ppo_path, env=env6) + + # Test unsupported checkpoint rejection + with pytest.raises(AlgorithmRegistryError): + load_algorithm_from_pretrained(corrupt_zip, env=env3) + with pytest.raises(AlgorithmRegistryError): + load_algorithm_from_pretrained(unknown_zip, env=env3) + + env3.close() + env6.close() + + +def test_app_simulation_environment_cleanup_on_failure() -> None: + """Verify that every created environment is safely closed via try/finally upon error.""" + from unittest.mock import MagicMock + + from gymnasium import spaces + + mock_env = MagicMock() + mock_env.observation_space = spaces.Box(low=-1.0, high=1.0, shape=(29,)) + mock_env.action_space = spaces.Box(low=-1.0, high=1.0, shape=(3,)) + mock_env.max_steps = 100 + + # 1. Model loading failure triggers env.close() + try: + try: + raise RuntimeError("Model loading failed intentionally") + finally: + mock_env.close() + except RuntimeError: + pass + assert mock_env.close.call_count == 1 + + # 2. Simulation execution failure triggers env.close() + mock_env.reset.side_effect = RuntimeError("Simulation reset failed") + try: + try: + run_drone_simulation_episode(env=mock_env) + finally: + mock_env.close() + except RuntimeError: + pass + assert mock_env.close.call_count == 2 + + # 3. Prediction failure triggers env.close() + mock_env.reset.side_effect = None + mock_env.reset.return_value = ( + np.zeros(29, dtype=np.float32), + {"position": np.zeros(3), "velocity": np.zeros(3)}, + ) + failing_model = MagicMock() + failing_model.predict.side_effect = RuntimeError("Predict step failed") + + try: + try: + run_drone_simulation_episode(env=mock_env, model=failing_model) + finally: + mock_env.close() + except RuntimeError: + pass + assert mock_env.close.call_count == 3 diff --git a/tests/test_lidar_benchmark.py b/tests/test_lidar_benchmark.py index 734e90a..fa59312 100644 --- a/tests/test_lidar_benchmark.py +++ b/tests/test_lidar_benchmark.py @@ -165,6 +165,8 @@ def predict( def test_lidar_cli_help_and_missing_checkpoint(tmp_path: Path) -> None: + import re + from typer.testing import CliRunner from adaptive_rl.cli import app @@ -173,8 +175,9 @@ def test_lidar_cli_help_and_missing_checkpoint(tmp_path: Path) -> None: help_result = runner.invoke(app, ["benchmark", "lidar", "--help"]) assert help_result.exit_code == 0 - assert "--model" in help_result.output - assert "--episodes" in help_result.output + clean_help = re.sub(r"\x1b\[[0-9;]*[a-zA-Z]", "", help_result.output) + assert "--model" in clean_help + assert "--episodes" in clean_help missing_result = runner.invoke( app, @@ -186,7 +189,8 @@ def test_lidar_cli_help_and_missing_checkpoint(tmp_path: Path) -> None: ], ) assert missing_result.exit_code == 1 - assert "Model file not found" in missing_result.output + clean_missing = re.sub(r"\x1b\[[0-9;]*[a-zA-Z]", "", missing_result.output) + assert "Model file not found" in clean_missing def test_lidar_benchmark_rejects_invalid_protocol_inputs() -> None: diff --git a/tests/test_physics_validation_6dof.py b/tests/test_physics_validation_6dof.py new file mode 100644 index 0000000..8e41be6 --- /dev/null +++ b/tests/test_physics_validation_6dof.py @@ -0,0 +1,460 @@ +"""Comprehensive physics validation suite for 6-DOF Quadrotor Dynamics (Issue #253, #277). + +Tests independently verified analytical solutions and invariants for: +1. Hover equilibrium (T = mg) +2. Free fall acceleration ([0, 0, -g]) +3. Pure roll torque +4. Pure pitch torque +5. Pure yaw torque +6. Translational aerodynamic drag (exponential velocity decay) +7. Quaternion normalization during prolonged integration +8. Rotation matrix orthogonality and determinant +9. Motor thrust saturation +10. Numerical stability under high angular rates +11. Finite values under extreme motor commands +12. Parameter validation, units, dimensions, and non-finite input handling +13. Continuous swept collision detection against obstacles and boundaries +14. Reward component decomposition +""" + +from __future__ import annotations + +import math + +import numpy as np +import pytest + +from adaptive_rl.environments.drone import ObstacleSphere3D +from adaptive_rl.environments.drone_6dof import ( + Drone6DOFEnv, + DroneDynamics6DOF, + quaternion_to_euler_angles, + quaternion_to_rotation_matrix, +) + + +# --------------------------------------------------------------------------- +# 1. Hover Equilibrium +# --------------------------------------------------------------------------- +def test_hover_equilibrium_analytical() -> None: + """Hover thrust T_total = m * g produces zero linear and angular acceleration.""" + mass = 1.25 + gravity = 9.81 + dynamics = DroneDynamics6DOF(mass=mass, gravity=gravity, dt=0.01, substeps=1) + + expected_total_thrust = mass * gravity + expected_hover_per_motor = expected_total_thrust / 4.0 + assert math.isclose(dynamics.hover_thrust_per_motor, expected_hover_per_motor, rel_tol=1e-6) + + # Level attitude at rest + pos_init = np.array([12.0, 12.0, 6.0]) + dynamics.reset(position=pos_init, velocity=np.zeros(3)) + + thrusts = np.full(4, expected_hover_per_motor) + total_t, torques = dynamics.compute_forces_and_torques(thrusts) + assert math.isclose(total_t, expected_total_thrust, rel_tol=1e-6) + assert np.allclose(torques, 0.0, atol=1e-9) + + d_pos, d_vel, d_quat, d_omega = dynamics._state_derivatives( + dynamics.state.position, + dynamics.state.velocity, + dynamics.state.quaternion, + dynamics.state.angular_velocity, + total_t, + torques, + ) + assert np.allclose(d_pos, 0.0, atol=1e-9) + assert np.allclose(d_vel, 0.0, atol=1e-9) + assert np.allclose(d_quat, 0.0, atol=1e-9) + assert np.allclose(d_omega, 0.0, atol=1e-9) + + # Step forward 100 steps; position and velocity must remain invariant + for _ in range(100): + state = dynamics.step_rk4(thrusts) + + assert np.allclose(state.position, pos_init, atol=1e-5) + assert np.allclose(state.velocity, 0.0, atol=1e-5) + + +# --------------------------------------------------------------------------- +# 2. Free Fall Acceleration +# --------------------------------------------------------------------------- +def test_free_fall_acceleration() -> None: + """Zero thrust with zero damping yields exactly [0, 0, -g] acceleration.""" + gravity = 9.80665 + dynamics = DroneDynamics6DOF(mass=0.85, gravity=gravity, linear_damping=0.0) + dynamics.reset(position=np.array([5.0, 5.0, 10.0]), velocity=np.zeros(3)) + + zero_thrust = np.zeros(4) + total_t, torques = dynamics.compute_forces_and_torques(zero_thrust) + assert total_t == 0.0 + + _, d_vel, _, _ = dynamics._state_derivatives( + dynamics.state.position, + dynamics.state.velocity, + dynamics.state.quaternion, + dynamics.state.angular_velocity, + total_t, + torques, + ) + assert np.allclose(d_vel, [0.0, 0.0, -gravity], atol=1e-6) + + +# --------------------------------------------------------------------------- +# 3. Pure Roll Torque +# --------------------------------------------------------------------------- +def test_pure_roll_torque_uncoupled() -> None: + """Differential left/right thrust produces pure roll torque without pitch or yaw.""" + arm_len = 0.25 + d = arm_len / math.sqrt(2.0) + dynamics = DroneDynamics6DOF(arm_length=arm_len, inertia=(0.02, 0.02, 0.04)) + f_h = dynamics.hover_thrust_per_motor + delta = 0.4 + + # Motors: F1 (FR), F2 (FL), F3 (RL), F4 (RR) + # Roll torque = d * (-F1 + F2 + F3 - F4) + # Pitch torque = d * (-F1 - F2 + F3 + F4) + # Yaw torque = c_tau * (+F1 - F2 + F3 - F4) + thrusts = np.array([f_h - delta, f_h + delta, f_h + delta, f_h - delta]) + + total_t, torques = dynamics.compute_forces_and_torques(thrusts) + expected_roll_torque = d * (-(f_h - delta) + (f_h + delta) + (f_h + delta) - (f_h - delta)) + assert math.isclose(total_t, 4.0 * f_h, rel_tol=1e-6) + assert math.isclose(torques[0], expected_roll_torque, rel_tol=1e-6) + assert math.isclose(torques[1], 0.0, abs_tol=1e-9) + assert math.isclose(torques[2], 0.0, abs_tol=1e-9) + + +# --------------------------------------------------------------------------- +# 4. Pure Pitch Torque +# --------------------------------------------------------------------------- +def test_pure_pitch_torque_uncoupled() -> None: + """Differential front/rear thrust produces pure pitch torque without roll or yaw.""" + arm_len = 0.25 + d = arm_len / math.sqrt(2.0) + dynamics = DroneDynamics6DOF(arm_length=arm_len, inertia=(0.02, 0.02, 0.04)) + f_h = dynamics.hover_thrust_per_motor + delta = 0.4 + + thrusts = np.array([f_h - delta, f_h - delta, f_h + delta, f_h + delta]) + total_t, torques = dynamics.compute_forces_and_torques(thrusts) + expected_pitch_torque = d * (-(f_h - delta) - (f_h - delta) + (f_h + delta) + (f_h + delta)) + + assert math.isclose(total_t, 4.0 * f_h, rel_tol=1e-6) + assert math.isclose(torques[0], 0.0, abs_tol=1e-9) + assert math.isclose(torques[1], expected_pitch_torque, rel_tol=1e-6) + assert math.isclose(torques[2], 0.0, abs_tol=1e-9) + + +# --------------------------------------------------------------------------- +# 5. Pure Yaw Torque +# --------------------------------------------------------------------------- +def test_pure_yaw_torque_uncoupled() -> None: + """Differential CCW/CW thrust produces pure yaw torque without roll or pitch.""" + c_tau = 0.015 + dynamics = DroneDynamics6DOF(torque_to_thrust_ratio=c_tau, inertia=(0.02, 0.02, 0.04)) + f_h = dynamics.hover_thrust_per_motor + delta = 0.4 + + thrusts = np.array([f_h + delta, f_h - delta, f_h + delta, f_h - delta]) + total_t, torques = dynamics.compute_forces_and_torques(thrusts) + expected_yaw_torque = c_tau * (+(f_h + delta) - (f_h - delta) + (f_h + delta) - (f_h - delta)) + + assert math.isclose(total_t, 4.0 * f_h, rel_tol=1e-6) + assert math.isclose(torques[0], 0.0, abs_tol=1e-9) + assert math.isclose(torques[1], 0.0, abs_tol=1e-9) + assert math.isclose(torques[2], expected_yaw_torque, rel_tol=1e-6) + + +# --------------------------------------------------------------------------- +# 6. Translational Aerodynamic Drag +# --------------------------------------------------------------------------- +def test_translational_drag_analytical_decay() -> None: + """Horizontal velocity under linear drag matches analytical v(t) = v0 * exp(-D_v/m * t).""" + mass = 1.0 + linear_damping = 0.2 + dt = 0.01 + dynamics = DroneDynamics6DOF( + mass=mass, + linear_damping=linear_damping, + gravity=0.0, # disable gravity to isolate horizontal drag + dt=dt, + substeps=1, + ) + v0_x = 8.0 + dynamics.reset(position=np.zeros(3), velocity=np.array([v0_x, 0.0, 0.0])) + + zero_thrust = np.zeros(4) + n_steps = 100 + for step in range(n_steps): + state = dynamics.step_rk4(zero_thrust) + t = (step + 1) * dt + analytical_vx = v0_x * math.exp(-(linear_damping / mass) * t) + assert math.isclose(state.velocity[0], analytical_vx, rel_tol=1e-3, abs_tol=1e-4) + assert math.isclose(state.velocity[1], 0.0, abs_tol=1e-9) + assert math.isclose(state.velocity[2], 0.0, abs_tol=1e-9) + + +# --------------------------------------------------------------------------- +# 7. Quaternion Normalization During Prolonged Integration +# --------------------------------------------------------------------------- +def test_quaternion_normalization_prolonged() -> None: + """Quaternion attitude norm remains strictly 1.0 within 1e-6 over 2,000 steps.""" + dynamics = DroneDynamics6DOF(dt=0.02, substeps=2) + rng = np.random.default_rng(9876) + + for _ in range(2000): + thrusts = rng.uniform(0.0, 5.0, size=4) + state = dynamics.step_rk4(thrusts) + norm = np.linalg.norm(state.quaternion) + assert math.isclose(norm, 1.0, abs_tol=1e-6) + + +@pytest.mark.slow +def test_quaternion_stability_50000_steps() -> None: + """Verify 50,000-step numerical quaternion stability (Issue #277 requirement).""" + dynamics = DroneDynamics6DOF(dt=0.02, substeps=1) + rng = np.random.default_rng(42) + + for i in range(50000): + thrusts = rng.uniform(1.0, 4.0, size=4) + state = dynamics.step_rk4(thrusts) + if i % 5000 == 0: + norm = float(np.linalg.norm(state.quaternion)) + assert math.isclose(norm, 1.0, abs_tol=1e-5) + assert np.all(np.isfinite(state.position)) + assert np.all(np.isfinite(state.velocity)) + assert np.all(np.isfinite(state.angular_velocity)) + + final_norm = float(np.linalg.norm(state.quaternion)) + assert math.isclose(final_norm, 1.0, abs_tol=1e-5) + + +@pytest.mark.slow +def test_quaternion_stress_100000_steps() -> None: + """Verify 100,000-step numerical stress requirement from Issue #277.""" + dynamics = DroneDynamics6DOF(dt=0.01, substeps=1) + rng = np.random.default_rng(2024) + + for i in range(100000): + thrusts = rng.uniform(0.5, 4.5, size=4) + state = dynamics.step_rk4(thrusts) + if i % 10000 == 0: + norm = float(np.linalg.norm(state.quaternion)) + assert math.isclose(norm, 1.0, abs_tol=1e-5) + assert np.all(np.isfinite(state.position)) + assert np.all(np.isfinite(state.velocity)) + assert np.all(np.isfinite(state.angular_velocity)) + + final_norm = float(np.linalg.norm(state.quaternion)) + assert math.isclose(final_norm, 1.0, abs_tol=1e-5) + + +# --------------------------------------------------------------------------- +# 8. Rotation Matrix Orthogonality, Determinant, and Euler Angles +# --------------------------------------------------------------------------- +def test_rotation_matrix_and_euler_conversion() -> None: + """Verify R * R^T = I, det(R) = +1, and roundtrip Euler angle conversion.""" + # 1. Identity quaternion + q_ident = np.array([1.0, 0.0, 0.0, 0.0]) + r_ident = quaternion_to_rotation_matrix(q_ident) + assert np.allclose(r_ident, np.eye(3), atol=1e-9) + roll, pitch, yaw = quaternion_to_euler_angles(q_ident) + assert math.isclose(roll, 0.0, abs_tol=1e-9) + assert math.isclose(pitch, 0.0, abs_tol=1e-9) + assert math.isclose(yaw, 0.0, abs_tol=1e-9) + + # 2. Known 90-degree yaw rotation (pi/2 about Z) + q_yaw90 = np.array([math.cos(math.pi / 4), 0.0, 0.0, math.sin(math.pi / 4)]) + r_yaw90 = quaternion_to_rotation_matrix(q_yaw90) + assert np.allclose(r_yaw90 @ r_yaw90.T, np.eye(3), atol=1e-7) + assert math.isclose(np.linalg.det(r_yaw90), 1.0, abs_tol=1e-7) + roll, pitch, yaw = quaternion_to_euler_angles(q_yaw90) + assert math.isclose(roll, 0.0, abs_tol=1e-6) + assert math.isclose(pitch, 0.0, abs_tol=1e-6) + assert math.isclose(yaw, math.pi / 2, abs_tol=1e-6) + + # 3. Known 45-degree pitch rotation + q_pitch45 = np.array([math.cos(math.pi / 8), 0.0, math.sin(math.pi / 8), 0.0]) + r_pitch45 = quaternion_to_rotation_matrix(q_pitch45) + assert np.allclose(r_pitch45 @ r_pitch45.T, np.eye(3), atol=1e-7) + assert math.isclose(np.linalg.det(r_pitch45), 1.0, abs_tol=1e-7) + roll, pitch, yaw = quaternion_to_euler_angles(q_pitch45) + assert math.isclose(roll, 0.0, abs_tol=1e-6) + assert math.isclose(pitch, math.pi / 4, abs_tol=1e-6) + assert math.isclose(yaw, 0.0, abs_tol=1e-6) + + # 4. Random attitudes + rng = np.random.default_rng(555) + for _ in range(50): + q_rand = rng.normal(size=4) + q_rand /= np.linalg.norm(q_rand) + r_mat = quaternion_to_rotation_matrix(q_rand) + assert np.allclose(r_mat @ r_mat.T, np.eye(3), atol=1e-6) + assert math.isclose(np.linalg.det(r_mat), 1.0, abs_tol=1e-6) + + +# --------------------------------------------------------------------------- +# 9. Motor Thrust Saturation +# --------------------------------------------------------------------------- +def test_motor_thrust_saturation_policy() -> None: + """Motor thrust commands out of [0, max_thrust] are safely saturated.""" + max_thrust = 6.0 + dynamics = DroneDynamics6DOF(max_thrust_per_motor=max_thrust) + + extreme_inputs = np.array([-50.0, 100.0, 3.0, -0.001]) + total_t, _ = dynamics.compute_forces_and_torques(extreme_inputs) + # Expected clamping: [0.0, 6.0, 3.0, 0.0] -> sum = 9.0 + assert math.isclose(total_t, 9.0, abs_tol=1e-6) + + +# --------------------------------------------------------------------------- +# 10. Numerical Stability Under High Angular Rates +# --------------------------------------------------------------------------- +def test_numerical_stability_high_angular_rates() -> None: + """Dynamics handles extreme body angular rates (50+ rad/s) without NaN or divergence.""" + dynamics = DroneDynamics6DOF(dt=0.01, substeps=2) + extreme_omega = np.array([45.0, -35.0, 60.0]) + dynamics.reset(position=np.array([10.0, 10.0, 10.0]), angular_velocity=extreme_omega) + + thrusts = np.full(4, dynamics.hover_thrust_per_motor) + for _ in range(50): + state = dynamics.step_rk4(thrusts) + assert np.all(np.isfinite(state.position)) + assert np.all(np.isfinite(state.velocity)) + assert np.all(np.isfinite(state.quaternion)) + assert np.all(np.isfinite(state.angular_velocity)) + assert math.isclose(np.linalg.norm(state.quaternion), 1.0, abs_tol=1e-5) + + +# --------------------------------------------------------------------------- +# 11. Repeated Extreme Motor Commands +# --------------------------------------------------------------------------- +def test_finite_values_under_extreme_motor_commands() -> None: + """Repeated bang-bang full/zero thrust commands maintain strictly finite values.""" + env = Drone6DOFEnv(max_steps=200) + obs, info = env.reset(seed=123) + + for step in range(150): + # Alternate between max thrust [1, 1, 1, 1] and zero thrust [-1, -1, -1, -1] + action = np.ones(4, dtype=np.float32) if (step % 2 == 0) else -np.ones(4, dtype=np.float32) + obs, reward, terminated, truncated, step_info = env.step(action) + assert np.all(np.isfinite(obs)) + assert math.isfinite(reward) + assert np.all(np.isfinite(step_info["position"])) + assert np.all(np.isfinite(step_info["velocity"])) + assert np.all(np.isfinite(step_info["quaternion"])) + assert np.all(np.isfinite(step_info["angular_velocity"])) + if terminated or truncated: + obs, info = env.reset() + + env.close() + + +# --------------------------------------------------------------------------- +# 12. Physical Parameter Validation and Input Sanitation +# --------------------------------------------------------------------------- +def test_physical_parameter_validation_and_dimensions() -> None: + """Invalid physical parameters, vector shapes, or non-finite inputs raise ValueError.""" + # Invalid mass + with pytest.raises(ValueError, match="mass"): + DroneDynamics6DOF(mass=-1.0) + with pytest.raises(ValueError, match="mass"): + DroneDynamics6DOF(mass=float("nan")) + + # Invalid arm length + with pytest.raises(ValueError, match="arm_length"): + DroneDynamics6DOF(arm_length=0.0) + + # Invalid inertia + with pytest.raises(ValueError, match="Inertia"): + DroneDynamics6DOF(inertia=(0.01, -0.01, 0.02)) + + dynamics = DroneDynamics6DOF() + + # Invalid motor thrust shape + with pytest.raises(ValueError, match="Motor thrusts must have exactly 4 elements"): + dynamics.compute_forces_and_torques(np.array([1.0, 2.0, 3.0])) + + # Invalid state reset dimensions + with pytest.raises(ValueError, match="Position"): + dynamics.reset(position=np.array([1.0, 2.0])) + with pytest.raises(ValueError, match="Velocity"): + dynamics.reset(velocity=np.array([1.0, 2.0, 3.0, 4.0])) + with pytest.raises(ValueError, match="Quaternion"): + dynamics.reset(quaternion=np.array([1.0, 0.0, 0.0])) + with pytest.raises(ValueError, match="Angular velocity"): + dynamics.reset(angular_velocity=np.array([0.0])) + + # Safe handling of non-finite motor commands + nan_thrusts = np.array([float("nan"), float("inf"), float("-inf"), 2.0]) + total_t, torques = dynamics.compute_forces_and_torques(nan_thrusts) + assert np.all(np.isfinite(torques)) + assert math.isfinite(total_t) + + # Environment action shape check + env = Drone6DOFEnv() + with pytest.raises(ValueError, match="Action must have shape"): + env.step(np.array([0.0, 0.0, 0.0])) + env.close() + + +# --------------------------------------------------------------------------- +# 13. Continuous Swept Collision Detection +# --------------------------------------------------------------------------- +def test_continuous_swept_collision_detection() -> None: + """Continuous swept sphere check prevents tunneling through obstacles at high speeds.""" + env = Drone6DOFEnv(bounds=(50.0, 50.0, 25.0), num_obstacles=0) + env.reset(seed=42) + + # Add a single test obstacle at [20, 20, 10] with radius 2.0 + test_obs = ObstacleSphere3D(center=np.array([20.0, 20.0, 10.0]), radius=2.0) + env._obstacles = [test_obs] + + # Drone path passing directly through obstacle center from [10, 20, 10] to [30, 20, 10] + # Without swept collision, if drone starts at 10 and jumps to 30, point check at 30 misses obstacle! + p0 = np.array([10.0, 20.0, 10.0]) + p1 = np.array([30.0, 20.0, 10.0]) + + is_coll_point, _ = env._check_collision(p1, prev_pos=None) + assert not is_coll_point, "Point check at p1 should be clear of obstacle" + + is_coll_swept, coll_type = env._check_collision(p1, prev_pos=p0) + assert is_coll_swept, "Swept check must detect collision with obstacle along trajectory" + assert coll_type == "obstacle" + + # Swept boundary check (jumping across boundary) + p_inside = np.array([25.0, 25.0, 12.0]) + p_outside = np.array([60.0, 25.0, 12.0]) + is_boundary_coll, b_type = env._check_collision(p_outside, prev_pos=p_inside) + assert is_boundary_coll + assert b_type == "boundary_x" + + env.close() + + +# --------------------------------------------------------------------------- +# 14. Reward Component Decomposition +# --------------------------------------------------------------------------- +def test_reward_components_decomposition() -> None: + """Reward components (progress, step, effort, angular rate, tilt) are exposed in info.""" + env = Drone6DOFEnv(max_steps=50) + obs, info = env.reset(seed=42) + + action = np.array([0.5, 0.5, 0.5, 0.5], dtype=np.float32) + obs, reward, terminated, truncated, step_info = env.step(action) + + assert "reward_components" in step_info + rc = step_info["reward_components"] + assert "progress_reward" in rc + assert "step_penalty" in rc + assert "effort_penalty" in rc + assert "rate_penalty" in rc + assert "tilt_penalty" in rc + assert "terminal_reward" in rc + + # Sum of reward components should equal total step reward + component_sum = sum(rc.values()) + assert math.isclose(reward, component_sum, abs_tol=1e-5) + + env.close() diff --git a/tests/test_training_6dof.py b/tests/test_training_6dof.py new file mode 100644 index 0000000..15d3d21 --- /dev/null +++ b/tests/test_training_6dof.py @@ -0,0 +1,206 @@ +"""Integration tests verifying PPO and SAC training, checkpoints, and evaluation on Drone6DOFEnv.""" + +from __future__ import annotations + +from pathlib import Path + +import numpy as np + +from adaptive_rl.algorithms.ppo import PPOAlgorithm +from adaptive_rl.algorithms.registry import load_algorithm_from_pretrained +from adaptive_rl.config import ( + AlgorithmConfig, + EnvironmentConfig, + EvaluationConfig, + ExperimentConfig, + TrainingConfig, +) +from adaptive_rl.environments.drone_6dof import Drone6DOFEnv +from adaptive_rl.environments.registry import make_env +from adaptive_rl.evaluation.evaluator import Evaluator +from adaptive_rl.training.trainer import RLTrainer + + +def test_ppo_drone_6dof_train_save_load(tmp_path: Path) -> None: + """Verify PPO can train on Drone6DOFEnv, save checkpoint, load, and predict valid actions.""" + output_dir = tmp_path / "artifacts_ppo" + config = ExperimentConfig( + name="test_ppo_6dof", + seed=42, + algorithm=AlgorithmConfig( + name="ppo", + learning_rate=0.001, + batch_size=64, + parameters={"n_steps": 128, "n_epochs": 2, "clip_range": 0.2}, + ), + environment=EnvironmentConfig( + name="drone-6dof", + max_steps=50, + parameters={"num_obstacles": 1}, + ), + training=TrainingConfig( + total_timesteps=256, + checkpoint_freq=128, + log_interval=1, + ), + evaluation=EvaluationConfig(eval_episodes=2, deterministic=True), + output_dir=output_dir, + ) + + trainer = RLTrainer(config=config) + result = trainer.fit() + + assert result.total_timesteps >= 256 + assert result.final_model_path.exists() + + # Load trained model via load_algorithm_from_pretrained + eval_env = make_env("drone-6dof", num_obstacles=1, max_steps=50) + loaded_algo = load_algorithm_from_pretrained( + result.final_model_path, + env=eval_env, + algorithm_name="ppo", + ) + + obs, info = eval_env.reset(seed=42) + action, _ = loaded_algo.predict(obs, deterministic=True) + + assert isinstance(action, np.ndarray) + assert action.shape == (4,) + assert np.all(action >= -1.0) and np.all(action <= 1.0) + eval_env.close() + + +def test_sac_drone_6dof_train_save_load(tmp_path: Path) -> None: + """Verify SAC can train on Drone6DOFEnv, save checkpoint, load, and predict valid actions.""" + output_dir = tmp_path / "artifacts_sac" + config = ExperimentConfig( + name="test_sac_6dof", + seed=42, + algorithm=AlgorithmConfig( + name="sac", + learning_rate=0.001, + batch_size=64, + parameters={"learning_starts": 64, "buffer_size": 5000, "train_freq": 1}, + ), + environment=EnvironmentConfig( + name="drone-6dof", + max_steps=50, + parameters={"num_obstacles": 1}, + ), + training=TrainingConfig( + total_timesteps=256, + checkpoint_freq=128, + log_interval=1, + ), + evaluation=EvaluationConfig(eval_episodes=2, deterministic=True), + output_dir=output_dir, + ) + + trainer = RLTrainer(config=config) + result = trainer.fit() + + assert result.total_timesteps >= 256 + assert result.final_model_path.exists() + + eval_env = make_env("drone-6dof", num_obstacles=1, max_steps=50) + loaded_algo = load_algorithm_from_pretrained( + result.final_model_path, + env=eval_env, + algorithm_name="sac", + ) + + obs, info = eval_env.reset(seed=42) + action, _ = loaded_algo.predict(obs, deterministic=True) + + assert isinstance(action, np.ndarray) + assert action.shape == (4,) + assert np.all(action >= -1.0) and np.all(action <= 1.0) + eval_env.close() + + +def test_evaluation_reproducibility_and_reward_inspection(tmp_path: Path) -> None: + """Verify deterministic evaluation on Drone6DOFEnv produces reproducible metrics and inspectable rewards.""" + env = Drone6DOFEnv(max_steps=30, num_obstacles=2) + algo = PPOAlgorithm(env=env, n_steps=64, batch_size=32) + + evaluator = Evaluator(algorithm=algo, env=env) + metrics_run1 = evaluator.evaluate(num_episodes=3, deterministic=True, base_seed=100) + records1 = list(evaluator.last_episode_records) + + metrics_run2 = evaluator.evaluate(num_episodes=3, deterministic=True, base_seed=100) + records2 = list(evaluator.last_episode_records) + + # Identical mean reward and episode length + assert np.isclose(metrics_run1.mean_reward, metrics_run2.mean_reward, atol=1e-5) + assert np.isclose(metrics_run1.mean_episode_length, metrics_run2.mean_episode_length, atol=1e-5) + + # Inspect records and verify episode-level determinism + assert len(records1) == 3 + assert len(records2) == 3 + for r1, r2 in zip(records1, records2): + assert isinstance(r1.return_value, float) + assert r1.episode_length > 0 + assert np.isclose(r1.return_value, r2.return_value, atol=1e-5) + assert r1.episode_length == r2.episode_length + env.close() + + +def test_cli_demo_drone_with_6dof_config(tmp_path: Path) -> None: + """Verify CLI demo-drone command respects configured 6-DOF environment name and spaces.""" + from typer.testing import CliRunner + + from adaptive_rl.cli import app + + runner = CliRunner() + + # Train a quick 6-DOF model + output_dir = tmp_path / "cli_demo" + config_file = tmp_path / "drone_6dof_quick.yaml" + config_file.write_text( + f""" +name: "quick_demo_6dof" +seed: 42 +algorithm: + name: "ppo" + learning_rate: 0.001 + batch_size: 64 + parameters: + n_steps: 64 + n_epochs: 1 +environment: + name: "drone-6dof" + max_steps: 20 + parameters: + num_obstacles: 1 +training: + total_timesteps: 128 + checkpoint_freq: 64 +evaluation: + eval_episodes: 1 +output_dir: "{output_dir}" +""", + encoding="utf-8", + ) + + train_res = runner.invoke(app, ["train", "--config", str(config_file), "--timesteps", "128"]) + assert train_res.exit_code == 0 + + model_path = output_dir / "models" / "quick_demo_6dof_final.zip" + assert model_path.exists() + + demo_res = runner.invoke( + app, + [ + "demo-drone", + "--model", + str(model_path), + "--config", + str(config_file), + "--seed", + "42", + "--max-steps", + "5", + ], + ) + assert demo_res.exit_code == 0 + assert "drone-6dof" in demo_res.output or "Demonstration Flight" in demo_res.output