From c5cc9f9e056f6be66d998eb4cf30526ffa0d50a6 Mon Sep 17 00:00:00 2001 From: Aryan Saxena Date: Sat, 14 Nov 2026 18:27:10 +0530 Subject: [PATCH] feat(ui): redesign Streamlit ARL flight deck and quadrotor visualization --- app.py | 693 +++++++++++++++++++++++----- src/adaptive_rl/gui/visualizer.py | 731 ++++++++++++++++++++++-------- tests/test_gui.py | 141 ++++++ 3 files changed, 1263 insertions(+), 302 deletions(-) diff --git a/app.py b/app.py index 685296b..4d4cfe0 100644 --- a/app.py +++ b/app.py @@ -1,8 +1,8 @@ -"""AdaptiveRL: Autonomous 3D Drone Navigation Demonstration GUI. +"""AdaptiveRL: Autonomous 3D Drone Navigation Flight Deck GUI. -A Streamlit-based presentation interface for college professors and evaluators, -visualizing real-time 3D flight trajectories, LiDAR sensor readings, PPO training, -random baseline comparison, obstacle-density experiments, and system architecture. +A premium aerospace-grade mission-control Streamlit interface for evaluating +autonomous drone flight systems, rigid-body 6-DOF dynamics, 3-DOF kinematic simulation, +16-ray spherical LiDAR sensing, PPO training, and empirical policy benchmarking. """ from __future__ import annotations @@ -14,9 +14,7 @@ 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, @@ -42,20 +40,258 @@ # Configure Streamlit Page st.set_page_config( - page_title="AdaptiveRL โ€” 3D Drone Navigation Demo", - page_icon="๐Ÿš", + page_title="ARL | Autonomous Flight Systems โ€” 3D Flight Deck", + page_icon="๐Ÿ›ธ", layout="wide", initial_sidebar_state="expanded", ) +def inject_custom_aerospace_css() -> None: + """Inject premium dark aerospace mission-control CSS styling.""" + st.markdown( + """ + + """, + unsafe_allow_html=True, + ) + + def main() -> None: - # Sidebar: Project Overview - st.sidebar.title("๐Ÿš AdaptiveRL") + inject_custom_aerospace_css() + + # Sidebar: Project Overview & Identity + st.sidebar.markdown( + """ +
+
+ ๐Ÿ›ธ ARL FLIGHT SYSTEMS +
+
+ Autonomous Navigation Research Deck +
+
+ """, + unsafe_allow_html=True, + ) st.sidebar.markdown( - "**Autonomous 3D Drone Navigation**\n\n" - "Demonstration Platform for 3-DOF Kinematic and 6-DOF Rigid-Body Simulation using PPO & SAC.\n\n" - "*(Educational simulation for academic evaluation)*" + "**Aerospace Simulation Platform**\n\n" + "Unified evaluation deck for 6-DOF rigid-body dynamics (RK4 integration) and " + "3-DOF kinematic drone simulation with deep reinforcement learning (PPO & SAC).\n\n" + "---\n" + "**Key Capabilities**\n" + "- ๐Ÿ›ฐ๏ธ 3D Mesh Airframe Telemetry\n" + "- ๐Ÿ“ก 16-Ray Spherical LiDAR Array\n" + "- ๐Ÿ”„ Playback Numerical Scrubber\n" + "- ๐Ÿงช Obstacle-Density Benchmarks" ) available_models = get_available_models() @@ -64,7 +300,7 @@ def main() -> None: tabs = st.tabs( [ - "๐ŸŽฎ Live 3D Demo", + "๐ŸŽฎ Live 3D Flight Deck", "๐Ÿ“ˆ Train PPO", "๐Ÿ“Š Evaluation & Baseline", "๐ŸŽฏ Difficulty Experiment", @@ -73,84 +309,108 @@ def main() -> None: ) # ========================================== - # TAB 1: LIVE 3D DEMO + # TAB 1: LIVE 3D FLIGHT DECK # ========================================== with tabs[0]: - st.subheader("Interactive 3D Drone Simulation Playback") - st.caption( - "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." + # Professional ARL Mission Header + st.markdown( + """ +
+
+
+ ARL | AUTONOMOUS FLIGHT SYSTEMS +
+
+ HIGH-FIDELITY FLIGHT DYNAMICS & NEURAL NAVIGATION DECK +
+
+
+
โ— SYSTEM ACTIVE
+
SIMULATION PLAYBACK
+
16-RAY LIDAR READY
+
+
+ """, + unsafe_allow_html=True, ) - 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="โ„น๏ธ", + st.caption( + "Visualizing simulated quadrotor trajectory, rigid-body quaternion attitude, and spherical LiDAR " + "rangefinder readings. Recorded step playback from numerical physics integration (RK4/Euler)." ) - ctrl_col1, ctrl_col2, ctrl_col3, ctrl_col4, ctrl_col5 = st.columns([1.5, 2, 1, 1, 1]) + # Simulation Control Toolbar / Card + with st.container(): + ctrl_col1, ctrl_col2, ctrl_col3, ctrl_col4, ctrl_col5 = st.columns([1.5, 2.0, 1.0, 1.0, 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"] - 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}") + with ctrl_col1: + env_choice = st.selectbox( + "Dynamics Model:", + options=["6-DOF Quadrotor", "3-DOF Kinematic Drone"], + index=0, + help="6-DOF rigid-body dynamics (quaternion attitude & 4 rotor thrusts) or 3-DOF point-mass kinematics.", + ) - selected_policy = st.selectbox( - "Select Policy:", - options=policy_options, - index=0, - help="Choose a trained model checkpoint or test an untrained random baseline.", - ) + with ctrl_col2: + policy_options = ["Random Action Baseline"] + 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 / Checkpoint:", + options=policy_options, + index=0, + help="Choose a trained neural network checkpoint or test an untrained random baseline.", + ) - with ctrl_col3: - demo_seed = st.number_input( - "Environment Seed:", - min_value=0, - max_value=999999, - value=42, - step=1, - help="Locks procedural obstacle placement and target location for reproducible evaluation.", - ) + with ctrl_col3: + demo_seed = st.number_input( + "Environment Seed:", + min_value=0, + max_value=999999, + value=42, + step=1, + help="Locks procedural obstacle placement and target location for reproducible evaluation.", + ) - with ctrl_col4: - num_obs = st.slider( - "Obstacles:", - min_value=0, - max_value=8, - value=4, - step=1, - help="Number of procedural spherical obstacles generated in the arena.", - ) + with ctrl_col4: + num_obs = st.slider( + "Obstacle Count:", + min_value=0, + max_value=8, + value=4, + step=1, + help="Number of procedural spherical obstacles generated in the arena.", + ) - with ctrl_col5: - show_lidar_rays = st.checkbox("Show 16-Ray LiDAR", value=True) + with ctrl_col5: + show_lidar_rays = st.checkbox("Show 16-Ray LiDAR", value=True) - run_col1, _ = st.columns([1, 4]) - with run_col1: - launch_clicked = st.button( - "๐Ÿš€ Run Flight Simulation", type="primary", use_container_width=True - ) + run_col1, run_col2 = st.columns([1.5, 4.5]) + with run_col1: + launch_clicked = st.button( + "๐Ÿš€ Execute Flight Simulation", type="primary", use_container_width=True + ) + with run_col2: + if selected_policy.startswith("Random Action Baseline"): + st.markdown( + "
โš ๏ธ Untrained Baseline Note: Random Action Baseline provides " + "stochastic control inputs for baseline comparison; it does not represent a trained agent.
", + unsafe_allow_html=True, + ) is_6dof = "6-DOF" in env_choice sim_key = f"flight_data_{'6dof' if is_6dof else '3dof'}" if launch_clicked or sim_key not in st.session_state: - with st.spinner(f"Executing {env_choice} simulation dynamics..."): + with st.spinner(f"Executing {env_choice} numerical dynamics simulation..."): + env: Any = None if is_6dof: env = Drone6DOFEnv(num_obstacles=int(num_obs)) else: @@ -207,30 +467,60 @@ def main() -> None: steps = flight_data["steps"] total_steps = len(steps) - 1 - # Outcome Banner - outcome_color = "#38a169" if flight_data["outcome"] == "SUCCESS" else "#e53e3e" + # Flight Outcome Status Banner + outcome_status = flight_data["outcome"] + is_success = outcome_status == "SUCCESS" + banner_cls = "success" if is_success else "failure" + badge_cls = "arl-pill-green" if is_success else "arl-pill-red" + outcome_banner_text = "MISSION SUCCESSFUL" if is_success else "COLLISION / TIMEOUT" + + final_dist = steps[-1]["distance_to_goal"] st.markdown( f""" -
-

Flight Outcome: {flight_data["outcome"]} ({env_choice})

-

- Completed in {flight_data["total_steps"]} timesteps with cumulative reward {flight_data["total_reward"]:.2f} (Seed: {flight_data["seed"]}). -

+
+
+
+ โ— {outcome_banner_text} + {env_choice} + | + Policy: {selected_policy.split(': ')[0]} +
+
+ Episode finished in {flight_data["total_steps"]} timesteps with cumulative reward {flight_data["total_reward"]:.2f} (Seed: {flight_data["seed"]}). +
+
+
+ Final Goal Dist: {final_dist:.2f} m
+ Physics Mode: {'RK4 6-DOF' if is_6dof else 'Kinematic 3-DOF'} +
""", unsafe_allow_html=True, ) - # Scrubber Slider for Trajectory Inspection - scrub_col, _ = st.columns([3, 1]) - with scrub_col: + # Scrubber Slider for Trajectory Playback + scrub_col1, scrub_col2 = st.columns([3.5, 1.0]) + with scrub_col1: selected_step = st.slider( - "Flight Step Playback:", + "Flight Step Playback Scrubber:", min_value=0, max_value=total_steps, value=total_steps, step=1, - help="Scrub through recorded flight time to inspect 3D drone attitude, position, speed, and sensor range.", + help="Scrub through recorded flight timesteps to inspect 3D drone attitude, position, speed, and sensor range.", + ) + with scrub_col2: + step_pct = (selected_step / max(1, total_steps)) * 100.0 + dt_step = 0.05 + sim_time_sec = selected_step * dt_step + st.markdown( + f""" +
+ Step: {selected_step} / {total_steps} ({step_pct:.0f}%)
+ Elapsed: {sim_time_sec:.2f}s +
+ """, + unsafe_allow_html=True, ) step_info = steps[selected_step] @@ -239,25 +529,7 @@ def main() -> None: cur_lidar = step_info["lidar_ranges"] cur_quat = step_info.get("quaternion") - # Build Interactive 3D Plotly Arena - fig = build_arena_3d_figure( - bounds=flight_data["bounds"], - obstacles=flight_data["obstacles"], - target=flight_data["target"], - start_pos=flight_data["start_pos"], - 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 ({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) - - # Telemetry Metrics Rows + # Telemetry calculations 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) @@ -267,23 +539,196 @@ def main() -> None: roll_deg, pitch_deg, yaw_deg = 0.0, 0.0, 0.0 omega_mag = step_info.get("angular_speed", 0.0) + ang_vel = step_info.get("angular_velocity") + if ang_vel is not None and len(ang_vel) == 3: + p_val, q_val, r_val = ang_vel + else: + p_val, q_val, r_val = 0.0, 0.0, 0.0 + + # Two-Column Master View: 3D Viewport on Left (dominant), Telemetry Console on Right + col_viewport, col_telemetry = st.columns([2.5, 1.0]) + + with col_viewport: + # Build Interactive 3D Plotly Arena + fig = build_arena_3d_figure( + bounds=flight_data["bounds"], + obstacles=flight_data["obstacles"], + target=flight_data["target"], + start_pos=flight_data["start_pos"], + 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 ({env_choice}) โ€” Step {selected_step}/{total_steps} (Distance to Goal: {step_info['distance_to_goal']:.2f}m)", + ) + # Give viewport substantial height for dominance + fig.update_layout(height=660) + st.plotly_chart(fig, use_container_width=True) + + with col_telemetry: + # 1. NAVIGATION TELEMETRY + rel_target = flight_data["target"] - cur_pos + st.markdown( + f""" +
+
๐Ÿ“ Navigation & Position
+
+
+ Altitude (Z) + {cur_pos[2]:.2f}m +
+
+ Range to Goal + {step_info['distance_to_goal']:.2f}m +
+
+ Nearest Obstacle + {step_info['min_obstacle_distance']:.2f}m +
+
+ Rel Target [X,Y] + [{rel_target[0]:.1f}, {rel_target[1]:.1f}] +
+
+
+ """, + unsafe_allow_html=True, + ) - # 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}") + # 2. FLIGHT DYNAMICS + st.markdown( + f""" +
+
โšก Flight Dynamics
+
+
+ Linear Speed + {step_info['speed']:.2f}m/s +
+
+ Angular Speed + {omega_mag:.2f}rad/s +
+
+ Velocity [Vx, Vy] + [{cur_vel[0]:.1f}, {cur_vel[1]:.1f}] +
+
+ Climb Rate (Vz) + {cur_vel[2]:+.2f}m/s +
+
+
+ """, + unsafe_allow_html=True, + ) + + # 3. ATTITUDE & ORIENTATION (6-DOF) / KINEMATICS (3-DOF) + if is_6dof and cur_quat is not None: + st.markdown( + f""" +
+
๐Ÿงญ Attitude & Orientation (6-DOF)
+
+
+ Roll (ฯ†) + {roll_deg:+.1f}ยฐ +
+
+ Pitch (ฮธ) + {pitch_deg:+.1f}ยฐ +
+
+ Yaw (ฯˆ) + {yaw_deg:+.1f}ยฐ +
+
+ Body Rates [p,q,r] + [{p_val:.1f}, {q_val:.1f}, {r_val:.1f}] +
+
+
+ """, + unsafe_allow_html=True, + ) + else: + st.markdown( + """ +
+
๐Ÿงญ Kinematic State (3-DOF)
+
+ Point-Mass Kinematics: Continuous 3D acceleration control. Rotational torque and attitude dynamics are not simulated in 3-DOF mode. +
+
+ """, + unsafe_allow_html=True, + ) + + # 4. SENSORS & LIDAR + min_lidar = float(np.min(cur_lidar)) if len(cur_lidar) > 0 else 0.0 + st.markdown( + f""" +
+
๐Ÿ“ก Sensors & Perception
+
+
+ LiDAR Rays + 16 Beams +
+
+ Closest Return + {min_lidar:.2f}m +
+
+
+ """, + unsafe_allow_html=True, + ) + + # 5. REWARDS & PERFORMANCE + st.markdown( + f""" +
+
๐ŸŽฏ Performance & Rewards
+
+
+ Step Reward + {step_info['reward']:+.2f} +
+
+ Cumulative Return + {step_info['cumulative_reward']:+.2f} +
+
+
+ """, + unsafe_allow_html=True, + ) + + # Expandable Simulation Diagnostics Section + with st.expander("๐Ÿ” Simulation Diagnostics & Physics Specifications"): + diag_col1, diag_col2 = st.columns(2) + with diag_col1: + st.markdown( + """ + **Dynamics Engine Specifications** + - **6-DOF Quadrotor Dynamics**: Runge-Kutta 4th Order (RK4) integration with 5 substeps ($\\Delta t_{\\text{sub}} = 0.01\\text{ s}$). + - **State Representation**: 36-dimensional state vector with position, linear velocity, unit quaternion attitude $\\mathbf{q} = [q_w, q_x, q_y, q_z]$, and body angular velocity $\\boldsymbol{\\omega} = [p, q, r]$. + - **Control**: Continuous 4-motor thrust commands mapped to physical body torques and collective thrust. + """ + ) + with diag_col2: + st.markdown( + """ + **Sensor & Obstacle Parameters** + - **Spherical LiDAR**: 16 analytical raycast rangefinders with geometric ray-sphere and bounding box intersection calculations. + - **Collision Detection**: Continuous sphere-to-sphere collision checking with obstacle clearance radii. + - **Playback Notice**: Recorded trajectory playback from deterministic numerical evaluation episodes. + """ + ) # ========================================== # TAB 2: TRAIN PPO @@ -319,7 +764,7 @@ def main() -> None: st.session_state["is_training"] = True st.info(f"Starting training run ({train_steps:,} timesteps on CPU)...") prog_bar = st.progress(0.0) - status_text = st.empty() + train_status_box: Any = st.empty() try: train_cfg = ExperimentConfig( @@ -344,7 +789,7 @@ def main() -> None: duration = time.time() - t0 prog_bar.progress(1.0) - status_text.text(f"Training Complete! Duration: {duration:.2f}s") + train_status_box.text(f"Training Complete! Duration: {duration:.2f}s") st.success(f"Checkpoint saved: `{result.final_model_path}`") st.session_state["latest_training_rewards"] = result.episode_rewards diff --git a/src/adaptive_rl/gui/visualizer.py b/src/adaptive_rl/gui/visualizer.py index 941057f..3297d78 100644 --- a/src/adaptive_rl/gui/visualizer.py +++ b/src/adaptive_rl/gui/visualizer.py @@ -1,4 +1,10 @@ -"""Plotly-based 3D visual arena and sensor visualizer for AdaptiveRL.""" +"""Plotly-based 3D visual arena and sensor visualizer for AdaptiveRL. + +Provides premium aerospace-grade 3D mesh rendering of autonomous quadrotors, +spherical obstacle safety fields, LiDAR sensor rays, and mission-control charts. +""" + +from __future__ import annotations import math from pathlib import Path @@ -14,6 +20,287 @@ ) +def _combine_meshes( + mesh_list: Sequence[Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray]], +) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray]: + """Combine multiple (x, y, z, i, j, k) mesh tuples into a single unified mesh.""" + xs, ys, zs = [], [], [] + is_, js_, ks_ = [], [], [] + offset = 0 + for x, y, z, i, j, k in mesh_list: + if len(x) == 0: + continue + xs.append(x) + ys.append(y) + zs.append(z) + is_.append(i + offset) + js_.append(j + offset) + ks_.append(k + offset) + offset += len(x) + if not xs: + empty = np.array([], dtype=np.float64) + empty_int = np.array([], dtype=np.int32) + return empty, empty, empty, empty_int, empty_int, empty_int + return ( + np.concatenate(xs), + np.concatenate(ys), + np.concatenate(zs), + np.concatenate(is_), + np.concatenate(js_), + np.concatenate(ks_), + ) + + +def _cylinder_mesh_body( + p_start: np.ndarray, + p_end: np.ndarray, + radius: float, + n_seg: int = 10, +) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray]: + """Generate 3D cylinder mesh between p_start and p_end in body coordinates.""" + v = p_end - p_start + length = float(np.linalg.norm(v)) + if length < 1e-6: + empty = np.array([], dtype=np.float64) + empty_int = np.array([], dtype=np.int32) + return empty, empty, empty, empty_int, empty_int, empty_int + v_n = v / length + if abs(v_n[0]) < 0.8: + arb = np.array([1.0, 0.0, 0.0], dtype=np.float64) + else: + arb = np.array([0.0, 1.0, 0.0], dtype=np.float64) + u1 = np.cross(v_n, arb) + u1 = u1 / np.linalg.norm(u1) + u2 = np.cross(v_n, u1) + + theta = np.linspace(0, 2 * np.pi, n_seg, endpoint=False) + pts_s = [p_start + radius * (math.cos(th) * u1 + math.sin(th) * u2) for th in theta] + pts_e = [p_end + radius * (math.cos(th) * u1 + math.sin(th) * u2) for th in theta] + verts = np.array(pts_s + pts_e + [p_start, p_end], dtype=np.float64) + + i_l, j_l, k_l = [], [], [] + c_s = 2 * n_seg + c_e = 2 * n_seg + 1 + for s in range(n_seg): + s_nxt = (s + 1) % n_seg + i_l.extend([s, s]) + j_l.extend([s_nxt, n_seg + s_nxt]) + k_l.extend([n_seg + s_nxt, n_seg + s]) + i_l.append(c_s) + j_l.append(s_nxt) + k_l.append(s) + i_l.append(c_e) + j_l.append(n_seg + s) + k_l.append(n_seg + s_nxt) + + return ( + verts[:, 0], + verts[:, 1], + verts[:, 2], + np.array(i_l, dtype=np.int32), + np.array(j_l, dtype=np.int32), + np.array(k_l, dtype=np.int32), + ) + + +def _sphere_mesh_body( + center: np.ndarray, + radius: float, + n_lat: int = 8, + n_lon: int = 12, +) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray]: + """Generate 3D sphere mesh in body coordinates.""" + verts = [] + verts.append(center + np.array([0.0, 0.0, radius])) + for lat in range(1, n_lat): + phi = math.pi * lat / n_lat + z = radius * math.cos(phi) + r_xy = radius * math.sin(phi) + for lon in range(n_lon): + theta = 2 * math.pi * lon / n_lon + x = r_xy * math.cos(theta) + y = r_xy * math.sin(theta) + verts.append(center + np.array([x, y, z])) + verts.append(center + np.array([0.0, 0.0, -radius])) + verts_arr = np.array(verts, dtype=np.float64) + + i_l, j_l, k_l = [], [], [] + for lon in range(n_lon): + nxt = (lon + 1) % n_lon + i_l.append(0) + j_l.append(1 + lon) + k_l.append(1 + nxt) + + for lat in range(n_lat - 2): + row1 = 1 + lat * n_lon + row2 = 1 + (lat + 1) * n_lon + for lon in range(n_lon): + nxt = (lon + 1) % n_lon + i_l.extend([row1 + lon, row1 + lon]) + j_l.extend([row1 + nxt, row2 + nxt]) + k_l.extend([row2 + nxt, row2 + lon]) + + south_idx = len(verts) - 1 + last_row = 1 + (n_lat - 2) * n_lon + for lon in range(n_lon): + nxt = (lon + 1) % n_lon + i_l.append(south_idx) + j_l.append(last_row + nxt) + k_l.append(last_row + lon) + + return ( + verts_arr[:, 0], + verts_arr[:, 1], + verts_arr[:, 2], + np.array(i_l, dtype=np.int32), + np.array(j_l, dtype=np.int32), + np.array(k_l, dtype=np.int32), + ) + + +def _blade_mesh_body( + rotor_center: np.ndarray, + blade_length: float, + chord: float, + angle_rad: float, +) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray]: + """Generate 2-blade aerodynamic propeller mesh in body coordinates.""" + cos_a = math.cos(angle_rad) + sin_a = math.sin(angle_rad) + dir_v = np.array([cos_a, sin_a, 0.0], dtype=np.float64) + nor_v = np.array([-sin_a, cos_a, 0.0], dtype=np.float64) + + z = rotor_center[2] + 0.04 + p0 = rotor_center + dir_v * 0.04 - nor_v * (chord * 0.5) + p1 = rotor_center + dir_v * 0.04 + nor_v * (chord * 0.5) + p2 = rotor_center + dir_v * blade_length + nor_v * (chord * 0.28) + p3 = rotor_center + dir_v * blade_length - nor_v * (chord * 0.28) + + p4 = rotor_center - dir_v * 0.04 - nor_v * (chord * 0.5) + p5 = rotor_center - dir_v * 0.04 + nor_v * (chord * 0.5) + p6 = rotor_center - dir_v * blade_length + nor_v * (chord * 0.28) + p7 = rotor_center - dir_v * blade_length - nor_v * (chord * 0.28) + + verts = np.array([ + [p0[0], p0[1], z], + [p1[0], p1[1], z], + [p2[0], p2[1], z + 0.01], + [p3[0], p3[1], z + 0.01], + [p4[0], p4[1], z], + [p5[0], p5[1], z], + [p6[0], p6[1], z - 0.01], + [p7[0], p7[1], z - 0.01], + ], dtype=np.float64) + + i = np.array([0, 0, 4, 4], dtype=np.int32) + j = np.array([1, 2, 5, 6], dtype=np.int32) + k = np.array([2, 3, 6, 7], dtype=np.int32) + return verts[:, 0], verts[:, 1], verts[:, 2], i, j, k + + +def _fuselage_mesh_body(d: float) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray]: + """Generate sleek 3D faceted aerospace fuselage pod in body coordinates.""" + top_pts = [ + [0.38 * d, 0.0, 0.02 * d], # 0: nose + [0.22 * d, 0.18 * d, 0.05 * d], # 1: front-left + [0.0, 0.22 * d, 0.05 * d], # 2: mid-left + [-0.22 * d, 0.16 * d, 0.04 * d], # 3: rear-left + [-0.34 * d, 0.0, 0.02 * d], # 4: tail + [-0.22 * d, -0.16 * d, 0.04 * d], # 5: rear-right + [0.0, -0.22 * d, 0.05 * d], # 6: mid-right + [0.22 * d, -0.18 * d, 0.05 * d], # 7: front-right + ] + bot_pts = [ + [0.34 * d, 0.0, -0.03 * d], + [0.20 * d, 0.16 * d, -0.06 * d], + [0.0, 0.20 * d, -0.06 * d], + [-0.20 * d, 0.14 * d, -0.05 * d], + [-0.30 * d, 0.0, -0.03 * d], + [-0.20 * d, -0.14 * d, -0.05 * d], + [0.0, -0.20 * d, -0.06 * d], + [0.20 * d, -0.16 * d, -0.06 * d], + ] + crest = [0.05 * d, 0.0, 0.09 * d] + keel = [0.0, 0.0, -0.08 * d] + + verts = np.array(top_pts + bot_pts + [crest, keel], dtype=np.float64) + c_idx = 16 + k_idx = 17 + + i_l, j_l, k_l = [], [], [] + for s in range(8): + nxt = (s + 1) % 8 + i_l.append(c_idx) + j_l.append(s) + k_l.append(nxt) + + for s in range(8): + nxt = (s + 1) % 8 + i_l.append(k_idx) + j_l.append(8 + nxt) + k_l.append(8 + s) + + for s in range(8): + nxt = (s + 1) % 8 + top_s, top_nxt = s, nxt + bot_s, bot_nxt = 8 + s, 8 + nxt + i_l.extend([top_s, top_s]) + j_l.extend([bot_s, top_nxt]) + k_l.extend([bot_nxt, bot_nxt]) + + return ( + verts[:, 0], + verts[:, 1], + verts[:, 2], + np.array(i_l, dtype=np.int32), + np.array(j_l, dtype=np.int32), + np.array(k_l, dtype=np.int32), + ) + + +def _chevron_mesh_body(d: float) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray]: + """Generate forward heading indicator chevron in body coordinates.""" + z0 = 0.03 * d + z1 = 0.06 * d + pts_2d = [ + [0.65 * d, 0.0], + [0.44 * d, 0.12 * d], + [0.50 * d, 0.0], + [0.44 * d, -0.12 * d], + ] + verts_lower = [[p[0], p[1], z0] for p in pts_2d] + verts_upper = [[p[0], p[1], z1] for p in pts_2d] + verts = np.array(verts_lower + verts_upper, dtype=np.float64) + + i = np.array([4, 4, 0, 0, 0, 0, 1, 1, 2, 2, 3, 3], dtype=np.int32) + j = np.array([5, 6, 2, 3, 1, 5, 2, 6, 3, 7, 0, 4], dtype=np.int32) + k = np.array([6, 7, 1, 2, 5, 4, 6, 5, 7, 6, 4, 7], dtype=np.int32) + return verts[:, 0], verts[:, 1], verts[:, 2], i, j, k + + +def _transform_mesh( + mesh_b: Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray], + center_pos: np.ndarray, + rot_mat: np.ndarray, +) -> Dict[str, np.ndarray]: + """Transform body mesh (xb, yb, zb, i, j, k) to world coordinates.""" + xb, yb, zb, i, j, k = mesh_b + if len(xb) == 0: + empty = np.array([], dtype=np.float64) + empty_int = np.array([], dtype=np.int32) + return {"x": empty, "y": empty, "z": empty, "i": empty_int, "j": empty_int, "k": empty_int} + v_b = np.column_stack([xb, yb, zb]) + v_w = center_pos + (rot_mat @ v_b.T).T + return { + "x": v_w[:, 0], + "y": v_w[:, 1], + "z": v_w[:, 2], + "i": i, + "j": j, + "k": k, + } + + def compute_quadrotor_3d_geometry( center_pos: np.ndarray | Sequence[float], quaternion: Optional[np.ndarray | Sequence[float]] = None, @@ -24,11 +311,13 @@ def compute_quadrotor_3d_geometry( """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]) + - Central body fuselage with 3D volume pod and forward heading nose (+X_B direction) + - 4 volumetric X-configuration arms with structural motor housings - 4 rotor hubs (R1: Front-Right, R2: Front-Left, R3: Rear-Left, R4: Rear-Right) - - 4 circular propeller blade paths + - 4 recognizable 3D propellers with front/rear color differentiation + - Dual port/starboard landing gear skids and angled structural struts + - Forward-facing optical sensor gimbal and camera assembly + - Illuminated 3D navigation chevron indicating heading (+X_B) Transformation: P_W = center_pos + R(q) @ P_B @@ -58,9 +347,9 @@ def compute_quadrotor_3d_geometry( 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), # 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 + np.array([-d, -d, 0.0], dtype=np.float64), # R4 Rear-Right ] # Transform rotors to world coordinates @@ -94,6 +383,90 @@ def compute_quadrotor_3d_geometry( ring_pts_w = np.array([c_pos + rot_mat @ pt for pt in ring_pts_b]) propellers_w.append(ring_pts_w) + # ========================================== + # DETAILED 3D MESH GEOMETRY COMPONENTS + # ========================================== + # 1. 3D Fuselage Pod + fuse_mesh_b = _fuselage_mesh_body(d) + mesh_fuselage = _transform_mesh(fuse_mesh_b, c_pos, rot_mat) + + # 2. Four Volumetric 3D Arms (Front Arms and Rear Arms) + arm_front_cylinders = [ + _cylinder_mesh_body(np.array([0.0, 0.0, 0.0]), body_rotors[0], 0.04 * d), + _cylinder_mesh_body(np.array([0.0, 0.0, 0.0]), body_rotors[1], 0.04 * d), + ] + arm_rear_cylinders = [ + _cylinder_mesh_body(np.array([0.0, 0.0, 0.0]), body_rotors[2], 0.04 * d), + _cylinder_mesh_body(np.array([0.0, 0.0, 0.0]), body_rotors[3], 0.04 * d), + ] + mesh_front_arms = _transform_mesh(_combine_meshes(arm_front_cylinders), c_pos, rot_mat) + mesh_rear_arms = _transform_mesh(_combine_meshes(arm_rear_cylinders), c_pos, rot_mat) + mesh_arms = _transform_mesh(_combine_meshes(arm_front_cylinders + arm_rear_cylinders), c_pos, rot_mat) + + # 3. Four Motor Housings + motors_front = [ + _cylinder_mesh_body(body_rotors[0] + np.array([0, 0, -0.07 * d]), body_rotors[0] + np.array([0, 0, 0.03 * d]), 0.15 * d), + _cylinder_mesh_body(body_rotors[1] + np.array([0, 0, -0.07 * d]), body_rotors[1] + np.array([0, 0, 0.03 * d]), 0.15 * d), + ] + motors_rear = [ + _cylinder_mesh_body(body_rotors[2] + np.array([0, 0, -0.07 * d]), body_rotors[2] + np.array([0, 0, 0.03 * d]), 0.15 * d), + _cylinder_mesh_body(body_rotors[3] + np.array([0, 0, -0.07 * d]), body_rotors[3] + np.array([0, 0, 0.03 * d]), 0.15 * d), + ] + mesh_motors_front = _transform_mesh(_combine_meshes(motors_front), c_pos, rot_mat) + mesh_motors_rear = _transform_mesh(_combine_meshes(motors_rear), c_pos, rot_mat) + + # 4. Four Rotor Hub Spinners and Assembly + hubs = [ + _cylinder_mesh_body(r + np.array([0, 0, 0.03 * d]), r + np.array([0, 0, 0.08 * d]), 0.07 * d) + for r in body_rotors + ] + mesh_rotor_hubs = _transform_mesh(_combine_meshes(hubs), c_pos, rot_mat) + mesh_rotor_assemblies = _transform_mesh(_combine_meshes(motors_front + motors_rear + hubs), c_pos, rot_mat) + + # 5. Four Recognizable Propeller Blades + blade_len = float(rotor_radius) + blade_chord = max(0.05 * d, 0.05) + props_front = [ + _blade_mesh_body(body_rotors[0], blade_len, blade_chord, 0.35), + _blade_mesh_body(body_rotors[1], blade_len, blade_chord, -0.35), + ] + props_rear = [ + _blade_mesh_body(body_rotors[2], blade_len, blade_chord, 0.35), + _blade_mesh_body(body_rotors[3], blade_len, blade_chord, -0.35), + ] + mesh_props_front = _transform_mesh(_combine_meshes(props_front), c_pos, rot_mat) + mesh_props_rear = _transform_mesh(_combine_meshes(props_rear), c_pos, rot_mat) + + # 6. Landing Gear (Dual Skids and 4 Angled Struts) + skid_port = [ + _cylinder_mesh_body(np.array([-0.36 * d, 0.22 * d, -0.16 * d]), np.array([0.36 * d, 0.22 * d, -0.16 * d]), 0.02 * d), + _cylinder_mesh_body(np.array([0.36 * d, 0.22 * d, -0.16 * d]), np.array([0.44 * d, 0.22 * d, -0.08 * d]), 0.02 * d), + _cylinder_mesh_body(np.array([-0.36 * d, 0.22 * d, -0.16 * d]), np.array([-0.44 * d, 0.22 * d, -0.08 * d]), 0.02 * d), + ] + skid_stbd = [ + _cylinder_mesh_body(np.array([-0.36 * d, -0.22 * d, -0.16 * d]), np.array([0.36 * d, -0.22 * d, -0.16 * d]), 0.02 * d), + _cylinder_mesh_body(np.array([0.36 * d, -0.22 * d, -0.16 * d]), np.array([0.44 * d, -0.22 * d, -0.08 * d]), 0.02 * d), + _cylinder_mesh_body(np.array([-0.36 * d, -0.22 * d, -0.16 * d]), np.array([-0.44 * d, -0.22 * d, -0.08 * d]), 0.02 * d), + ] + struts = [ + _cylinder_mesh_body(np.array([0.16 * d, 0.14 * d, 0.0]), np.array([0.18 * d, 0.22 * d, -0.16 * d]), 0.018 * d), + _cylinder_mesh_body(np.array([-0.16 * d, 0.14 * d, 0.0]), np.array([-0.18 * d, 0.22 * d, -0.16 * d]), 0.018 * d), + _cylinder_mesh_body(np.array([0.16 * d, -0.14 * d, 0.0]), np.array([0.18 * d, -0.22 * d, -0.16 * d]), 0.018 * d), + _cylinder_mesh_body(np.array([-0.16 * d, -0.14 * d, 0.0]), np.array([-0.18 * d, -0.22 * d, -0.16 * d]), 0.018 * d), + ] + mesh_landing_gear = _transform_mesh(_combine_meshes(skid_port + skid_stbd + struts), c_pos, rot_mat) + + # 7. Forward Camera & Sensor Gimbal Assembly + camera_parts = [ + _cylinder_mesh_body(np.array([0.32 * d, 0.0, 0.0]), np.array([0.40 * d, 0.0, -0.03 * d]), 0.025 * d), + _sphere_mesh_body(np.array([0.42 * d, 0.0, -0.04 * d]), 0.06 * d), + _cylinder_mesh_body(np.array([0.42 * d, 0.0, -0.04 * d]), np.array([0.48 * d, 0.0, -0.04 * d]), 0.035 * d), + ] + mesh_camera = _transform_mesh(_combine_meshes(camera_parts), c_pos, rot_mat) + + # 8. Forward Heading Indicator Chevron (+X_B) + mesh_heading = _transform_mesh(_chevron_mesh_body(d), c_pos, rot_mat) + return { "center": c_pos, "rotors": world_rotors, @@ -102,6 +475,21 @@ def compute_quadrotor_3d_geometry( "propellers": propellers_w, "front_arms": (world_rotors[0], c_pos, world_rotors[1]), "rear_arms": (world_rotors[2], c_pos, world_rotors[3]), + "mesh_fuselage": mesh_fuselage, + "mesh_arms": mesh_arms, + "mesh_front_arms": mesh_front_arms, + "mesh_rear_arms": mesh_rear_arms, + "mesh_motors_front": mesh_motors_front, + "mesh_motors_rear": mesh_motors_rear, + "mesh_rotor_hubs": mesh_rotor_hubs, + "mesh_rotor_assemblies": mesh_rotor_assemblies, + "mesh_props_front": mesh_props_front, + "mesh_props_rear": mesh_props_rear, + "mesh_landing_gear": mesh_landing_gear, + "mesh_camera": mesh_camera, + "mesh_heading_indicator": mesh_heading, + "rot_mat": rot_mat, + "body_rotors": body_rotors, } @@ -124,9 +512,6 @@ def _generate_box_wireframe( ) -> Tuple[List[Optional[float]], List[Optional[float]], List[Optional[float]]]: """Generate line coordinates for the 12 edges of the 3D arena boundary box.""" bx, by, bz = bounds - # Define 8 corners - # 0: 0,0,0; 1: bx,0,0; 2: bx,by,0; 3: 0,by,0 - # 4: 0,0,bz; 5: bx,0,bz; 6: bx,by,bz; 7: 0,by,bz c = [ (0.0, 0.0, 0.0), (bx, 0.0, 0.0), @@ -139,21 +524,9 @@ def _generate_box_wireframe( ] edges = [ - # Bottom perimeter - (0, 1), - (1, 2), - (2, 3), - (3, 0), - # Top perimeter - (4, 5), - (5, 6), - (6, 7), - (7, 4), - # Vertical pillars - (0, 4), - (1, 5), - (2, 6), - (3, 7), + (0, 1), (1, 2), (2, 3), (3, 0), + (4, 5), (5, 6), (6, 7), (7, 4), + (0, 4), (1, 5), (2, 6), (3, 7), ] xs: List[Optional[float]] = [] @@ -176,23 +549,25 @@ def build_arena_3d_figure( trajectory: Optional[Sequence[np.ndarray | Sequence[float]]] = None, current_pos: Optional[np.ndarray | Sequence[float]] = None, current_vel: Optional[np.ndarray | Sequence[float]] = None, - attitude_quaternion: Optional[Sequence[float]] = None, + attitude_quaternion: Optional[Sequence[float] | np.ndarray] = None, wind_vector: Optional[Sequence[float]] = None, lidar_rays: Optional[np.ndarray] = None, 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, + arm_span: float = 1.8, + rotor_radius: float = 0.42, ) -> go.Figure: """Build an interactive 3D Plotly flight arena visualizer. Visualizes: - - Bounded flight volume with wireframe arena walls - - Procedural 3D spherical obstacles - - Goal target waypoint and launch start position + - Bounded flight volume with wireframe arena boundary walls + - Procedural 3D spherical obstacles with proximity clearance zones + - Goal target waypoint with altitude beacon column and destination label + - Launch start origin position - Real-time or historical drone trajectory - - Current drone 3D position and velocity vector + - Detailed 3D quadrotor airframe model with meshes, propellers, and landing gear + - Current drone 3D position, heading chevron, and velocity vector - 16-ray spherical LiDAR sensor beams and contact points """ fig = go.Figure() @@ -206,18 +581,18 @@ def build_arena_3d_figure( y=box_y, z=box_z, mode="lines", - line=dict(color="#4a5568", width=3, dash="dot"), + line=dict(color="#334155", width=2, dash="dot"), name="Arena Boundary", hoverinfo="skip", ) ) - # 2. Obstacles (3D Spheres) + # 2. Obstacles (3D Spheres with Proximity / Safety Zones) if obstacles: for idx, obs in enumerate(obstacles): + # Physical Obstacle Core ox, oy, oz = _generate_sphere_surface(obs.center, obs.radius, n_points=14) - # Custom red colorscale for obstacle spheres - red_scale = [[0.0, "#e53e3e"], [1.0, "#c53030"]] + red_scale = [[0.0, "#991b1b"], [1.0, "#dc2626"]] fig.add_trace( go.Surface( x=ox, @@ -225,15 +600,53 @@ def build_arena_3d_figure( z=oz, colorscale=red_scale, showscale=False, - opacity=0.65, + opacity=0.60, name=f"Obstacle {idx + 1}", hoverinfo="text", - text=f"Obstacle {idx + 1}
Pos: [{obs.center[0]:.1f}, {obs.center[1]:.1f}, {obs.center[2]:.1f}]
Radius: {obs.radius:.1f}m", + text=( + f"Obstacle {idx + 1}
" + f"Center: [{obs.center[0]:.1f}, {obs.center[1]:.1f}, {obs.center[2]:.1f}]
" + f"Radius: {obs.radius:.1f} m" + ), ) ) - # 3. Target Waypoint + # Obstacle Proximity / Safety Clearance Perimeter Ring + safety_r = obs.radius + 0.8 + th = np.linspace(0.0, 2.0 * math.pi, 24) + ring_x = obs.center[0] + safety_r * np.cos(th) + ring_y = obs.center[1] + safety_r * np.sin(th) + ring_z = np.full_like(th, obs.center[2]) + fig.add_trace( + go.Scatter3d( + x=ring_x, + y=ring_y, + z=ring_z, + mode="lines", + line=dict(color="rgba(239, 68, 68, 0.4)", width=2, dash="dash"), + name=f"Safety Margin {idx + 1}", + showlegend=False, + hoverinfo="skip", + ) + ) + + # 3. Target Waypoint with Altitude Beacon Column if target is not None: + # Vertical Altitude Beacon Line from ground to waypoint + fig.add_trace( + go.Scatter3d( + x=[target[0], target[0]], + y=[target[1], target[1]], + z=[0.0, target[2] + 1.5], + mode="lines", + line=dict(color="#10b981", width=2, dash="dot"), + name="Waypoint Beacon Column", + showlegend=False, + hoverinfo="skip", + ) + ) + + # Target Waypoint Marker fig.add_trace( go.Scatter3d( x=[target[0]], @@ -242,13 +655,13 @@ def build_arena_3d_figure( mode="markers+text", marker=dict( symbol="diamond", - size=12, - color="#48bb78", + size=14, + color="#10b981", line=dict(color="#ffffff", width=2), ), - text=["TARGET"], + text=["WAYPOINT GOAL"], textposition="top center", - textfont=dict(color="#48bb78", size=13), + textfont=dict(color="#10b981", size=13), name="Target Waypoint", hoverinfo="text", hovertext=f"Target Goal
Pos: [{target[0]:.2f}, {target[1]:.2f}, {target[2]:.2f}]", @@ -262,16 +675,19 @@ def build_arena_3d_figure( x=[start_pos[0]], y=[start_pos[1]], z=[start_pos[2]], - mode="markers", + mode="markers+text", marker=dict( symbol="circle", - size=8, - color="#4299e1", - line=dict(color="#ffffff", width=1), + size=9, + color="#06b6d4", + line=dict(color="#ffffff", width=1.5), ), + text=["LAUNCH"], + textposition="bottom center", + textfont=dict(color="#06b6d4", size=11), name="Start Position", hoverinfo="text", - hovertext=f"Start
Pos: [{start_pos[0]:.2f}, {start_pos[1]:.2f}, {start_pos[2]:.2f}]", + hovertext=f"Launch Origin
Pos: [{start_pos[0]:.2f}, {start_pos[1]:.2f}, {start_pos[2]:.2f}]", ) ) @@ -284,7 +700,7 @@ def build_arena_3d_figure( y=traj_arr[:, 1], z=traj_arr[:, 2], mode="lines", - line=dict(color="#ecc94b", width=5), + line=dict(color="#f59e0b", width=5), name="Flight Path", hoverinfo="text", hovertext=[ @@ -318,13 +734,13 @@ def build_arena_3d_figure( y=ly, z=lz, mode="lines", - line=dict(color="rgba(0, 255, 255, 0.45)", width=2), + line=dict(color="rgba(0, 242, 254, 0.40)", width=2), name="16-Ray LiDAR", hoverinfo="skip", ) ) - # 7. Current Drone Quadrotor Geometry, Attitude & Velocity + # 7. Current Quadrotor Airframe: 3D Meshes, Attitude & Telemetry if current_pos is not None: geom = compute_quadrotor_3d_geometry( current_pos, @@ -333,112 +749,50 @@ def build_arena_3d_figure( 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", - ) - ) + def _add_mesh(mesh_dict: Dict[str, np.ndarray], color: str, name: str, opacity: float = 1.0, showlegend: bool = False) -> None: + if len(mesh_dict["x"]) > 0: + fig.add_trace( + go.Mesh3d( + x=mesh_dict["x"], + y=mesh_dict["y"], + z=mesh_dict["z"], + i=mesh_dict["i"], + j=mesh_dict["j"], + k=mesh_dict["k"], + color=color, + opacity=opacity, + name=name, + flatshading=True, + lighting=dict(ambient=0.6, diffuse=0.8, roughness=0.3, specular=0.4), + hoverinfo="name", + showlegend=showlegend, + ) + ) - # 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", - ) - ) + # 3D Fuselage Pod + _add_mesh(geom["mesh_fuselage"], "#1e293b", "Fuselage", opacity=0.95, showlegend=True) - # 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", - ) - ) + # 3D Carbon Fiber Arms (Front and Rear) + _add_mesh(geom["mesh_front_arms"], "#ef4444", "Front Arms", opacity=1.0, showlegend=True) + _add_mesh(geom["mesh_rear_arms"], "#3b82f6", "Rear Arms", opacity=1.0, showlegend=True) - # 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", - ) - ) + # Rotor Assemblies (Motors and Hubs) + _add_mesh(geom["mesh_rotor_assemblies"], "#94a3b8", "Rotor Assemblies", opacity=1.0, showlegend=True) - # 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)"], - ) - ) + # Front & Rear 3D Propeller Blades + _add_mesh(geom["mesh_props_front"], "#00e5ff", "Front Propellers", opacity=0.9, showlegend=True) + _add_mesh(geom["mesh_props_rear"], "#3b82f6", "Rear Propellers", opacity=0.9, showlegend=True) - # 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)"], - ) - ) + # Dual Landing Gear Skids & Struts + _add_mesh(geom["mesh_landing_gear"], "#475569", "Landing Gear", opacity=1.0, showlegend=True) - # 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", - ) - ) + # Forward Optical Sensor Gimbal + _add_mesh(geom["mesh_camera"], "#06b6d4", "Sensor Gimbal & Lens", opacity=1.0, showlegend=True) + + # Forward Heading Indicator Chevron (+X_B) + _add_mesh(geom["mesh_heading_indicator"], "#fbbf24", "Forward Heading (+X_B)", opacity=1.0, showlegend=True) + # Center Marker (Drone Trace) fig.add_trace( go.Scatter3d( x=[current_pos[0]], @@ -447,16 +801,16 @@ def build_arena_3d_figure( mode="markers+text", marker=dict( symbol="diamond", - size=10, + size=8, color="#ed8936", line=dict(color="#ffffff", width=2), ), text=["DRONE"], textposition="bottom center", - textfont=dict(color="#ed8936", size=13), + textfont=dict(color="#ed8936", size=12), name="Drone", hoverinfo="text", - hovertext=f"Drone
Pos: [{current_pos[0]:.2f}, {current_pos[1]:.2f}, {current_pos[2]:.2f}]", + hovertext=f"Drone Center
Pos: [{current_pos[0]:.2f}, {current_pos[1]:.2f}, {current_pos[2]:.2f}]", ) ) @@ -473,7 +827,7 @@ def build_arena_3d_figure( y=[current_pos[1], vel_endpoint[1]], z=[current_pos[2], vel_endpoint[2]], mode="lines", - line=dict(color="#ed8936", width=4), + line=dict(color="#f97316", width=4), name="Velocity Vector", hoverinfo="skip", ) @@ -482,7 +836,7 @@ def build_arena_3d_figure( # 6-DOF Rigid-body Attitude Coordinate Frame (Roll/Pitch/Yaw) if attitude_quaternion is not None: rot_mat = quaternion_to_rotation_matrix(attitude_quaternion) - axis_len = 1.2 + axis_len = 1.3 axes_colors = [ ("#ef4444", "Body X (Roll)"), ("#22c55e", "Body Y (Pitch)"), @@ -499,13 +853,13 @@ def build_arena_3d_figure( line=dict(color=col, width=4), name=ax_name, hoverinfo="name", + showlegend=False, ) ) # Environmental Wind Vector if wind_vector is not None and float(np.linalg.norm(wind_vector)) > 0.05: wx, wy, wz = [float(v) for v in wind_vector] - # Draw wind indicator near the top corner w_origin = [bx * 0.1, by * 0.1, bz * 0.85] w_scale = 0.5 fig.add_trace( @@ -521,51 +875,55 @@ def build_arena_3d_figure( ) ) - # Configure 3D Camera, Lighting, and Dark Flight Deck Styling + # Configure 3D Camera, Lighting, and Aerospace Mission-Control Styling fig.update_layout( template="plotly_dark", - paper_bgcolor="#111827", - plot_bgcolor="#111827", - title=dict(text=title, font=dict(size=18, color="#f7fafc"), x=0.05, y=0.95), + paper_bgcolor="#080c14", + plot_bgcolor="#080c14", + title=dict(text=title, font=dict(size=17, color="#f1f5f9"), x=0.02, y=0.98), scene=dict( xaxis=dict( - title="X Distance (m)", + title="X Position (m)", range=[0, bx], - backgroundcolor="#1a202c", - gridcolor="#2d3748", + backgroundcolor="#0b0f19", + gridcolor="#1e293b", showbackground=True, - zerolinecolor="#4a5568", + zerolinecolor="#334155", ), yaxis=dict( - title="Y Distance (m)", + title="Y Position (m)", range=[0, by], - backgroundcolor="#1a202c", - gridcolor="#2d3748", + backgroundcolor="#0b0f19", + gridcolor="#1e293b", showbackground=True, - zerolinecolor="#4a5568", + zerolinecolor="#334155", ), zaxis=dict( title="Z Altitude (m)", range=[0, bz], - backgroundcolor="#1a202c", - gridcolor="#2d3748", + backgroundcolor="#0b0f19", + gridcolor="#1e293b", showbackground=True, - zerolinecolor="#4a5568", + zerolinecolor="#334155", ), aspectratio=dict(x=1.0, y=1.0, z=bz / max(bx, by)), camera=dict( - eye=dict(x=1.5, y=-1.5, z=1.2), + eye=dict(x=1.35, y=-1.35, z=0.85), + center=dict(x=0.0, y=0.0, z=-0.08), up=dict(x=0, y=0, z=1), ), ), - margin=dict(l=0, r=0, b=0, t=40), + margin=dict(l=0, r=0, b=0, t=35), legend=dict( - yanchor="top", - y=0.95, + orientation="h", + yanchor="bottom", + y=1.01, xanchor="left", - x=0.02, - bgcolor="rgba(17, 24, 39, 0.7)", - font=dict(color="#e2e8f0"), + x=0.0, + bgcolor="rgba(11, 15, 25, 0.85)", + bordercolor="#1e293b", + borderwidth=1, + font=dict(color="#cbd5e1", size=11), ), ) @@ -702,6 +1060,8 @@ def build_training_curve_figure( if not episode_rewards: fig.update_layout( template="plotly_dark", + paper_bgcolor="#080c14", + plot_bgcolor="#080c14", title=title, annotations=[ dict( @@ -745,6 +1105,8 @@ def build_training_curve_figure( fig.update_layout( template="plotly_dark", + paper_bgcolor="#080c14", + plot_bgcolor="#080c14", title=title, xaxis_title="Episode", yaxis_title="Return (Cumulative Reward)", @@ -767,6 +1129,13 @@ def build_comparison_bar_chart( categories = ["Success Rate (%)", "Collision Rate (%)"] colors = ["#00E676", "#FF5252", "#FFD700", "#7C4DFF"] + for i, policy_name in enumerate(policies): + m = metrics_map[policy_name] + succ = (m.success_rate if hasattr(m, "success_rate") else m.get("success_rate", 0.0)) or 0.0 + coll = ( + m.collision_rate if hasattr(m, "collision_rate") else m.get("collision_rate", 0.0) + ) or 0.0 + for i, policy_name in enumerate(policies): m = metrics_map[policy_name] succ = (m.success_rate if hasattr(m, "success_rate") else m.get("success_rate", 0.0)) or 0.0 @@ -787,6 +1156,8 @@ def build_comparison_bar_chart( fig.update_layout( template="plotly_dark", + paper_bgcolor="#080c14", + plot_bgcolor="#080c14", barmode="group", title=title, yaxis=dict(title="Percentage (%)", range=[0, 105]), @@ -833,6 +1204,8 @@ def build_density_experiment_figure( fig.update_layout( template="plotly_dark", + paper_bgcolor="#080c14", + plot_bgcolor="#080c14", barmode="group", title=title, yaxis=dict(title="Rate (%)", range=[0, 105]), @@ -1003,6 +1376,8 @@ def build_recovery_curve_figure( fig.update_layout( template="plotly_dark", + paper_bgcolor="#080c14", + plot_bgcolor="#080c14", title=title, xaxis=dict(title="Post-Shift Episode Index (k)", tickmode="linear", tick0=1, dtick=1), yaxis=dict(title="Episodic Return"), diff --git a/tests/test_gui.py b/tests/test_gui.py index ff82d7e..723fe6a 100644 --- a/tests/test_gui.py +++ b/tests/test_gui.py @@ -495,3 +495,144 @@ def test_app_simulation_environment_cleanup_on_failure() -> None: except RuntimeError: pass assert mock_env.close.call_count == 3 + + +def test_detailed_quadrotor_mesh_geometry_components() -> None: + """Verify that compute_quadrotor_3d_geometry generates complete 3D mesh components.""" + center = np.array([12.0, 14.0, 6.0]) + quat = np.array([1.0, 0.0, 0.0, 0.0]) + geom = compute_quadrotor_3d_geometry(center_pos=center, quaternion=quat, arm_span=1.6) + + # Required mesh keys + required_mesh_keys = [ + "mesh_fuselage", + "mesh_arms", + "mesh_front_arms", + "mesh_rear_arms", + "mesh_motors_front", + "mesh_motors_rear", + "mesh_rotor_hubs", + "mesh_rotor_assemblies", + "mesh_props_front", + "mesh_props_rear", + "mesh_landing_gear", + "mesh_camera", + "mesh_heading_indicator", + ] + for key in required_mesh_keys: + assert key in geom, f"Missing mesh component key: {key}" + mesh = geom[key] + assert "x" in mesh and "y" in mesh and "z" in mesh + assert "i" in mesh and "j" in mesh and "k" in mesh + assert len(mesh["x"]) > 0, f"Empty vertex array for {key}" + assert len(mesh["i"]) > 0, f"Empty triangle array for {key}" + # Triangular indices must be within bounds of vertices + n_verts = len(mesh["x"]) + assert np.all(mesh["i"] < n_verts) + assert np.all(mesh["j"] < n_verts) + assert np.all(mesh["k"] < n_verts) + + +def test_quadrotor_mesh_translation_and_rotation_invariance() -> None: + """Verify that drone mesh coordinates strictly translate with center and rotate with quaternion.""" + c1 = np.array([0.0, 0.0, 0.0]) + c2 = np.array([25.0, 30.0, 10.0]) + q_ident = np.array([1.0, 0.0, 0.0, 0.0]) + + g1 = compute_quadrotor_3d_geometry(center_pos=c1, quaternion=q_ident, arm_span=1.5) + g2 = compute_quadrotor_3d_geometry(center_pos=c2, quaternion=q_ident, arm_span=1.5) + + # Translation offset must be exact across all fuselage vertices + diff = np.column_stack([ + g2["mesh_fuselage"]["x"] - g1["mesh_fuselage"]["x"], + g2["mesh_fuselage"]["y"] - g1["mesh_fuselage"]["y"], + g2["mesh_fuselage"]["z"] - g1["mesh_fuselage"]["z"], + ]) + assert np.allclose(diff, c2 - c1, atol=1e-5) + + # 180-deg Yaw rotation around Z axis: [0, 0, 1, 0] in (w, x, y, z) or [0, 0, 0, 1] + q_yaw180 = np.array([0.0, 0.0, 0.0, 1.0]) + g_rot = compute_quadrotor_3d_geometry(center_pos=c1, quaternion=q_yaw180, arm_span=1.5) + + # Rotors rotated 180 deg around Z: R1 (+d, -d) rotates to R3 (-d, +d) + d = 1.5 / (2.0 * np.sqrt(2.0)) + assert np.allclose(g1["rotors"][0], [d, -d, 0.0]) + assert np.allclose(g_rot["rotors"][0], [-d, d, 0.0], atol=1e-5) + + +def test_drone_distinguishable_from_obstacles_in_plotly_figure() -> None: + """Verify that drone mesh traces and obstacle surfaces are visually and structurally distinct.""" + bounds = (40.0, 40.0, 20.0) + obstacles = [ + ObstacleSphere3D(center=np.array([10.0, 10.0, 5.0]), radius=2.0), + ObstacleSphere3D(center=np.array([20.0, 20.0, 8.0]), radius=2.5), + ] + drone_pos = np.array([15.0, 15.0, 6.0]) + fig = build_arena_3d_figure( + bounds=bounds, + obstacles=obstacles, + current_pos=drone_pos, + attitude_quaternion=[1.0, 0.0, 0.0, 0.0], + ) + + # Collect trace types + mesh_traces = [t for t in fig.data if isinstance(t, go.Mesh3d)] + surface_traces = [t for t in fig.data if isinstance(t, go.Surface)] + + assert len(mesh_traces) >= 6, "Expected at least 6 distinct 3D drone mesh components" + assert len(surface_traces) == 2, "Expected 2 obstacle sphere surfaces" + + # Drone mesh traces have names matching quadrotor parts + mesh_names = [t.name for t in mesh_traces if hasattr(t, "name")] + assert "Fuselage" in mesh_names + assert "Front Arms" in mesh_names + assert "Rear Arms" in mesh_names + assert "Front Propellers" in mesh_names + assert "Rear Propellers" in mesh_names + assert "Landing Gear" in mesh_names + + # Obstacle surfaces are named Obstacle 1, Obstacle 2 + surface_names = [t.name for t in surface_traces if hasattr(t, "name")] + assert "Obstacle 1" in surface_names + assert "Obstacle 2" in surface_names + + +def test_3dof_vs_6dof_representations_remain_distinct() -> None: + """Verify distinction between 3-DOF kinematic point-mass and 6-DOF rigid-body outputs.""" + env3 = DroneNavigation3DEnv(num_obstacles=1, max_steps=5) + env6 = Drone6DOFEnv(num_obstacles=1, max_steps=5) + + res3 = run_drone_simulation_episode(env=env3, seed=123) + res6 = run_drone_simulation_episode(env=env6, seed=123) + + # 3-DOF does not simulate quaternion attitude + assert res3["steps"][0]["quaternion"] is None + assert res3["steps"][0]["action"].shape == (3,) + + # 6-DOF provides 4-element unit quaternion and 4 rotor thrust actions + q6 = res6["steps"][0]["quaternion"] + assert q6 is not None + assert len(q6) == 4 + assert np.isclose(np.linalg.norm(q6), 1.0, atol=1e-4) + assert res6["steps"][0]["action"].shape == (4,) + + env3.close() + env6.close() + + +def test_telemetry_consistency_with_simulation_steps() -> None: + """Verify that playback step info perfectly correlates with trajectory positions and sensor ranges.""" + env = Drone6DOFEnv(num_obstacles=2, max_steps=12) + episode = run_drone_simulation_episode(env=env, seed=77) + steps = episode["steps"] + + for idx, s in enumerate(steps): + assert s["step"] == idx + assert np.allclose(s["position"], episode["trajectory"][idx]) + assert s["altitude"] == s["position"][2] + assert s["speed"] >= 0.0 + assert s["distance_to_goal"] >= 0.0 + assert len(s["lidar_ranges"]) == 16 + assert np.all(s["lidar_ranges"] >= 0.0) + + env.close()