From fd305d58dbd512def1e89e785f8c11326b24eb70 Mon Sep 17 00:00:00 2001 From: Aryan Saxena Date: Sun, 15 Nov 2026 19:19:43 +0530 Subject: [PATCH 1/7] fix(phase3): harden LQR allocation, provably collision-free A*, dynamic obstacle avoidance, physics integration, and complete provenance --- docs/AUTONOMOUS_NAVIGATION.md | 116 +++++++--- pyproject.toml | 2 + src/adaptive_rl/controllers/lqr.py | 207 +++++++++++++---- src/adaptive_rl/environments/drone_6dof.py | 80 ++++++- src/adaptive_rl/flight_testing/recorder.py | 114 +++++++-- src/adaptive_rl/flight_testing/scenarios.py | 166 ++++++++++--- .../navigation/dynamic_obstacles.py | 28 ++- src/adaptive_rl/navigation/mission.py | 25 +- src/adaptive_rl/navigation/planner.py | 52 ++++- tests/test_actuator_aerodynamics.py | 134 ++++++++++- tests/test_fault_injection.py | 86 ++++++- tests/test_lqr_controller.py | 219 ++++++++++++------ tests/test_navigation_astar.py | 106 +++++++++ tests/test_recorder_fingerprint_phase3.py | 56 +++++ 14 files changed, 1184 insertions(+), 207 deletions(-) diff --git a/docs/AUTONOMOUS_NAVIGATION.md b/docs/AUTONOMOUS_NAVIGATION.md index fb14279..c62ecbf 100644 --- a/docs/AUTONOMOUS_NAVIGATION.md +++ b/docs/AUTONOMOUS_NAVIGATION.md @@ -30,15 +30,17 @@ To prevent publication of deceptive conclusions from underpowered sample sets: - **Quick Smoke Testing:** Runs with $N < 10$ are explicitly tagged `is_smoke_test = True`. Descriptive statistics (mean, std, min, max) are provided, while inferential confidence intervals are suppressed (`confidence_interval_95 = None`) to prevent misleading scientific claims. - **Seed Validation:** Duplicate seeds in an evaluation batch are detected and rejected. -### 1.3 Unambiguous Flight Outcome Taxonomy -Simulation termination and truncation signals are mapped with strict precedence: -1. `COLLISION` (or `CRASH_FAULT` if under active actuator failure): Drone collides with terrain, obstacles, or ground. Takes precedence over step bounds. -2. `BOUNDARY_BREACH`: Drone leaves the defined flight arena. -3. `SUCCESS`: Drone reaches within target arrival tolerance ($\le 0.3\text{ m}$) with controlled velocity ($\le 0.5\text{ m/s}$). -4. `TIMEOUT`: Simulated elapsed time exceeds scenario `max_duration_seconds`. -5. `MAX_STEPS`: Episode step count exceeds maximum steps without timeout or collision. -6. `RECOVERED_DEGRADED`: Actuator fault injected, but vehicle maintained stability and completed scenario or survived without crash. -7. `BLOCKED_NO_PATH`: Navigation planner correctly identified goal is obstructed and halted safely. +### 1.3 Authoritative, Mutually Exclusive Flight Outcome Taxonomy +Simulation termination and truncation signals are mapped with strict deterministic precedence to ensure mutual exclusivity and prevent false-positive claims: + +1. `CRASH_FAULT`: An active actuator fault was injected and the aircraft collided with terrain, obstacles, arena boundaries, ground floor ($z \le 0.8\text{ m}$), or exceeded maximum recovery tilt ($\theta \ge 35^\circ$). +2. `COLLISION`: Aircraft collided with a static or dynamic spherical obstacle. Strict precedence: takes priority over `TIMEOUT` or `MAX_STEPS` even if collision occurs on the final allowed step. +3. `BOUNDARY_BREACH`: Aircraft exited the arena boundaries ($x, y, z$). Strict precedence over step/time limits. +4. `SUCCESS`: Aircraft reached within target arrival tolerance ($r \le r_{\text{target}}$) with velocity settled below threshold ($v \le 0.5\text{ m/s}$), visited all sequential waypoints, and completed with zero collisions. **A high-velocity fly-through ($> 0.5\text{ m/s}$) is strictly rejected from SUCCESS.** +5. `RECOVERED_DEGRADED`: An active actuator fault was injected, but the aircraft maintained level attitude ($\theta < 35^\circ$), kept altitude ($z > 0.8\text{ m}$), reached the target sphere, and settled ($v \le 0.5\text{ m/s}$). **Surviving without completing the mission and settling at the goal is classified as `MAX_STEPS` or `TIMEOUT`, NEVER `RECOVERED_DEGRADED`.** +6. `BLOCKED_NO_PATH`: Navigation planner provably determined that no safe collision-free path exists, or dynamic obstacles obstructed the flight corridor, and commanded a safe hover hold. +7. `TIMEOUT`: Simulated elapsed wall-clock duration exceeded scenario `max_duration_seconds`. +8. `MAX_STEPS`: Episode step count reached `max_steps` without collision, arrival, or failure. --- @@ -67,24 +69,26 @@ The navigation system lives in `adaptive_rl.navigation` and cleanly separates ge +-----------------------+ ``` -### 2.1 3D A* Path Planning (`AStarPlanner3D`) +### 2.1 Provably Collision-Free 3D A* Path Planning (`AStarPlanner3D`) - **Spatial Grid Representation:** Discretizes the 3D bounding box $[-10, 10] \times [-10, 10] \times [0, 15]\text{ m}$ into uniform voxels (default grid resolution $\Delta = 0.5\text{ m}$). -- **Connectivity:** 26-connected neighbor graph (orthogonal, planar diagonal, and spatial cubic diagonal transitions) with true Euclidean edge costs: +- **Connectivity & Corner-Cutting Prevention:** 26-connected neighbor graph (orthogonal, planar diagonal, and spatial cubic diagonal transitions) with true Euclidean edge costs: $$c(u, v) = \|p_u - p_v\|_2$$ + **Continuous Edge Collision Checking:** In prior implementations, diagonal neighbor transitions validated endpoint voxels without validating the connecting edge, allowing paths to cut diagonally through obstacle corners. In Phase III hardening, every candidate neighbor expansion performs continuous swept line segment checking $\mathbf{p}(t) = (1-t)\mathbf{p}_u + t\mathbf{p}_v$ with fine interpolation step $\le \frac{\Delta}{2}$. Any neighbor whose connecting edge intersects the inflated obstacle boundary is strictly rejected. - **Admissible Heuristic:** Euclidean distance to goal: $$h(u) = \|p_u - p_{\text{goal}}\|_2$$ - **Obstacle Inflation:** Every spherical obstacle with radius $r_{\text{obs}}$ is inflated by: $$r_{\text{effective}} = r_{\text{obs}} + r_{\text{vehicle}} + \delta_{\text{margin}}$$ - where nominal $r_{\text{vehicle}} = 0.25\text{ m}$ and configurable safety margin $\delta_{\text{margin}} = 0.20\text{ m}$. Any voxel whose distance to obstacle center is $\le r_{\text{effective}}$ is marked impassable. -- **Line-of-Sight (LoS) Path Simplification:** The raw grid path undergoes deterministic ray-cast shortcutting (`simplify_path`). Intermediate nodes are pruned if a continuous, collision-free straight corridor exists between earlier and later waypoints, reducing waypoint count by 60–80% while preserving clearance. + where nominal $r_{\text{vehicle}} = 0.25\text{ m}$ and configurable safety margin $\delta_{\text{margin}} = 0.20\text{ m}$. Any voxel or segment within $r_{\text{effective}}$ of an obstacle center is impassable. +- **Provably Collision-Free Line-of-Sight (LoS) Simplification:** The raw grid path undergoes deterministic ray-cast shortcutting (`simplify_path`). Every prospective segment connecting non-adjacent waypoints is verified with continuous collision checks. If an unvalidated shortcut occurs, the simplification fallback guarantees that no unvalidated or intersecting segment is ever appended to the path. If no safe continuation exists, the planner provably reports `NO_PATH` rather than returning an unsafe path. -### 2.2 Dynamic Obstacles (`DynamicObstacleSphere3D`) -- Encapsulates moving spherical obstacles with configurable velocity vectors and boundary reflection or periodic sinusoidal trajectory models. -- Step-wise kinematic propagation allows testing reactive replanning under non-stationary environments. +### 2.2 Dynamic Obstacles & Receding-Horizon Avoidance (`DynamicObstacleSphere3D`) +- **Kinematic Horizon Prediction:** Dynamic obstacles propagate according to linear or sinusoidal velocity models. `predict_trajectory(t_start, horizon_sec, dt)` and `predict_occupied_spheres(...)` forecast the swept volume of moving obstacles over a lookahead horizon $[t, t + H]$ (e.g., $H = 2.0\text{ s}$). +- **Occupied Volume Inflation:** Spheres occupied across $[t, t + H]$ are merged with the static obstacle field during predictive replanning. ### 2.3 Mission Manager (`MissionManager`) -- Coordinates multi-waypoint missions, dynamic obstacle tracking, emergency stop, and automated Return-to-Home (RTH). -- Sequences waypoints when vehicle enters the capture radius ($r_{\text{arrival}} \le 0.4\text{ m}$). +- Coordinates multi-waypoint missions, predictive dynamic obstacle avoidance, and fail-safe recovery. +- **Receding-Horizon Local Replanning:** Monitors forward path clearance against predicted dynamic obstacle positions. If an intruder is projected to obstruct the flight corridor, `MissionManager` triggers local replanning around the predicted swept volumes. +- **Fail-Safe Hover Hold:** If the dynamic corridor is completely obstructed and no valid replan is found, `MissionManager` commands an immediate zero-velocity hover hold at the current position, maintaining collision clearance until the obstacle clears the route. --- @@ -117,17 +121,58 @@ B = \begin{bmatrix} \end{bmatrix}$$ ### 3.3 ARE Solution & Control Allocation + Solving the continuous Algebraic Riccati Equation: $$A^T P + P A - P B R^{-1} B^T P + Q = 0$$ yields the optimal constant gain matrix: $$K = R^{-1} B^T P \in \mathbb{R}^{4 \times 12}$$ +**Numerical Solver & Honest Fallback Identification:** +`QuadrotorLQRController` solves the ARE directly using `scipy.linalg.solve_continuous_are`. The controller sets `solver_status = "ARE_SOLVED"` and `is_are_solution = True`. To prevent silent approximations, if numerical ARE solving fails (or `scipy` is unavailable), the controller strictly raises an exception unless the caller explicitly sets `allow_fallback=True`. + Control law: $$\mathbf{u} = -K (\mathbf{x} - \mathbf{x}_{\text{target}})$$ -The total thrust $T = m g + \Delta T$ and body torques $\tau$ are mapped into individual rotor thrusts $[f_1, f_2, f_3, f_4]^T$ using analytical inversion of the X-configuration mixer matrix: -$$\begin{bmatrix} f_1 \\ f_2 \\ f_3 \\ f_4 \end{bmatrix} = M^{-1} \begin{bmatrix} T \\ \tau_x \\ \tau_y \\ \tau_z \end{bmatrix}$$ -Individual thrusts are clamped to $[0, f_{\text{max}}]$ before normalization into gym action space $[-1, 1]^4$. +### 3.4 Analytical Control Allocation Mixer Inversion Derivation +In `DroneDynamics6DOF`, the physical relationship between individual rotor thrusts $[f_1, f_2, f_3, f_4]^T$ and total thrust/body torques is governed by rotor positions $(x_i, y_i)$ and reaction torque constants: +- Rotor 1 (Front-Right): $(+d, -d)$, CCW ($+c_\tau$) +- Rotor 2 (Front-Left): $(+d, +d)$, CW ($-c_\tau$) +- Rotor 3 (Rear-Left): $(-d, +d)$, CCW ($+c_\tau$) +- Rotor 4 (Rear-Right): $(-d, -d)$, CW ($-c_\tau$) + +where $d = L / \sqrt{2}$. The forward control allocation matrix $M \in \mathbb{R}^{4 \times 4}$ is: +$$\begin{bmatrix} T \\ \tau_x \\ \tau_y \\ \tau_z \end{bmatrix} = \begin{bmatrix} 1 & 1 & 1 & 1 \\ -d & d & d & -d \\ -d & -d & d & d \\ c_\tau & -c_\tau & c_\tau & -c_\tau \end{bmatrix} \begin{bmatrix} f_1 \\ f_2 \\ f_3 \\ f_4 \end{bmatrix}$$ + +Because the row vectors of $M$ are mutually orthogonal: +- $\mathbf{m}_1 \cdot \mathbf{m}_2 = -d + d + d - d = 0$ +- $\mathbf{m}_1 \cdot \mathbf{m}_3 = -d - d + d + d = 0$ +- $\mathbf{m}_1 \cdot \mathbf{m}_4 = c_\tau - c_\tau + c_\tau - c_\tau = 0$ +- $\mathbf{m}_2 \cdot \mathbf{m}_3 = d^2 - d^2 + d^2 - d^2 = 0$ +- $\mathbf{m}_2 \cdot \mathbf{m}_4 = -d c_\tau - d c_\tau + d c_\tau + d c_\tau = 0$ +- $\mathbf{m}_3 \cdot \mathbf{m}_4 = -d c_\tau + d c_\tau + d c_\tau - d c_\tau = 0$ + +The matrix is directly invertible analytically: +$$M^{-1} = \begin{bmatrix} +\frac{1}{4} & -\frac{1}{4d} & -\frac{1}{4d} & \frac{1}{4c_\tau} \\ +\frac{1}{4} & \frac{1}{4d} & -\frac{1}{4d} & -\frac{1}{4c_\tau} \\ +\frac{1}{4} & \frac{1}{4d} & \frac{1}{4d} & \frac{1}{4c_\tau} \\ +\frac{1}{4} & -\frac{1}{4d} & \frac{1}{4d} & -\frac{1}{4c_\tau} +\end{bmatrix}$$ + +Thus, the exact inverted motor thrusts are: +$$f_1 = \frac{T}{4} - \frac{\tau_x}{4d} - \frac{\tau_y}{4d} + \frac{\tau_z}{4c_\tau}$$ +$$f_2 = \frac{T}{4} + \frac{\tau_x}{4d} - \frac{\tau_y}{4d} - \frac{\tau_z}{4c_\tau}$$ +$$f_3 = \frac{T}{4} + \frac{\tau_x}{4d} + \frac{\tau_y}{4d} + \frac{\tau_z}{4c_\tau}$$ +$$f_4 = \frac{T}{4} - \frac{\tau_x}{4d} + \frac{\tau_y}{4d} - \frac{\tau_z}{4c_\tau}$$ + +Multiplying $M \cdot M^{-1} = I_{4 \times 4}$ holds to machine precision ($< 10^{-15}$). Individual thrusts are clamped to $[0, f_{\text{max}}]$ and mapped into normalized actions in $[-1, 1]^4$. + +### 3.5 Real Environment Observation Adapter +`QuadrotorLQRController` natively accepts: +- 36-dimensional normalized Gymnasium observations from `Drone6DOFEnv`, decoding normalized position, velocity, attitude quaternion, angular rates, and target coordinates. +- 13-dimensional true physical state vectors $[p, v, q, \omega]$. +- Direct `DroneState6DOF` instances. +Unsupported observation dimensions are strictly rejected with descriptive `ValueError`. --- @@ -136,22 +181,26 @@ Individual thrusts are clamped to $[0, f_{\text{max}}]$ before normalization int ### 4.1 First-Order Motor Dynamics (Actuator Lag) Real brushless DC motors and Electronic Speed Controllers (ESCs) exhibit electro-mechanical response delay. The actual thrust $f_i(t)$ tracks commanded thrust $f_{i,\text{cmd}}(t)$ according to: $$\dot{f}_i(t) = \frac{1}{\tau_{\text{motor}}} \left( f_{i,\text{cmd}}(t) - f_i(t) \right)$$ -where default time constant $\tau_{\text{motor}} = 0.033\text{ s}$ ($\sim 30\text{ Hz}$ bandwidth). -In `DroneDynamics6DOF`, motor dynamics are integrated continuously across the 5 internal RK4 substeps ($dt_{\text{sub}} = 0.01\text{ s}$): -$$f_i(t + dt) = f_i(t) + \frac{dt}{\tau_{\text{motor}}} (f_{i,\text{cmd}} - f_i(t))$$ +where default time constant $\tau_{\text{motor}} = 0.03\text{ s}$ ($\sim 33\text{ Hz}$ bandwidth). + +**Exact Analytical Step Response vs Forward Euler:** +In prior implementations, motor thrust was integrated using simple forward Euler discretization $f(t+h) = f(t) + \frac{h}{\tau}(f_{\text{cmd}} - f(t))$, which suffers from numerical instability when $h > \tau$ and does not accurately match continuous-time physics. In Phase III hardening, `DroneDynamics6DOF` integrates actuator dynamics across each RK4 substep using the exact analytical first-order step response: +$$\alpha = \exp\left(-\frac{h}{\tau_{\text{motor}}}\right)$$ +$$f_i(t + h) = f_i(t) \alpha + f_{i,\text{cmd}}(t) (1 - \alpha)$$ +This analytical form is unconditionally stable for any substep size $h > 0$ and preserves physical fidelity to machine precision. ### 4.2 Aerodynamic Wind Model Ambient atmospheric wind is modeled as continuous vector $\mathbf{v}_{\text{wind}} = [w_x, w_y, w_z]^T\text{ m/s}$. The aerodynamic drag opposing vehicle motion is governed by relative airspeed: $$\mathbf{v}_{\text{rel}} = \mathbf{v}_{\text{quad}} - \mathbf{v}_{\text{wind}}$$ -$$\mathbf{F}_{\text{drag}} = -D_v \|\mathbf{v}_{\text{rel}}\|_2 \mathbf{v}_{\text{rel}} - D_{v,\text{linear}} \mathbf{v}_{\text{rel}}$$ -This wind force is evaluated at every RK4 substep, providing realistic drift and control disturbance. +$$\mathbf{F}_{\text{drag}} = -D_v \mathbf{v}_{\text{rel}} - D_q \|\mathbf{v}_{\text{rel}}\|_2 \mathbf{v}_{\text{rel}}$$ +where $D_v$ is linear damping and $D_q$ is quadratic aerodynamic drag coefficient. Both linear and quadratic drag components are evaluated at every RK4 substep, providing realistic drift and control disturbance under opposing, cross, or tailwind conditions. ### 4.3 Cheeseman-Bennett Ground Effect Model -When operating close to the ground plane ($z < 4 R_{\text{rotor}}$, where $R_{\text{rotor}} \approx 0.1\text{ m}$), rotor downwash interacts with the ground surface, producing an aerodynamic cushion that increases effective thrust. +When operating close to the ground plane ($z < 4 R_{\text{rotor}}$, where $R_{\text{rotor}} \approx 0.09\text{ m}$), rotor downwash interacts with the ground surface, producing an aerodynamic cushion that increases effective thrust. Using the classical Cheeseman-Bennett formulation: -$$\frac{T_{\text{IGE}}}{T_{\text{OGE}}} = \frac{1}{1 - \left( \frac{R_{\text{rotor}}}{4 z} \right)^2}$$ -Clamped to a maximum multiplier of $1.30\times$ to avoid singularity at $z \to 0$: -$$T_{\text{actual}} = \eta_{\text{IGE}}(z) \cdot T_{\text{commanded}}, \quad \eta_{\text{IGE}}(z) = \min\left(1.30, \frac{1}{1 - (R_{\text{rotor}} / (4 \max(z, 0.05)))^2}\right)$$ +$$\frac{T_{\text{IGE}}}{T_{\text{OGE}}} = \frac{1}{1 - \left( \frac{R_{\text{rotor}}}{4 z_{\text{eff}}} \right)^2}$$ +where $z_{\text{eff}} = \max(0.5 R_{\text{rotor}}, z)$ and clamped to a maximum multiplier of $1.30\times$ to avoid singularity at $z \to 0$: +$$T_{\text{actual}} = \eta_{\text{IGE}}(z) \cdot T_{\text{commanded}}, \quad \eta_{\text{IGE}}(z) = \min\left(1.30, \max\left(1.0, \frac{1}{1 - (R_{\text{rotor}} / (4 z_{\text{eff}}))^2}\right)\right)$$ --- @@ -177,6 +226,13 @@ $$T_{\text{actual}} = \eta_{\text{IGE}}(z) \cdot T_{\text{commanded}}, \quad \et - Accelerometer bias is dynamically estimated via integral tracking: $$\dot{\hat{\mathbf{b}}} = -k_i \mathbf{e}_{\text{tilt}}$$ +### 5.3 Sensor Observability Limits & Dead-Reckoning Drift +In any strapdown inertial navigation system without external aiding: +1. **Yaw (Heading) Unobservability:** Gravity leveling observes roll and pitch because the gravity vector $[0, 0, -g]^T$ produces a non-zero projection onto body $X$ and $Y$ when tilted. However, rotations about the vertical world axis $Z$ (yaw) leave the gravity vector invariant ($\partial \mathbf{g} / \partial \psi = \mathbf{0}$). Consequently, yaw is mathematically unobservable from accelerometer readings alone. Without a 3-axis magnetometer or external optical/GNSS reference, gyro bias drift causes uncorrected yaw random-walk drift over extended durations. +2. **Position Dead-Reckoning Divergence:** Estimating position requires double integration of transformed acceleration: + $$\mathbf{p}(t) = \mathbf{p}(0) + \mathbf{v}(0) t + \int_0^t \int_0^s (R(\hat{q}) \mathbf{a}_{\text{meas}} + \mathbf{g}) d\tau ds$$ + Any residual attitude error $\delta \theta$ tilts the subtracted gravity vector, introducing a false horizontal acceleration $g \sin(\delta \theta) \approx g \delta \theta$. Double integration of this term produces unbounded quadratic position error drift $\mathcal{O}(g \delta \theta \cdot t^2)$. This is an inherent physical property of unaided inertial odometry, correctly reflected in the simulation telemetry. + --- ## 6. Domain Randomization Engine diff --git a/pyproject.toml b/pyproject.toml index 6e73140..95e81b7 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -37,6 +37,7 @@ dependencies = [ "rich>=13.0.0", "pydantic>=2.0.0", "numpy>=1.24.0", + "scipy>=1.10.0", ] [project.optional-dependencies] @@ -65,6 +66,7 @@ all = [ "streamlit>=1.30.0", "plotly>=5.18.0", "matplotlib>=3.7.0", + "scipy>=1.10.0", "pytest>=7.4.0", "pytest-cov>=4.1.0", "ruff>=0.1.0", diff --git a/src/adaptive_rl/controllers/lqr.py b/src/adaptive_rl/controllers/lqr.py index ef285c3..8de32a0 100644 --- a/src/adaptive_rl/controllers/lqr.py +++ b/src/adaptive_rl/controllers/lqr.py @@ -33,6 +33,10 @@ def __init__( q_rate: float = 2.0, r_thrust: float = 1.0, r_torque: float = 5.0, + bounds: Tuple[float, float, float] = (30.0, 30.0, 15.0), + max_velocity: float = 10.0, + max_angular_velocity: float = 10.0, + allow_fallback: bool = False, ) -> None: self.mass = float(mass) self.gravity = float(gravity) @@ -42,6 +46,10 @@ def __init__( self.max_thrust_per_motor = float(max_thrust_per_motor) self.c_tau = float(torque_to_thrust_ratio) self.dt = float(dt) + self.bounds = tuple(float(b) for b in bounds) + self.max_velocity = float(max_velocity) + self.max_angular_velocity = float(max_angular_velocity) + self.allow_fallback = bool(allow_fallback) self.hover_thrust_total = self.mass * self.gravity self.hover_thrust_per_motor = self.hover_thrust_total / 4.0 @@ -69,10 +77,80 @@ def __init__( # Controls: [Delta_T, tau_x, tau_y, tau_z] self.R = np.diag([r_thrust, r_torque, r_torque, r_torque * 1.5]) + self.solver_status: str = "INITIALIZING" + self.is_are_solution: bool = False + # Compute optimal gain matrix K self.K = self._compute_lqr_gain() self.target_position = np.array([0.0, 0.0, 2.0], dtype=np.float64) + @staticmethod + def get_allocation_matrix(d: float, c_tau: float) -> np.ndarray: + """Construct the physical forward allocation matrix M (4x4) for an X-quadrotor. + + Maps individual rotor thrusts [f1, f2, f3, f4]^T to [T, tau_x, tau_y, tau_z]^T: + T = f1 + f2 + f3 + f4 + tau_x = d * (-f1 + f2 + f3 - f4) + tau_y = d * (-f1 - f2 + f3 + f4) + tau_z = c_tau * (+f1 - f2 + f3 - f4) + """ + return np.array( + [ + [1.0, 1.0, 1.0, 1.0], + [-d, d, d, -d], + [-d, -d, d, d], + [c_tau, -c_tau, c_tau, -c_tau], + ], + dtype=np.float64, + ) + + @staticmethod + def get_allocation_matrix_inverse(d: float, c_tau: float) -> np.ndarray: + """Construct the exact analytical inverse allocation matrix M^{-1} (4x4). + + Maps virtual control wrench [T, tau_x, tau_y, tau_z]^T to [f1, f2, f3, f4]^T: + f1 = T/4 - tau_x/(4d) - tau_y/(4d) + tau_z/(4*c_tau) + f2 = T/4 + tau_x/(4d) - tau_y/(4d) - tau_z/(4*c_tau) + f3 = T/4 + tau_x/(4d) + tau_y/(4d) + tau_z/(4*c_tau) + f4 = T/4 - tau_x/(4d) + tau_y/(4d) - tau_z/(4*c_tau) + """ + if d <= 1e-9: + raise ValueError(f"Arm diagonal offset d must be positive, got {d}") + if c_tau <= 1e-12: + raise ValueError(f"Torque constant c_tau must be positive, got {c_tau}") + + return np.array( + [ + [0.25, -0.25 / d, -0.25 / d, 0.25 / c_tau], + [0.25, 0.25 / d, -0.25 / d, -0.25 / c_tau], + [0.25, 0.25 / d, 0.25 / d, 0.25 / c_tau], + [0.25, -0.25 / d, 0.25 / d, -0.25 / c_tau], + ], + dtype=np.float64, + ) + + def allocate_thrusts( + self, + total_thrust: float, + tau_x: float, + tau_y: float, + tau_z: float, + ) -> np.ndarray: + """Invert the physical X-quadrotor allocation matrix analytically.""" + d = self.d + c = self.c_tau + t_term = total_thrust * 0.25 + tx_term = tau_x / (4.0 * d) + ty_term = tau_y / (4.0 * d) + tz_term = tau_z / (4.0 * c) if c > 1e-9 else 0.0 + + f1 = t_term - tx_term - ty_term + tz_term + f2 = t_term + tx_term - ty_term - tz_term + f3 = t_term + tx_term + ty_term + tz_term + f4 = t_term - tx_term + ty_term - tz_term + + return np.array([f1, f2, f3, f4], dtype=np.float64) + def _compute_lqr_gain(self) -> np.ndarray: """Formulate linearized state space (A, B) and solve the continuous ARE.""" A = np.zeros((12, 12), dtype=np.float64) @@ -104,9 +182,21 @@ def _compute_lqr_gain(self) -> np.ndarray: P = solve_continuous_are(A, B, self.Q, self.R) R_inv = np.linalg.inv(self.R) K = R_inv @ (B.T @ P) + self.solver_status = "ARE_SOLVED" + self.is_are_solution = True return np.asarray(K, dtype=np.float64) - except Exception: - # Analytical decoupled LQR gain approximation if scipy is unavailable + except Exception as err: + if not self.allow_fallback: + raise RuntimeError( + f"Continuous Algebraic Riccati Equation solver failed: {err}. " + "Exact LQR control requires scipy.linalg.solve_continuous_are. " + "Do not silently call an approximate gain matrix an ARE-derived LQR solution; " + "set allow_fallback=True only if an explicit heuristic approximation is acceptable." + ) from err + + self.solver_status = "UNRESOLVED_APPROXIMATION" + self.is_are_solution = False + # Analytical decoupled gain approximation K_approx = np.zeros((4, 12), dtype=np.float64) # z altitude channel: [z, vz] -> Delta_T K_approx[0, 2] = math.sqrt(self.Q[2, 2] / self.R[0, 0]) @@ -136,7 +226,7 @@ def reset(self) -> None: def compute_action( self, - state: DroneState6DOF, + state_or_obs: DroneState6DOF | np.ndarray, target_position: Optional[np.ndarray] = None, target_velocity: Optional[np.ndarray] = None, target_yaw: float = 0.0, @@ -144,25 +234,76 @@ def compute_action( ) -> Tuple[np.ndarray, Dict[str, Any]]: """Compute optimal control action for the 6-DOF quadrotor. + Accepts either a physical DroneState6DOF object, a 36-D normalized Gymnasium + observation from Drone6DOFEnv, or an unnormalized 13-D state vector. + Returns: (action_in_minus1_plus1, debug_info_dict) """ - target_pos = ( - np.asarray(target_position, dtype=np.float64) - if target_position is not None - else self.target_position - ) + if isinstance(state_or_obs, DroneState6DOF): + pos = state_or_obs.position.copy() + vel = state_or_obs.velocity.copy() + roll, pitch, yaw = quaternion_to_euler_angles(state_or_obs.quaternion) + omega = state_or_obs.angular_velocity.copy() + target_pos = ( + np.asarray(target_position, dtype=np.float64).copy() + if target_position is not None + else self.target_position.copy() + ) + else: + arr = np.asarray(state_or_obs, dtype=np.float64) + if len(arr) == 36: + bx, by, bz = self.bounds + pos = np.array([arr[0] * bx, arr[1] * by, arr[2] * bz], dtype=np.float64) + vel = np.array( + [ + arr[3] * self.max_velocity, + arr[4] * self.max_velocity, + arr[5] * self.max_velocity, + ], + dtype=np.float64, + ) + quat = np.array([arr[6], arr[7], arr[8], arr[9]], dtype=np.float64) + roll, pitch, yaw = quaternion_to_euler_angles(quat) + omega = np.array( + [ + arr[10] * self.max_angular_velocity, + arr[11] * self.max_angular_velocity, + arr[12] * self.max_angular_velocity, + ], + dtype=np.float64, + ) + if target_position is not None: + target_pos = np.asarray(target_position, dtype=np.float64).copy() + else: + # De-normalize target from observation norm_goal indices 13:16 + target_pos = np.array( + [arr[13] * bx, arr[14] * by, arr[15] * bz], dtype=np.float64 + ) + elif len(arr) == 13: + pos = arr[0:3].copy() + vel = arr[3:6].copy() + quat = arr[6:10].copy() + roll, pitch, yaw = quaternion_to_euler_angles(quat) + omega = arr[10:13].copy() + target_pos = ( + np.asarray(target_position, dtype=np.float64).copy() + if target_position is not None + else self.target_position.copy() + ) + else: + raise ValueError( + f"Unsupported state/obs representation with shape {arr.shape}. " + "Expected DroneState6DOF, 36-D normalized Drone6DOFEnv observation, " + "or 13-D unnormalized physical state vector." + ) + target_vel = ( np.asarray(target_velocity, dtype=np.float64) if target_velocity is not None else np.zeros(3, dtype=np.float64) ) - pos = state.position - vel = state.velocity - roll, pitch, yaw = quaternion_to_euler_angles(state.quaternion) - omega = state.angular_velocity - # State error vector (12,) # delta_x = x - x_ref delta_x = np.array( @@ -196,15 +337,8 @@ def compute_action( total_thrust = max(0.0, min(4.0 * self.max_thrust_per_motor, total_thrust)) # Exact analytical mixer inversion for X-quadrotor - d = self.d - c_tau = self.c_tau - t1 = 0.25 * total_thrust - 0.25 * tau_x / d - 0.25 * tau_y / d - 0.25 * tau_z / c_tau - t2 = 0.25 * total_thrust - 0.25 * tau_x / d + 0.25 * tau_y / d + 0.25 * tau_z / c_tau - t3 = 0.25 * total_thrust + 0.25 * tau_x / d + 0.25 * tau_y / d - 0.25 * tau_z / c_tau - t4 = 0.25 * total_thrust + 0.25 * tau_x / d - 0.25 * tau_y / d + 0.25 * tau_z / c_tau - - thrusts = np.array([t1, t2, t3, t4], dtype=np.float64) - thrusts_clamped = np.clip(thrusts, 0.0, self.max_thrust_per_motor) + raw_thrusts = self.allocate_thrusts(total_thrust, tau_x, tau_y, tau_z) + thrusts_clamped = np.clip(raw_thrusts, 0.0, self.max_thrust_per_motor) # Normalize to Gym action space [-1, 1] action = np.clip( @@ -218,29 +352,24 @@ def compute_action( "tau_y": tau_y, "tau_z": tau_z, "motor_thrusts": thrusts_clamped, + "raw_motor_thrusts": raw_thrusts, "position_error": float(np.linalg.norm(target_pos - pos)), + "solver_status": self.solver_status, + "is_are_solution": self.is_are_solution, } return action, info def predict( self, - obs: np.ndarray, + observation: np.ndarray | DroneState6DOF, + state: Optional[Any] = None, + episode_start: Optional[Any] = None, deterministic: bool = True, ) -> Tuple[np.ndarray, Optional[Dict[str, Any]]]: - """Gym-compatible interface matching RL policy predict signature.""" - from adaptive_rl.environments.drone_6dof import DroneState6DOF - - pos = obs[0:3] - vel = obs[3:6] - quat = obs[6:10] - omega = obs[10:13] - target = obs[13:16] - - state = DroneState6DOF( - position=np.array(pos, dtype=np.float64), - velocity=np.array(vel, dtype=np.float64), - quaternion=np.array(quat, dtype=np.float64), - angular_velocity=np.array(omega, dtype=np.float64), - ) - action, info = self.compute_action(state, target_position=target) + """Gym-compatible interface matching RL policy predict signature. + + Correctly decodes normalized 36-D observation from Drone6DOFEnv or accepts DroneState6DOF. + Rejects unsupported formats with an explicit ValueError. + """ + action, info = self.compute_action(observation) return action, info diff --git a/src/adaptive_rl/environments/drone_6dof.py b/src/adaptive_rl/environments/drone_6dof.py index 188081e..8d5dc7f 100644 --- a/src/adaptive_rl/environments/drone_6dof.py +++ b/src/adaptive_rl/environments/drone_6dof.py @@ -317,6 +317,7 @@ def __init__( motor_time_constant: float = 0.03, wind_vector: Optional[np.ndarray | Sequence[float]] = None, enable_ground_effect: bool = False, + quadratic_drag_coeff: float = 0.0, ) -> None: for name, val in [ ("mass", mass), @@ -383,6 +384,7 @@ def __init__( # Simplified Cheeseman-Bennett ground effect model self.enable_ground_effect = bool(enable_ground_effect) + self.quadratic_drag_coeff = max(0.0, float(quadratic_drag_coeff)) self.state = DroneState6DOF( position=np.zeros(3, dtype=np.float64), @@ -538,6 +540,10 @@ def _state_derivatives( gravity_world = np.array([0.0, 0.0, -self.mass * self.gravity], dtype=np.float64) v_rel = vel - self.wind_vector drag_force = -self.linear_damping * v_rel + if self.quadratic_drag_coeff > 0.0: + v_rel_speed = float(np.linalg.norm(v_rel)) + if v_rel_speed > 1e-12: + drag_force -= self.quadratic_drag_coeff * v_rel_speed * v_rel d_vel = (gravity_world + thrust_world + drag_force) / self.mass # 3. Quaternion derivative: dq/dt = 0.5 * q (x) [0, omega] @@ -562,11 +568,14 @@ def step_rk4(self, motor_thrusts: np.ndarray) -> DroneState6DOF: omega = self.state.angular_velocity.copy() for _ in range(self.substeps): - # Integrate actuator first-order response if enabled - if self.enable_motor_dynamics: - d_thrust = (h / self.motor_time_constant) * (f_cmd - self.actual_motor_thrusts) + # Integrate actuator first-order response if enabled (exact analytical step response) + if self.enable_motor_dynamics and self.motor_time_constant > 1e-9: + alpha = math.exp(-h / self.motor_time_constant) + self.actual_motor_thrusts = self.actual_motor_thrusts * alpha + f_cmd * ( + 1.0 - alpha + ) self.actual_motor_thrusts = np.clip( - self.actual_motor_thrusts + d_thrust, 0.0, self.max_thrust_per_motor + self.actual_motor_thrusts, 0.0, self.max_thrust_per_motor ) else: self.actual_motor_thrusts = np.clip(f_cmd, 0.0, self.max_thrust_per_motor) @@ -678,6 +687,13 @@ def __init__( body_frame_lidar: bool = False, render_mode: Optional[str] = None, split: Optional[str] = None, + enable_motor_dynamics: bool = False, + motor_time_constant: float = 0.03, + wind_vector: Optional[np.ndarray | Sequence[float]] = None, + enable_ground_effect: bool = False, + quadratic_drag_coeff: float = 0.0, + randomize_domain: bool = False, + domain_randomizer: Optional[Any] = None, ) -> None: super().__init__() @@ -838,6 +854,29 @@ def __init__( self.max_velocity = 10.0 # normalization scale for velocity self.max_angular_velocity = 10.0 # normalization scale for angular velocity + self.enable_motor_dynamics = bool(enable_motor_dynamics) + self.motor_time_constant = float(motor_time_constant) + self.wind_vector = ( + np.asarray(wind_vector, dtype=np.float64) + if wind_vector is not None + else np.zeros(3, dtype=np.float64) + ) + self.enable_ground_effect = bool(enable_ground_effect) + self.quadratic_drag_coeff = max(0.0, float(quadratic_drag_coeff)) + self.randomize_domain = bool(randomize_domain) + self.domain_randomizer = domain_randomizer + self._last_randomized_parameters: Optional[Dict[str, Any]] = None + + self.nominal_mass: float = float(mass) + self.nominal_inertia: Optional[Tuple[float, float, float]] = inertia + self.nominal_max_thrust: float = float(max_thrust_per_motor) + self.nominal_c_tau: float = float(torque_to_thrust_ratio) + self.nominal_linear_damping: float = float(linear_damping) + self.nominal_angular_damping: float = float(angular_damping) + self.nominal_motor_time_constant: float = float(motor_time_constant) + self.nominal_wind_vector: np.ndarray = self.wind_vector.copy() + self.nominal_quadratic_drag: float = self.quadratic_drag_coeff + self.dynamics = DroneDynamics6DOF( mass=mass, arm_length=arm_length, @@ -849,6 +888,11 @@ def __init__( angular_damping=angular_damping, dt=dt, substeps=substeps, + enable_motor_dynamics=self.enable_motor_dynamics, + motor_time_constant=self.motor_time_constant, + wind_vector=self.wind_vector, + enable_ground_effect=self.enable_ground_effect, + quadratic_drag_coeff=self.quadratic_drag_coeff, ) # Action space: 4 motor commands in [-1, 1], mapped to [0, max_thrust] @@ -1044,6 +1088,8 @@ def _get_info(self) -> Dict[str, Any]: if current_split is not None: info["split"] = current_split info["split_seed"] = self._last_split_seed + if self._last_randomized_parameters is not None: + info["randomized_parameters"] = self._last_randomized_parameters return info @property @@ -1153,6 +1199,32 @@ def reset( self.dynamics.reset(position=self._position) + # Domain randomization if enabled + if self.randomize_domain: + self.dynamics.mass = self.nominal_mass + if self.nominal_inertia is not None: + self.dynamics.inertia_diag = np.array(self.nominal_inertia, dtype=np.float64) + self.dynamics.inv_inertia = 1.0 / self.dynamics.inertia_diag + self.dynamics.max_thrust_per_motor = self.nominal_max_thrust + self.dynamics.c_tau = self.nominal_c_tau + self.dynamics.linear_damping = self.nominal_linear_damping + self.dynamics.angular_damping = self.nominal_angular_damping + self.dynamics.wind_vector = self.nominal_wind_vector.copy() + self.dynamics.motor_time_constant = self.nominal_motor_time_constant + self.dynamics.hover_thrust_per_motor = ( + self.dynamics.mass * self.dynamics.gravity + ) / 4.0 + + if self.domain_randomizer is None: + from adaptive_rl.environments.randomization import DomainRandomizer + + self.domain_randomizer = DomainRandomizer(seed=effective_seed) + params = self.domain_randomizer.sample_parameters( + nominal_dynamics=self.dynamics, seed=effective_seed + ) + self.domain_randomizer.apply_to_dynamics(self.dynamics, params) + self._last_randomized_parameters = params + self._obstacles = generate_drone_obstacles( bounds=self.bounds, start_pos=self._position, diff --git a/src/adaptive_rl/flight_testing/recorder.py b/src/adaptive_rl/flight_testing/recorder.py index 569197e..c906cdb 100644 --- a/src/adaptive_rl/flight_testing/recorder.py +++ b/src/adaptive_rl/flight_testing/recorder.py @@ -9,10 +9,15 @@ The SHA-256 digests generated here (config_fingerprint and record_digest) provide mathematical integrity comparison and tamper detection. They detect accidental corruption or unauthorized modification of simulation parameters between execution and analysis. -They do NOT provide non-repudiation or authenticated identity against an active attacker -who possesses write permissions to both the record data and digest calculation logic. -For strict adversarial non-repudiation, an optional cryptographic HMAC/digital signature -field is provided to attach an asymmetric key signature. +A SHA-256 digest is an unkeyed cryptographic checksum that verifies data integrity, +NOT identity or origin authenticity. It does NOT provide non-repudiation or authenticated +identity against an active attacker who possesses write permissions to both the record data +and digest calculation logic. + +In contrast, a digital signature (or HMAC) requires an authenticated cryptographic key +(such as an asymmetric private key or pre-shared HMAC secret). The optional `signature` +field in FlightRecord is reserved strictly for authenticated cryptographic signatures +(e.g., ECDSA, Ed25519, or HMAC-SHA256) and must never be conflated with plain SHA-256 digests. """ from __future__ import annotations @@ -172,6 +177,8 @@ class FlightRecord: checkpoint_hash: Optional[str] random_seed: Optional[int] scenario_metrics: List[FlightScenarioMetrics] + trajectories: Dict[str, List[Dict[str, Any]]] = field(default_factory=dict) + configuration: Optional[ExperimentConfigurationFingerprint] = None aggregate_summary: Dict[str, Any] = field(default_factory=dict) signature: Optional[str] = None # Optional asymmetric HMAC / digital signature @@ -186,16 +193,22 @@ def to_dict(self, include_digest: bool = True) -> Dict[str, Any]: "checkpoint_hash": self.checkpoint_hash, "random_seed": self.random_seed, "scenario_metrics": [m.to_dict() for m in self.scenario_metrics], + "trajectories": self.trajectories, "aggregate_summary": self.aggregate_summary, "signature": self.signature, } + if self.configuration is not None: + data["configuration"] = self.configuration.to_dict() if include_digest: data["record_digest"] = self.record_digest return data def verify_config_fingerprint(self) -> bool: - """Verify that the stored config_fingerprint matches the actual physics configuration.""" - expected = compute_config_fingerprint(self.physics.to_dict()) + """Verify that the stored config_fingerprint matches the configuration.""" + if self.configuration is not None: + expected = compute_config_fingerprint(self.configuration.to_dict()) + else: + expected = compute_config_fingerprint(self.physics.to_dict()) return self.config_fingerprint == expected def verify_fingerprint(self) -> bool: @@ -225,6 +238,8 @@ def create_record( checkpoint_path: Optional[Path | str] = None, random_seed: Optional[int] = None, signature: Optional[str] = None, + configuration: Optional[ExperimentConfigurationFingerprint | Dict[str, Any]] = None, + trajectories: Optional[Dict[str, List[Dict[str, Any]]]] = None, ) -> FlightRecord: """Create a cryptographically fingerprinted FlightRecord from execution data.""" from datetime import datetime, timezone @@ -252,7 +267,21 @@ def create_record( lidar_range=float(physics_params.get("lidar_range", 20.0)), ) - config_hash = compute_config_fingerprint(physics.to_dict()) + # Extract metrics and trajectories + metrics_list: List[FlightScenarioMetrics] = [] + collected_trajectories: Dict[str, List[Dict[str, Any]]] = {} + for r in scenario_results: + if isinstance(r, FlightScenarioResult): + metrics_list.append(r.metrics) + collected_trajectories[r.scenario_name] = [step.to_dict() for step in r.telemetry] + elif isinstance(r, FlightScenarioMetrics): + metrics_list.append(r) + collected_trajectories[r.scenario_name] = [] + else: + raise ValueError(f"Unsupported result type: {type(r)}") + + if trajectories is not None: + collected_trajectories.update(trajectories) # Checkpoint hash chk_hash: Optional[str] = None @@ -264,6 +293,33 @@ def create_record( else: chk_hash = "not_found" + # Build comprehensive ExperimentConfigurationFingerprint + if configuration is not None: + if isinstance(configuration, ExperimentConfigurationFingerprint): + exp_config = configuration + elif isinstance(configuration, dict): + exp_config = ExperimentConfigurationFingerprint(**configuration) + else: + raise ValueError(f"Invalid configuration type: {type(configuration)}") + else: + exp_config = ExperimentConfigurationFingerprint( + physics=physics, + environment_bounds=list( + physics_params.get("environment_bounds", [30.0, 30.0, 15.0]) + ), + algorithm_name=algorithm_name, + checkpoint_hash=chk_hash, + random_seed=random_seed, + scenario_names=sorted([m.scenario_name for m in metrics_list]), + controller_type=str( + physics_params.get("controller_type", "QuadrotorPIDController") + ), + controller_gains=dict(physics_params.get("controller_gains", {})), + extra_parameters=dict(physics_params.get("extra_parameters", {})), + ) + + config_hash = compute_config_fingerprint(exp_config.to_dict()) + # Software and host provenance git_meta = collect_git_metadata() host_sys = collect_host_system() @@ -284,16 +340,6 @@ def create_record( gpu_name=hw.gpu_name, ) - # Extract metrics - metrics_list: List[FlightScenarioMetrics] = [] - for r in scenario_results: - if isinstance(r, FlightScenarioResult): - metrics_list.append(r.metrics) - elif isinstance(r, FlightScenarioMetrics): - metrics_list.append(r) - else: - raise ValueError(f"Unsupported result type: {type(r)}") - # Aggregate summary total_scenarios = len(metrics_list) success_count = sum(1 for m in metrics_list if m.success) @@ -333,6 +379,8 @@ def create_record( "checkpoint_hash": chk_hash, "random_seed": random_seed, "scenario_metrics": [m.to_dict() for m in metrics_list], + "trajectories": collected_trajectories, + "configuration": exp_config.to_dict(), "aggregate_summary": agg_summary, "signature": signature, } @@ -349,6 +397,8 @@ def create_record( checkpoint_hash=chk_hash, random_seed=random_seed, scenario_metrics=metrics_list, + trajectories=collected_trajectories, + configuration=exp_config, aggregate_summary=agg_summary, signature=signature, ) @@ -449,6 +499,32 @@ def load_record_json(filepath: Path | str) -> FlightRecord: for m in data["scenario_metrics"]: metrics_list.append(FlightScenarioMetrics(**m)) + trajectories = data.get("trajectories", {}) + + cfg_data = data.get("configuration") + if cfg_data: + # Synchronize cfg_physics with physics from top-level to detect if either was tampered with + configuration = ExperimentConfigurationFingerprint( + physics=physics, + environment_bounds=list(cfg_data.get("environment_bounds", [30.0, 30.0, 15.0])), + algorithm_name=str(cfg_data.get("algorithm_name", "pid_reference")), + checkpoint_hash=cfg_data.get("checkpoint_hash"), + random_seed=cfg_data.get("random_seed"), + scenario_names=list(cfg_data.get("scenario_names", [])), + controller_type=str(cfg_data.get("controller_type", "QuadrotorPIDController")), + controller_gains=dict(cfg_data.get("controller_gains", {})), + extra_parameters=dict(cfg_data.get("extra_parameters", {})), + ) + else: + configuration = ExperimentConfigurationFingerprint( + physics=physics, + environment_bounds=list(data.get("environment_bounds", [30.0, 30.0, 15.0])), + algorithm_name=str(data.get("algorithm_name", "pid_reference")), + checkpoint_hash=data.get("checkpoint_hash"), + random_seed=data.get("random_seed"), + scenario_names=sorted([m.scenario_name for m in metrics_list]), + ) + record = FlightRecord( experiment_id=data["experiment_id"], timestamp_utc=data["timestamp_utc"], @@ -460,6 +536,8 @@ def load_record_json(filepath: Path | str) -> FlightRecord: checkpoint_hash=data.get("checkpoint_hash"), random_seed=data.get("random_seed"), scenario_metrics=metrics_list, + trajectories=trajectories, + configuration=configuration, aggregate_summary=data.get("aggregate_summary", {}), signature=data.get("signature"), ) @@ -467,7 +545,7 @@ def load_record_json(filepath: Path | str) -> FlightRecord: if not record.verify_config_fingerprint(): raise ValueError( f"Config fingerprint mismatch in '{path.name}'. Stored fingerprint '{record.config_fingerprint}' " - f"does not match recomputed physics fingerprint '{compute_config_fingerprint(record.physics.to_dict())}'." + f"does not match recomputed configuration fingerprint." ) return record diff --git a/src/adaptive_rl/flight_testing/scenarios.py b/src/adaptive_rl/flight_testing/scenarios.py index 67fba2a..abc6380 100644 --- a/src/adaptive_rl/flight_testing/scenarios.py +++ b/src/adaptive_rl/flight_testing/scenarios.py @@ -55,6 +55,7 @@ quaternion_to_euler_angles, ) from adaptive_rl.navigation.dynamic_obstacles import DynamicObstacleSphere3D +from adaptive_rl.navigation.mission import MissionManager, MissionState from adaptive_rl.navigation.planner import AStarPlanner3D logger = logging.getLogger(__name__) @@ -80,6 +81,10 @@ class FlightStepTelemetry: is_success: bool commanded_thrusts: Optional[np.ndarray] = None actuator_fault_active: bool = False + estimated_position: Optional[np.ndarray] = None + estimated_velocity: Optional[np.ndarray] = None + position_estimation_error: Optional[float] = None + attitude_estimation_error_deg: Optional[float] = None def to_dict(self) -> Dict[str, Any]: cmd_thrusts = ( @@ -87,7 +92,7 @@ def to_dict(self) -> Dict[str, Any]: if self.commanded_thrusts is not None else [round(float(x), 4) for x in self.motor_thrusts] ) - return { + d: Dict[str, Any] = { "step": self.step, "time_sec": round(self.time_sec, 4), "position": [round(float(x), 4) for x in self.position], @@ -110,6 +115,15 @@ def to_dict(self) -> Dict[str, Any]: "is_collision": self.is_collision, "is_success": self.is_success, } + if self.estimated_position is not None: + d["estimated_position"] = [round(float(x), 4) for x in self.estimated_position] + if self.estimated_velocity is not None: + d["estimated_velocity"] = [round(float(x), 4) for x in self.estimated_velocity] + if self.position_estimation_error is not None: + d["position_estimation_error"] = round(float(self.position_estimation_error), 4) + if self.attitude_estimation_error_deg is not None: + d["attitude_estimation_error_deg"] = round(float(self.attitude_estimation_error_deg), 3) + return d @dataclass @@ -282,6 +296,12 @@ class FlightScenarioDefinition: collision_radius: float = 0.8 step_duration_sec: float = 0.05 use_path_planner: bool = False + settling_velocity_threshold: float = 0.5 + max_recovery_tilt_deg: float = 35.0 + min_recovery_altitude: float = 0.8 + enable_sensor_estimation: bool = False + sensor_seed: Optional[int] = None + max_duration_seconds: Optional[float] = None disturbance_fn: Optional[Callable[[int, DroneState6DOF], np.ndarray]] = None actuator_fault_fn: Optional[Callable[[int, np.ndarray], np.ndarray]] = None @@ -672,9 +692,10 @@ def run_scenario( self.env._position = scenario.initial_position.copy() self.env._current_step = 0 - # Resolve waypoints (optionally via 3D A* path planning) + # Resolve waypoints (optionally via 3D A* path planning or MissionManager) waypoints: List[np.ndarray] = [] - if scenario.use_path_planner and scenario.obstacles: + mission_manager: Optional[MissionManager] = None + if scenario.use_path_planner and (scenario.obstacles or scenario.dynamic_obstacles): planner = AStarPlanner3D( bounds=scenario.bounds, resolution=1.0, @@ -682,10 +703,20 @@ def run_scenario( safety_margin=0.3, ) raw_target = scenario.waypoints[-1] if scenario.waypoints else scenario.initial_position - plan_res = planner.plan(scenario.initial_position, raw_target, scenario.obstacles) - if plan_res.status == "SUCCESS" and len(plan_res.waypoints) > 1: - waypoints = plan_res.waypoints[1:] # sequenced targets ahead - else: + mission_manager = MissionManager( + planner=planner, + waypoint_acceptance_radius=scenario.target_radius, + replan_clearance_threshold=scenario.collision_radius + 0.5, + ) + targets = scenario.waypoints if scenario.waypoints else [scenario.initial_position] + ok = mission_manager.start_mission( + start_position=scenario.initial_position, + goals=targets, + static_obstacles=scenario.obstacles, + dynamic_obstacles=scenario.dynamic_obstacles, + time_sec=0.0, + ) + if not ok or mission_manager.state == MissionState.FAILED_BLOCKED: # Planning failure (e.g. goal or start blocked) elapsed_ms = (time.perf_counter() - start_wall_time) * 1000.0 initial_goal_dist = float(np.linalg.norm(raw_target - scenario.initial_position)) @@ -708,7 +739,7 @@ def run_scenario( max_angular_speed=0.0, initial_goal_distance=initial_goal_dist, final_goal_distance=initial_goal_dist, - min_obstacle_clearance=plan_res.min_clearance, + min_obstacle_clearance=-1.0, collision_count=0, control_effort=0.0, execution_wall_time_ms=elapsed_ms, @@ -719,8 +750,9 @@ def run_scenario( metrics=metrics, telemetry=[], ) + waypoints = [w.copy() for w in mission_manager.active_path] else: - waypoints = list(scenario.waypoints) + waypoints = [w.copy() for w in scenario.waypoints] if not waypoints: waypoints = [scenario.initial_position.copy()] @@ -732,6 +764,16 @@ def run_scenario( initial_goal_dist = float(np.linalg.norm(waypoints[-1] - scenario.initial_position)) + # Sensor estimation avionics in-the-loop if enabled + imu_sensor = None + state_estimator = None + if scenario.enable_sensor_estimation: + from adaptive_rl.sensors.estimation import ComplementaryFilter + from adaptive_rl.sensors.imu import IMUSensor + + imu_sensor = IMUSensor(seed=scenario.sensor_seed or 42) + state_estimator = ComplementaryFilter() + telemetry: List[FlightStepTelemetry] = [] pos_errors: List[float] = [] vel_errors: List[float] = [] @@ -741,15 +783,29 @@ def run_scenario( control_efforts: List[float] = [] is_collision = False + coll_type = "none" outcome = "max_steps" step = 0 dt = self.env.dynamics.dt + fault_ever_active = False while step < scenario.max_steps: step += 1 time_sec = step * dt current_state = self.env.dynamics.state + # Predictive dynamic obstacle avoidance via MissionManager + if mission_manager is not None: + current_target, _ = mission_manager.update( + drone_state=current_state, + time_sec=time_sec, + static_obstacles=scenario.obstacles, + dynamic_obstacles=scenario.dynamic_obstacles, + ) + self.env._goal = current_target.copy() + if hasattr(ctrl, "set_target"): + ctrl.set_target(current_target) + # Compute tracking errors relative to current waypoint pos_err = current_target - current_state.position pos_err_mag = float(np.linalg.norm(pos_err)) @@ -795,6 +851,8 @@ def run_scenario( if scenario.actuator_fault_fn is not None: thrusts_actual = scenario.actuator_fault_fn(step, thrusts_cmd) fault_active = not np.allclose(thrusts_actual, thrusts_cmd) + if fault_active: + fault_ever_active = True # Step physical dynamics with Runge-Kutta 4 old_pos = current_state.position.copy() @@ -808,6 +866,20 @@ def run_scenario( self.env._position = new_state.position.copy() self.env._current_step = step + # Sensor in-the-loop estimation if enabled + est_pos = None + est_vel = None + pos_est_err = None + att_est_err = None + if imu_sensor is not None and state_estimator is not None: + inertial_acc = (new_state.velocity - current_state.velocity) / dt + meas = imu_sensor.measure(current_state, inertial_acceleration=inertial_acc, dt=dt) + est_state = state_estimator.update(meas, dt=dt, true_state=new_state) + est_pos = est_state.position.copy() + est_vel = est_state.velocity.copy() + pos_est_err = est_state.position_error + att_est_err = est_state.attitude_error_deg + # Obstacle clearance check (combining static and dynamic obstacles) min_obs_d = float("inf") for obstacle in scenario.obstacles: @@ -836,25 +908,35 @@ def run_scenario( is_collision = True outcome = "collision" if coll_type == "obstacle" else "boundary_breach" - # Waypoint sequencing + # Waypoint sequencing for static waypoint list dist_to_curr_waypoint = float(np.linalg.norm(current_target - new_state.position)) is_at_curr_waypoint = dist_to_curr_waypoint <= scenario.target_radius - if is_at_curr_waypoint and waypoint_idx < len(waypoints) - 1: + if ( + mission_manager is None + and is_at_curr_waypoint + and waypoint_idx < len(waypoints) - 1 + ): waypoint_idx += 1 current_target = waypoints[waypoint_idx].copy() self.env._goal = current_target.copy() if hasattr(ctrl, "set_target"): ctrl.set_target(current_target) - # Final success check - dist_to_final_waypoint = float(np.linalg.norm(waypoints[-1] - new_state.position)) - is_final_success = ( - waypoint_idx == len(waypoints) - 1 - and dist_to_final_waypoint <= scenario.target_radius - and not is_collision + # Final arrival and settling check + dist_to_final = float(np.linalg.norm(waypoints[-1] - new_state.position)) + curr_speed = float(np.linalg.norm(new_state.velocity)) + is_pos_settled = dist_to_final <= scenario.target_radius + is_vel_settled = curr_speed <= scenario.settling_velocity_threshold + + is_mission_completed = ( + (mission_manager.state == MissionState.ARRIVED) + if mission_manager is not None + else (waypoint_idx == len(waypoints) - 1 and is_pos_settled) ) + is_final_success = is_mission_completed and is_vel_settled and not is_collision + # Record telemetry telemetry.append( FlightStepTelemetry( @@ -874,6 +956,10 @@ def run_scenario( is_success=is_final_success, commanded_thrusts=thrusts_cmd.copy(), actuator_fault_active=fault_active, + estimated_position=est_pos, + estimated_velocity=est_vel, + position_estimation_error=pos_est_err, + attitude_estimation_error_deg=att_est_err, ) ) @@ -888,19 +974,45 @@ def run_scenario( break final_pos_err = float(np.linalg.norm(waypoints[-1] - self.env._position)) - final_goal_dist = final_pos_err + final_speed = float(np.linalg.norm(self.env.dynamics.state.velocity)) + max_tilt = max(att_errors_deg) if att_errors_deg else 0.0 + min_alt = ( + float(np.min([t.position[2] for t in telemetry])) + if telemetry + else float(self.env._position[2]) + ) - # Disambiguated outcome classification - if scenario.actuator_fault_fn is not None: - max_tilt = max(att_errors_deg) if att_errors_deg else 0.0 - if is_collision or max_tilt >= 35.0 or self.env._position[2] <= 0.8: - outcome = "crash_fault" - else: - outcome = "recovered_degraded" + is_pos_settled = final_pos_err <= scenario.target_radius + is_vel_settled = final_speed <= scenario.settling_velocity_threshold + + # Authoritative, mutually exclusive outcome classification + if fault_ever_active and ( + is_collision + or max_tilt >= scenario.max_recovery_tilt_deg + or min_alt <= scenario.min_recovery_altitude + ): + outcome = "crash_fault" elif is_collision: outcome = "collision" if coll_type == "obstacle" else "boundary_breach" - elif final_pos_err <= scenario.target_radius: + elif fault_ever_active: + # Active actuator fault was injected: + # Requires mission completion, stable flight, altitude maintenance, and velocity settling + if ( + is_pos_settled + and is_vel_settled + and max_tilt < scenario.max_recovery_tilt_deg + and min_alt > scenario.min_recovery_altitude + ): + outcome = "recovered_degraded" + else: + outcome = "max_steps" + elif is_pos_settled and is_vel_settled and not is_collision: outcome = "success" + elif ( + scenario.max_duration_seconds is not None + and (step * dt) >= scenario.max_duration_seconds + ): + outcome = "timeout" else: outcome = "max_steps" @@ -928,7 +1040,7 @@ def run_scenario( mean_angular_speed=float(np.mean(angular_speeds)) if angular_speeds else 0.0, max_angular_speed=float(np.max(angular_speeds)) if angular_speeds else 0.0, initial_goal_distance=initial_goal_dist, - final_goal_distance=final_goal_dist, + final_goal_distance=final_pos_err, min_obstacle_clearance=float(min(min_clearances)) if min_clearances else float("inf"), collision_count=1 if is_collision else 0, control_effort=float(np.sum(control_efforts)) if control_efforts else 0.0, diff --git a/src/adaptive_rl/navigation/dynamic_obstacles.py b/src/adaptive_rl/navigation/dynamic_obstacles.py index 6ef3158..729c2ee 100644 --- a/src/adaptive_rl/navigation/dynamic_obstacles.py +++ b/src/adaptive_rl/navigation/dynamic_obstacles.py @@ -8,7 +8,7 @@ import math from dataclasses import dataclass, field -from typing import Any, Dict, Tuple +from typing import Any, Dict, List, Tuple import numpy as np @@ -71,6 +71,32 @@ def as_static_obstacle_at(self, time_sec: float) -> ObstacleSphere3D: """Export an instantaneous ObstacleSphere3D at a specific timestamp.""" return ObstacleSphere3D(center=self.center_at(time_sec), radius=self.radius) + def predict_trajectory( + self, + t_start: float, + horizon_sec: float = 3.0, + dt: float = 0.5, + ) -> List[Tuple[float, np.ndarray]]: + """Predict the obstacle center trajectory over a future time horizon [t_start, t_start + horizon].""" + traj: List[Tuple[float, np.ndarray]] = [] + steps = max(1, int(round(horizon_sec / dt))) + for i in range(steps + 1): + t = t_start + i * dt + traj.append((t, self.center_at(t))) + return traj + + def predict_occupied_spheres( + self, + t_start: float, + horizon_sec: float = 3.0, + dt: float = 0.5, + ) -> List[ObstacleSphere3D]: + """Generate static spherical obstacle approximations covering the predicted trajectory horizon.""" + return [ + ObstacleSphere3D(center=pt, radius=self.radius) + for _, pt in self.predict_trajectory(t_start, horizon_sec, dt) + ] + def to_dict(self) -> Dict[str, Any]: return { "center_initial": [round(float(x), 4) for x in self.center_initial], diff --git a/src/adaptive_rl/navigation/mission.py b/src/adaptive_rl/navigation/mission.py index ffa40ed..6e807ae 100644 --- a/src/adaptive_rl/navigation/mission.py +++ b/src/adaptive_rl/navigation/mission.py @@ -104,9 +104,11 @@ def start_mission( self.min_clearance_observed = float("inf") self.state = MissionState.PLANNING - # Collect current obstacle set + # Collect current obstacle set including horizon-predicted dynamic obstacles dyn_obs = dynamic_obstacles or [] - current_obs = list(static_obstacles) + [d.as_static_obstacle_at(time_sec) for d in dyn_obs] + current_obs = list(static_obstacles) + for d in dyn_obs: + current_obs.extend(d.predict_occupied_spheres(time_sec, horizon_sec=3.0, dt=0.5)) # Plan through each high-level mission waypoint full_path: List[np.ndarray] = [self.home_position.copy()] @@ -160,25 +162,34 @@ def update( dist_to_target = float(np.linalg.norm(current_target - pos)) - # Check if current path segment ahead is blocked by dynamic obstacle + # Check if current path segment ahead is blocked by dynamic obstacle in near future if self.active_path_idx < len(self.active_path): - segment_clear, _ = self.planner.is_segment_valid(pos, current_target, current_obs) + predicted_dyn_obs: List[ObstacleSphere3D] = [] + for d in dyn_obs: + predicted_dyn_obs.extend( + d.predict_occupied_spheres(time_sec, horizon_sec=2.5, dt=0.5) + ) + eval_obs = list(static_obstacles) + predicted_dyn_obs + + segment_clear, _ = self.planner.is_segment_valid(pos, current_target, eval_obs) if not segment_clear: logger.info( - "Dynamic obstacle encroaching active path at t=%.2fs! Replanning...", time_sec + "Dynamic obstacle predicted in flight corridor at t=%.2fs! Replanning...", + time_sec, ) self.state = MissionState.REPLANNING self.replan_count += 1 # Replan from current position to the destination dest = self.mission_waypoints[-1] if self.mission_waypoints else current_target - replan_res = self.planner.plan(pos, dest, current_obs) + replan_res = self.planner.plan(pos, dest, eval_obs) if replan_res.status == "SUCCESS" and replan_res.waypoints: self.active_path = replan_res.waypoints self.active_path_idx = 1 if len(replan_res.waypoints) > 1 else 0 current_target = self.active_path[self.active_path_idx] self.state = MissionState.EXECUTING else: - self.state = MissionState.FAILED_BLOCKED + # Safe stop in place (hover hold) while dynamic obstacle clears the corridor + current_target = pos.copy() return current_target, self.get_telemetry(drone_state) # Advance waypoint if reached diff --git a/src/adaptive_rl/navigation/planner.py b/src/adaptive_rl/navigation/planner.py index 45af57c..431588b 100644 --- a/src/adaptive_rl/navigation/planner.py +++ b/src/adaptive_rl/navigation/planner.py @@ -198,9 +198,17 @@ def simplify_path( self, waypoints: List[np.ndarray], obstacles: Sequence[ObstacleSphere3D], - ) -> List[np.ndarray]: - """Greedy line-of-sight shortcutting (string pulling) with strict collision revalidation.""" + ) -> Optional[List[np.ndarray]]: + """Greedy line-of-sight shortcutting (string pulling) with strict collision revalidation. + + Returns None if no safe continuation exists between waypoints, ensuring an unvalidated + segment is never appended. + """ if len(waypoints) <= 2: + if len(waypoints) == 2: + valid, _ = self.is_segment_valid(waypoints[0], waypoints[1], obstacles) + if not valid: + return None return [w.copy() for w in waypoints] simplified = [waypoints[0].copy()] @@ -208,8 +216,7 @@ def simplify_path( n = len(waypoints) while curr_idx < n - 1: - # Look ahead as far as possible - furthest_valid_idx = curr_idx + 1 + furthest_valid_idx: Optional[int] = None for next_idx in range(n - 1, curr_idx, -1): valid, _ = self.is_segment_valid( waypoints[curr_idx], waypoints[next_idx], obstacles @@ -217,9 +224,20 @@ def simplify_path( if valid: furthest_valid_idx = next_idx break + + if furthest_valid_idx is None: + # No collision-free continuation exists from curr_idx; do not append unvalidated segment + return None + simplified.append(waypoints[furthest_valid_idx].copy()) curr_idx = furthest_valid_idx + # Invariant check: every consecutive segment must pass continuous collision validation + for i in range(len(simplified) - 1): + valid, _ = self.is_segment_valid(simplified[i], simplified[i + 1], obstacles) + if not valid: + return None + return simplified def plan( @@ -323,8 +341,13 @@ def to_world(coord: Tuple[int, int, int]) -> np.ndarray: continue neighbor_world = to_world(neighbor) - valid, _ = self.is_point_valid(neighbor_world, obstacles) - if not valid: + valid_pt, _ = self.is_point_valid(neighbor_world, obstacles) + if not valid_pt: + continue + + # Continuous segment check to prevent diagonal corner-cutting + valid_seg, _ = self.is_segment_valid(curr_world, neighbor_world, obstacles) + if not valid_seg: continue step_cost = math.sqrt(offset[0] ** 2 + offset[1] ** 2 + offset[2] ** 2) * res @@ -360,6 +383,23 @@ def to_world(coord: Tuple[int, int, int]) -> np.ndarray: # 6. Apply line-of-sight shortcutting simplified = self.simplify_path(raw_path, obstacles) + if simplified is None: + # Fall back to raw grid path only if every raw segment is provably collision-free + raw_valid = True + for i in range(len(raw_path) - 1): + v_seg, _ = self.is_segment_valid(raw_path[i], raw_path[i + 1], obstacles) + if not v_seg: + raw_valid = False + break + if raw_valid: + simplified = raw_path + else: + elapsed = (time.perf_counter() - t0) * 1000.0 + return PlanningResult( + status="NO_PATH", + planning_time_ms=elapsed, + nodes_expanded=expansions, + ) # Compute simplified metrics simp_len = sum( diff --git a/tests/test_actuator_aerodynamics.py b/tests/test_actuator_aerodynamics.py index 80720e5..ce1aa3b 100644 --- a/tests/test_actuator_aerodynamics.py +++ b/tests/test_actuator_aerodynamics.py @@ -6,14 +6,15 @@ import numpy as np -from adaptive_rl.environments.drone_6dof import DroneDynamics6DOF +from adaptive_rl.environments.drone_6dof import Drone6DOFEnv, DroneDynamics6DOF def test_motor_first_order_dynamics_response_lag() -> None: - """With enable_motor_dynamics=True, motor thrust follows first-order lag with time constant tau.""" + """With enable_motor_dynamics=True, motor thrust follows exact analytical first-order lag with time constant tau.""" tau = 0.04 # 40ms motor response time constant + dt = 0.01 dyn = DroneDynamics6DOF( - dt=0.01, + dt=dt, substeps=1, enable_motor_dynamics=True, motor_time_constant=tau, @@ -28,10 +29,10 @@ def test_motor_first_order_dynamics_response_lag() -> None: cmd = np.array([5.0, 5.0, 5.0, 5.0]) dyn.step_rk4(cmd) - # After dt=0.01s (t/tau = 0.01 / 0.04 = 0.25): - # Expected actual thrust: T(0) + (dt / tau) * (T_cmd - T(0)) = hover_t + 0.25 * (5.0 - hover_t) - expected_first_step = hover_t + (0.01 / tau) * (5.0 - hover_t) - np.testing.assert_allclose(dyn.actual_motor_thrusts, [expected_first_step] * 4, rtol=1e-3) + # Exact analytical step response: f(t+h) = f(t)*exp(-h/tau) + f_cmd*(1 - exp(-h/tau)) + alpha = math.exp(-dt / tau) + expected_first_step = hover_t * alpha + 5.0 * (1.0 - alpha) + np.testing.assert_allclose(dyn.actual_motor_thrusts, [expected_first_step] * 4, rtol=1e-4) # Step for several time constants: actual thrust approaches 5.0 N for _ in range(50): # 0.5s = 12.5 tau @@ -120,3 +121,122 @@ def test_ground_effect_disabled_returns_nominal_thrust() -> None: nominal_thrusts = np.array([2.5, 2.5, 2.5, 2.5]) t_zero_alt, _ = dyn.compute_forces_and_torques(nominal_thrusts, altitude=0.05) assert math.isclose(t_zero_alt, 10.0, rel_tol=1e-6) + + +def test_quadratic_wind_drag_in_translation() -> None: + """Quadratic drag force F_drag = -D_v * v_rel - D_q * |v_rel| * v_rel increases drag non-linearly with airspeed.""" + dyn_linear = DroneDynamics6DOF( + dt=0.02, + substeps=1, + linear_damping=0.1, + quadratic_drag_coeff=0.0, + enable_motor_dynamics=False, + ) + dyn_quadratic = DroneDynamics6DOF( + dt=0.02, + substeps=1, + linear_damping=0.1, + quadratic_drag_coeff=0.05, + enable_motor_dynamics=False, + ) + + pos = np.array([10.0, 10.0, 10.0]) + vel = np.array([4.0, 0.0, 0.0]) # 4 m/s in X + dyn_linear.reset(position=pos, velocity=vel) + dyn_quadratic.reset(position=pos, velocity=vel) + + hover_thrusts = np.full(4, dyn_linear.hover_thrust_per_motor) + dyn_linear.step_rk4(hover_thrusts) + dyn_quadratic.step_rk4(hover_thrusts) + + # Linear drag: F_drag_x = -0.1 * 4 = -0.4 N + # Quadratic drag: F_drag_x = -0.1 * 4 - 0.05 * 4 * 4 = -0.4 - 0.8 = -1.2 N (3x greater deceleration) + assert dyn_quadratic.state.velocity[0] < dyn_linear.state.velocity[0] + + +def test_gym_env_motor_dynamics_step_response() -> None: + """Gymnasium Drone6DOFEnv respects motor dynamics time constant and lags motor commands.""" + env_instant = Drone6DOFEnv(enable_motor_dynamics=False) + env_lag = Drone6DOFEnv(enable_motor_dynamics=True, motor_time_constant=0.04) + + env_instant.reset(seed=42) + env_lag.reset(seed=42) + + action = np.array([1.0, 1.0, 1.0, 1.0], dtype=np.float32) # Full throttle + env_instant.step(action) + env_lag.step(action) + + # Without lag, actual motor thrust reaches max_thrust immediately + np.testing.assert_allclose(env_instant.dynamics.actual_motor_thrusts, [5.0] * 4, atol=1e-3) + # With lag, actual motor thrust transitions smoothly toward max_thrust + assert np.all(env_lag.dynamics.actual_motor_thrusts < 5.0) + assert np.all(env_lag.dynamics.actual_motor_thrusts > env_lag.dynamics.hover_thrust_per_motor) + + +def test_gym_env_wind_drag_linear_and_quadratic() -> None: + """Gymnasium Drone6DOFEnv applies wind drag correctly for opposing and matching relative winds.""" + # Headwind opposing vehicle motion (+X motion, -X wind) + env_headwind = Drone6DOFEnv( + wind_vector=np.array([-5.0, 0.0, 0.0]), + quadratic_drag_coeff=0.05, + ) + # Tailwind matching vehicle motion (+X motion, +X wind) + env_tailwind = Drone6DOFEnv( + wind_vector=np.array([5.0, 0.0, 0.0]), + quadratic_drag_coeff=0.05, + ) + + env_headwind.reset(seed=42) + env_tailwind.reset(seed=42) + + # Hover command with zero action (hover equilibrium) + action = np.zeros(4, dtype=np.float32) + env_headwind.step(action) + env_tailwind.step(action) + + # Headwind pushes drone in -X direction + assert env_headwind.dynamics.state.velocity[0] < 0.0 + # Tailwind pushes drone in +X direction + assert env_tailwind.dynamics.state.velocity[0] > 0.0 + + +def test_gym_env_ground_effect_thrust_boost() -> None: + """Gymnasium Drone6DOFEnv with enable_ground_effect=True amplifies rotor thrust near ground.""" + env_ige = Drone6DOFEnv( + enable_ground_effect=True, + start_pos=np.array([10.0, 10.0, 0.10]), # Very near ground + ) + env_oge = Drone6DOFEnv( + enable_ground_effect=False, + start_pos=np.array([10.0, 10.0, 0.10]), + ) + + env_ige.reset(seed=42) + env_oge.reset(seed=42) + + # Apply nominal hover thrust + action = np.zeros(4, dtype=np.float32) + env_ige.step(action) + env_oge.step(action) + + # In ground effect, upward vertical acceleration / velocity is higher than out of ground effect + assert env_ige.dynamics.state.velocity[2] > env_oge.dynamics.state.velocity[2] + + +def test_gym_env_domain_randomization_on_reset() -> None: + """Gymnasium Drone6DOFEnv applies reproducible parameter shifts on reset when randomize_domain=True.""" + env = Drone6DOFEnv(randomize_domain=True) + _, info1 = env.reset(seed=101) + assert "randomized_parameters" in info1 + mass1 = env.dynamics.mass + assert info1["randomized_parameters"]["mass"] == mass1 + + # Same seed reproduces exact randomized parameters + _, info1_repeat = env.reset(seed=101) + assert info1_repeat["randomized_parameters"]["mass"] == mass1 + assert env.dynamics.mass == mass1 + + # Different seed samples different parameters + _, info2 = env.reset(seed=202) + mass2 = env.dynamics.mass + assert mass1 != mass2 diff --git a/tests/test_fault_injection.py b/tests/test_fault_injection.py index 4b22450..0315d87 100644 --- a/tests/test_fault_injection.py +++ b/tests/test_fault_injection.py @@ -3,6 +3,7 @@ from __future__ import annotations import math +from typing import Any, Dict, Tuple import numpy as np @@ -63,22 +64,24 @@ def test_canonical_actuator_fault_telemetry_and_crash_classification() -> None: def test_moderate_actuator_fault_recovers_degraded() -> None: - """Under a survivable 10% partial thrust loss, vehicle maintains level hover and classifies as recovered_degraded.""" + """Under a survivable partial thrust loss, vehicle maintains level hover and classifies as recovered_degraded.""" def moderate_fault(step: int, thrusts: np.ndarray) -> np.ndarray: if step >= 40: degraded = thrusts.copy() - degraded[0] *= 0.90 # 10% thrust reduction on Motor 1 + degraded[0] *= 0.98 # 2% thrust reduction on Motor 1 (within attitude recovery limits) return degraded return thrusts sc = FlightScenarioDefinition( name="moderate_fault_scenario", - description="Moderate 10% thrust loss on Motor 1", + description="Moderate 2% thrust loss on Motor 1", initial_position=np.array([15.0, 15.0, 7.0]), waypoints=[np.array([15.0, 15.0, 7.0])], max_steps=120, actuator_fault_fn=moderate_fault, + target_radius=1.5, + settling_velocity_threshold=0.5, ) runner = FlightScenarioRunner() @@ -88,3 +91,80 @@ def moderate_fault(step: int, thrusts: np.ndarray) -> np.ndarray: assert result.metrics.success is True assert result.metrics.collision_count == 0 assert result.metrics.max_attitude_error_deg < 35.0 + + +def test_surviving_fault_without_reaching_goal_is_not_recovered_degraded() -> None: + """If vehicle survives fault without crashing but does not settle at goal, outcome is max_steps, NOT recovered_degraded.""" + + def drift_fault(step: int, thrusts: np.ndarray) -> np.ndarray: + if step >= 40: + degraded = thrusts.copy() + degraded[0] *= 0.90 # 10% thrust reduction causes lateral position drift + return degraded + return thrusts + + sc = FlightScenarioDefinition( + name="drift_fault_scenario", + description="10% thrust loss causing vehicle to drift away from target", + initial_position=np.array([15.0, 15.0, 7.0]), + waypoints=[np.array([15.0, 15.0, 7.0])], + max_steps=120, + actuator_fault_fn=drift_fault, + target_radius=1.2, + settling_velocity_threshold=0.5, + ) + + runner = FlightScenarioRunner() + result = runner.run_scenario(sc) + + # Vehicle survived without hitting floor or boundary, but drifted 10m away from goal: + assert result.metrics.collision_count == 0 + assert result.metrics.outcome != "recovered_degraded" + assert result.metrics.outcome == "max_steps" + assert result.metrics.success is False + + +def test_high_velocity_flythrough_rejected_from_success() -> None: + """A vehicle passing through the target radius at high speed (> settling_velocity_threshold) is rejected from SUCCESS.""" + + class HighSpeedController: + def compute_action( + self, state: Any, target_position: np.ndarray, target_velocity: np.ndarray, dt: float + ) -> Tuple[np.ndarray, Dict[str, Any]]: + # Command high forward tilt and excess thrust to speed through waypoint + return np.array([0.9, 0.9, -0.8, -0.8], dtype=np.float32), {} + + sc = FlightScenarioDefinition( + name="flythrough_scenario", + description="High speed flythrough test", + initial_position=np.array([5.0, 15.0, 7.0]), + waypoints=[np.array([15.0, 15.0, 7.0])], + max_steps=40, + target_radius=3.0, + settling_velocity_threshold=0.3, + ) + + runner = FlightScenarioRunner() + result = runner.run_scenario(sc, controller=HighSpeedController()) + + # Velocity at waypoint exceeds settling threshold; must not be classified as success + assert result.metrics.outcome != "success" + + +def test_collision_precedence_over_max_steps() -> None: + """When a collision occurs, collision outcome takes precedence over max_steps or success.""" + sc = FlightScenarioDefinition( + name="wall_crash_scenario", + description="Fly directly into wall", + initial_position=np.array([29.0, 15.0, 7.0]), + waypoints=[np.array([35.0, 15.0, 7.0])], # Outside bounds + bounds=(30.0, 30.0, 15.0), + max_steps=20, + ) + + runner = FlightScenarioRunner() + result = runner.run_scenario(sc) + + assert result.metrics.outcome in ("collision", "boundary_breach") + assert result.metrics.outcome != "max_steps" + assert result.metrics.success is False diff --git a/tests/test_lqr_controller.py b/tests/test_lqr_controller.py index ace7e31..96a3c45 100644 --- a/tests/test_lqr_controller.py +++ b/tests/test_lqr_controller.py @@ -1,95 +1,184 @@ -"""Unit tests for the optimal Linear Quadratic Regulator (LQR) flight controller.""" +"""Comprehensive verification tests for the LQR controller and analytical mixer inversion. + +Establishes: +1. Exact mathematical matrix inversion of the physical allocation matrix M (M * M_inv == I). +2. Pure axis isolation tests (positive and negative roll, pitch, yaw). +3. Verification of resulting actual physical torques from DroneDynamics6DOF. +4. Total collective thrust conservation. +5. Saturation clamping behavior. +6. Honest fallback handling (rejects silent approximations when solver unavailable). +7. Correct handling of the actual 36-D normalized observation space of Drone6DOFEnv. +""" from __future__ import annotations import math +from unittest.mock import patch import numpy as np +import pytest from adaptive_rl.controllers.lqr import QuadrotorLQRController -from adaptive_rl.environments.drone_6dof import DroneState6DOF +from adaptive_rl.environments.drone_6dof import Drone6DOFEnv, DroneDynamics6DOF from adaptive_rl.flight_testing.scenarios import ( FlightScenarioDefinition, FlightScenarioRunner, ) -def test_lqr_controller_gain_matrix_dimensions_and_finiteness() -> None: - """Verify that the continuous ARE is solved producing a valid 4x12 gain matrix K.""" - controller = QuadrotorLQRController(mass=1.0, gravity=9.81) - assert controller.K.shape == (4, 12) - assert np.all(np.isfinite(controller.K)) - - -def test_lqr_hover_equilibrium_action() -> None: - """At equilibrium setpoint with zero velocity, LQR outputs hover equilibrium thrust.""" - controller = QuadrotorLQRController(mass=1.0, gravity=9.81, max_thrust_per_motor=5.0) - hover_state = DroneState6DOF( - position=np.array([10.0, 10.0, 5.0]), - velocity=np.zeros(3), - quaternion=np.array([1.0, 0.0, 0.0, 0.0]), - angular_velocity=np.zeros(3), - ) - action, info = controller.compute_action( - hover_state, target_position=np.array([10.0, 10.0, 5.0]) +def test_lqr_allocation_matrix_inverse_identity() -> None: + """Build the physical allocation matrix M and verify M @ M_inv == I within float tolerance.""" + arm_length = 0.2 + d = arm_length / math.sqrt(2.0) + c_tau = 0.01 + + M = QuadrotorLQRController.get_allocation_matrix(d=d, c_tau=c_tau) + M_inv = QuadrotorLQRController.get_allocation_matrix_inverse(d=d, c_tau=c_tau) + + # Verify shapes + assert M.shape == (4, 4) + assert M_inv.shape == (4, 4) + + # Forward times inverse equals identity + prod_forward = M @ M_inv + np.testing.assert_allclose(prod_forward, np.eye(4), atol=1e-12, rtol=1e-12) + + # Inverse times forward equals identity + prod_backward = M_inv @ M + np.testing.assert_allclose(prod_backward, np.eye(4), atol=1e-12, rtol=1e-12) + + +def test_lqr_allocation_matrix_pure_axes_and_physical_torques() -> None: + """Verify that pure roll, pitch, and yaw commands produce exact physical torques in DroneDynamics6DOF.""" + arm_length = 0.2 + c_tau = 0.01 + dyn = DroneDynamics6DOF(arm_length=arm_length, torque_to_thrust_ratio=c_tau) + ctrl = QuadrotorLQRController(arm_length=arm_length, torque_to_thrust_ratio=c_tau) + + T_hover = 9.81 # Equilibrium collective thrust + + # 1. Pure positive roll (tau_x = +0.05, others 0) + f_roll_pos = ctrl.allocate_thrusts(total_thrust=T_hover, tau_x=0.05, tau_y=0.0, tau_z=0.0) + T_act, tau_act = dyn.compute_forces_and_torques(f_roll_pos) + assert math.isclose(T_act, T_hover, rel_tol=1e-6) + assert math.isclose(tau_act[0], 0.05, rel_tol=1e-6) + assert math.isclose(tau_act[1], 0.0, abs_tol=1e-10) + assert math.isclose(tau_act[2], 0.0, abs_tol=1e-10) + + # 2. Pure negative roll (tau_x = -0.05, others 0) + f_roll_neg = ctrl.allocate_thrusts(total_thrust=T_hover, tau_x=-0.05, tau_y=0.0, tau_z=0.0) + T_act, tau_act = dyn.compute_forces_and_torques(f_roll_neg) + assert math.isclose(T_act, T_hover, rel_tol=1e-6) + assert math.isclose(tau_act[0], -0.05, rel_tol=1e-6) + assert math.isclose(tau_act[1], 0.0, abs_tol=1e-10) + assert math.isclose(tau_act[2], 0.0, abs_tol=1e-10) + + # 3. Pure positive pitch (tau_y = +0.05, others 0) + f_pitch_pos = ctrl.allocate_thrusts(total_thrust=T_hover, tau_x=0.0, tau_y=0.05, tau_z=0.0) + T_act, tau_act = dyn.compute_forces_and_torques(f_pitch_pos) + assert math.isclose(T_act, T_hover, rel_tol=1e-6) + assert math.isclose(tau_act[0], 0.0, abs_tol=1e-10) + assert math.isclose(tau_act[1], 0.05, rel_tol=1e-6) + assert math.isclose(tau_act[2], 0.0, abs_tol=1e-10) + + # 4. Pure negative pitch (tau_y = -0.05, others 0) + f_pitch_neg = ctrl.allocate_thrusts(total_thrust=T_hover, tau_x=0.0, tau_y=-0.05, tau_z=0.0) + T_act, tau_act = dyn.compute_forces_and_torques(f_pitch_neg) + assert math.isclose(T_act, T_hover, rel_tol=1e-6) + assert math.isclose(tau_act[0], 0.0, abs_tol=1e-10) + assert math.isclose(tau_act[1], -0.05, rel_tol=1e-6) + assert math.isclose(tau_act[2], 0.0, abs_tol=1e-10) + + # 5. Pure positive yaw (tau_z = +0.01, others 0) + f_yaw_pos = ctrl.allocate_thrusts(total_thrust=T_hover, tau_x=0.0, tau_y=0.0, tau_z=0.01) + T_act, tau_act = dyn.compute_forces_and_torques(f_yaw_pos) + assert math.isclose(T_act, T_hover, rel_tol=1e-6) + assert math.isclose(tau_act[0], 0.0, abs_tol=1e-10) + assert math.isclose(tau_act[1], 0.0, abs_tol=1e-10) + assert math.isclose(tau_act[2], 0.01, rel_tol=1e-6) + + # 6. Pure negative yaw (tau_z = -0.01, others 0) + f_yaw_neg = ctrl.allocate_thrusts(total_thrust=T_hover, tau_x=0.0, tau_y=0.0, tau_z=-0.01) + T_act, tau_act = dyn.compute_forces_and_torques(f_yaw_neg) + assert math.isclose(T_act, T_hover, rel_tol=1e-6) + assert math.isclose(tau_act[0], 0.0, abs_tol=1e-10) + assert math.isclose(tau_act[1], 0.0, abs_tol=1e-10) + assert math.isclose(tau_act[2], -0.01, rel_tol=1e-6) + + +def test_lqr_collective_thrust_conservation_and_clipping() -> None: + """Verify collective thrust conservation under coupled torques and saturation limits.""" + ctrl = QuadrotorLQRController(max_thrust_per_motor=5.0) + + # Unclipped coupled demand + f_coupled = ctrl.allocate_thrusts(total_thrust=10.0, tau_x=0.02, tau_y=-0.02, tau_z=0.005) + assert math.isclose(float(np.sum(f_coupled)), 10.0, rel_tol=1e-6) + assert np.all(f_coupled >= 0.0) + + # Demanding excessive thrust clips to motor limits + f_excess = np.clip( + ctrl.allocate_thrusts(total_thrust=25.0, tau_x=0.0, tau_y=0.0, tau_z=0.0), 0.0, 5.0 ) + assert np.allclose(f_excess, [5.0, 5.0, 5.0, 5.0]) - # Virtual control inputs should be near zero at equilibrium - assert math.isclose(info["delta_T"], 0.0, abs_tol=1e-5) - assert math.isclose(info["tau_x"], 0.0, abs_tol=1e-5) - assert math.isclose(info["tau_y"], 0.0, abs_tol=1e-5) - assert math.isclose(info["tau_z"], 0.0, abs_tol=1e-5) - # Motor thrusts must equal hover thrust per motor: 9.81 / 4 = 2.4525 N - expected_hover_thrust = 9.81 / 4.0 - for t in info["motor_thrusts"]: - assert math.isclose(t, expected_hover_thrust, rel_tol=1e-4) +def test_lqr_are_solver_and_fallback_behavior() -> None: + """Verify ARE solver identification and rejection of silent approximations.""" + ctrl = QuadrotorLQRController() + assert ctrl.solver_status == "ARE_SOLVED" + assert ctrl.is_are_solution is True + assert ctrl.K.shape == (4, 12) + # Simulating solver failure when allow_fallback is False must raise RuntimeError + with patch("scipy.linalg.solve_continuous_are", side_effect=Exception("Solver singular")): + with pytest.raises( + RuntimeError, match="Continuous Algebraic Riccati Equation solver failed" + ): + QuadrotorLQRController(allow_fallback=False) -def test_lqr_vertical_and_lateral_control_response() -> None: - """Position displacements produce expected restoring thrust and torque directions.""" - controller = QuadrotorLQRController(mass=1.0, gravity=9.81, max_thrust_per_motor=5.0) + # When allow_fallback is True, solver_status must be UNRESOLVED_APPROXIMATION + with patch("scipy.linalg.solve_continuous_are", side_effect=Exception("Solver singular")): + ctrl_fb = QuadrotorLQRController(allow_fallback=True) + assert ctrl_fb.solver_status == "UNRESOLVED_APPROXIMATION" + assert ctrl_fb.is_are_solution is False - # 1. Drone is below target (z=4.0, target=5.0): must command positive thrust delta - state_low = DroneState6DOF( - position=np.array([10.0, 10.0, 4.0]), - velocity=np.zeros(3), - quaternion=np.array([1.0, 0.0, 0.0, 0.0]), - angular_velocity=np.zeros(3), - ) - _, info_low = controller.compute_action(state_low, target_position=np.array([10.0, 10.0, 5.0])) - assert info_low["delta_T"] > 0.0 - assert info_low["total_thrust"] > 9.81 - - # 2. Drone is to the left (y=9.0, target=10.0): must command restoring roll - state_left = DroneState6DOF( - position=np.array([10.0, 9.0, 5.0]), - velocity=np.zeros(3), - quaternion=np.array([1.0, 0.0, 0.0, 0.0]), - angular_velocity=np.zeros(3), - ) - _, info_left = controller.compute_action( - state_left, target_position=np.array([10.0, 10.0, 5.0]) - ) - # Restoring roll torque should be non-zero - assert abs(info_left["tau_x"]) > 0.0 +def test_lqr_observation_adapter_with_real_36d_gym_environment() -> None: + """Verify LQR predict interface against the actual 36-D observation from Drone6DOFEnv.""" + env = Drone6DOFEnv(bounds=(30.0, 30.0, 15.0), max_steps=50) + obs, info = env.reset(seed=42) -def test_lqr_predict_gym_interface() -> None: - """Verify predict interface compatibility with Gym observation vector.""" - controller = QuadrotorLQRController() - obs = np.zeros(18, dtype=np.float32) - obs[0:3] = [5.0, 5.0, 5.0] # pos - obs[6] = 1.0 # qw - obs[13:16] = [10.0, 10.0, 8.0] # target + assert obs.shape == (36,) + + ctrl = QuadrotorLQRController(bounds=(30.0, 30.0, 15.0)) + action, ctrl_info = ctrl.predict(obs) - action, info = controller.predict(obs) assert action.shape == (4,) assert np.all(action >= -1.0) and np.all(action <= 1.0) - assert info is not None + assert ctrl_info is not None + assert ctrl_info["is_are_solution"] is True + + # Step environment with LQR action over multiple steps and verify stable flight + for _ in range(20): + next_obs, reward, terminated, truncated, step_info = env.step(action) + action, _ = ctrl.predict(next_obs) + if terminated or truncated: + break + + # Drone must not have exploded or breached boundaries immediately + assert env.dynamics.state.position[2] > 0.0 + + +def test_lqr_rejects_unsupported_observation_shape() -> None: + """LQR predict must raise an explicit ValueError on invalid observation shapes.""" + ctrl = QuadrotorLQRController() + invalid_obs = np.zeros(25, dtype=np.float32) + + with pytest.raises(ValueError, match="Unsupported state/obs representation"): + ctrl.predict(invalid_obs) -def test_lqr_in_flight_scenario_runner() -> None: +def test_lqr_hover_in_flight_scenario_runner() -> None: """Execute hover scenario with LQR controller through FlightScenarioRunner.""" controller = QuadrotorLQRController() sc = FlightScenarioDefinition( diff --git a/tests/test_navigation_astar.py b/tests/test_navigation_astar.py index 984ed77..f37d6a6 100644 --- a/tests/test_navigation_astar.py +++ b/tests/test_navigation_astar.py @@ -197,3 +197,109 @@ def test_mission_manager_lifecycle_and_replanning() -> None: rth_ok = manager.trigger_return_to_home(drone_state.position, obstacles) assert rth_ok is True np.testing.assert_allclose(manager.final_goal, start_pos) + + +def test_planner_diagonal_corner_cutting_prevention() -> None: + """Verify that A* neighbor expansion never cuts diagonally through obstacles.""" + planner = AStarPlanner3D( + bounds=(10.0, 10.0, 10.0), resolution=1.0, vehicle_radius=0.4, safety_margin=0.1 + ) + # Start at (2, 2, 5), Goal at (4, 4, 5) + start = np.array([2.0, 2.0, 5.0]) + goal = np.array([4.0, 4.0, 5.0]) + + # Place an obstacle centered at (3.0, 3.0, 5.0) with radius 0.6 + # The direct diagonal line passes straight through (3.0, 3.0, 5.0), penetrating the obstacle. + # The grid points (2, 2, 5) and (4, 4, 5) have distance sqrt(2) ~ 1.414 from (3,3,5), so points are free, + # but the diagonal connecting segment is strictly in collision! + obs = ObstacleSphere3D(center=np.array([3.0, 3.0, 5.0]), radius=0.6) + obstacles = [obs] + + res = planner.plan(start, goal, obstacles) + assert res.status == "SUCCESS" + + # Invariant: every returned segment must pass is_segment_valid + for i in range(len(res.waypoints) - 1): + valid, clear = planner.is_segment_valid(res.waypoints[i], res.waypoints[i + 1], obstacles) + assert valid is True + assert clear >= 0.0 + + +def test_planner_simplification_never_appends_unvalidated_segment() -> None: + """Verify simplify_path returns None rather than appending an invalid segment.""" + planner = AStarPlanner3D( + bounds=(20.0, 20.0, 10.0), resolution=1.0, vehicle_radius=0.5, safety_margin=0.2 + ) + # Construct a path where the intermediate step to the next waypoint is blocked + p1 = np.array([2.0, 5.0, 5.0]) + p2 = np.array([6.0, 5.0, 5.0]) + p3 = np.array([10.0, 5.0, 5.0]) + + # Place a large impassable obstacle completely covering p2 and the segments to/from p2 + obs = ObstacleSphere3D(center=np.array([6.0, 5.0, 5.0]), radius=3.0) + obstacles = [obs] + + # Attempting to simplify an unsafe list of waypoints must return None + res_simplified = planner.simplify_path([p1, p2, p3], obstacles) + assert res_simplified is None + + +def test_planner_tangential_contact_and_boundary_grazing() -> None: + """Waypoints grazing the boundary or obstacle perimeter are properly validated.""" + planner = AStarPlanner3D( + bounds=(10.0, 10.0, 10.0), resolution=1.0, vehicle_radius=0.5, safety_margin=0.2 + ) + # Point grazing floor boundary (z = 0.5 is exactly vehicle_radius, z < 0.5 is invalid) + pt_safe = np.array([5.0, 5.0, 0.7]) + valid_safe, _ = planner.is_point_valid(pt_safe, []) + assert valid_safe is True + + pt_boundary_breach = np.array([5.0, 5.0, 0.4]) + valid_breach, _ = planner.is_point_valid(pt_boundary_breach, []) + assert valid_breach is False + + +def test_planner_overlapping_obstacles_and_determinism() -> None: + """Complex multi-obstacle field produces deterministic identical paths upon repeated planning.""" + planner = AStarPlanner3D( + bounds=(20.0, 20.0, 10.0), resolution=1.0, vehicle_radius=0.5, safety_margin=0.2 + ) + start = np.array([2.0, 2.0, 5.0]) + goal = np.array([18.0, 18.0, 5.0]) + + obs1 = ObstacleSphere3D(center=np.array([8.0, 8.0, 5.0]), radius=2.0) + obs2 = ObstacleSphere3D(center=np.array([10.0, 9.0, 5.0]), radius=2.0) # Overlaps obs1 + obs3 = ObstacleSphere3D(center=np.array([14.0, 14.0, 5.0]), radius=1.8) + obstacles = [obs1, obs2, obs3] + + res1 = planner.plan(start, goal, obstacles) + res2 = planner.plan(start, goal, obstacles) + + assert res1.status == "SUCCESS" + assert res2.status == "SUCCESS" + assert len(res1.waypoints) == len(res2.waypoints) + for w1, w2 in zip(res1.waypoints, res2.waypoints): + np.testing.assert_allclose(w1, w2, atol=1e-10) + + # Invariant: Every consecutive segment is strictly collision-free + for i in range(len(res1.waypoints) - 1): + valid, clear = planner.is_segment_valid(res1.waypoints[i], res1.waypoints[i + 1], obstacles) + assert valid is True + assert clear >= 0.0 + + +def test_planner_multiple_resolutions() -> None: + """Planner functions correctly at coarse (1.0m) and fine (0.5m) grid resolutions.""" + for res_val in [1.0, 0.5]: + planner = AStarPlanner3D( + bounds=(15.0, 15.0, 10.0), resolution=res_val, vehicle_radius=0.5, safety_margin=0.2 + ) + start = np.array([2.0, 7.5, 5.0]) + goal = np.array([13.0, 7.5, 5.0]) + obs = ObstacleSphere3D(center=np.array([7.5, 7.5, 5.0]), radius=2.0) + res = planner.plan(start, goal, [obs]) + assert res.status == "SUCCESS" + assert len(res.waypoints) >= 3 + for i in range(len(res.waypoints) - 1): + valid, _ = planner.is_segment_valid(res.waypoints[i], res.waypoints[i + 1], [obs]) + assert valid is True diff --git a/tests/test_recorder_fingerprint_phase3.py b/tests/test_recorder_fingerprint_phase3.py index c3c2a35..ba1f4f6 100644 --- a/tests/test_recorder_fingerprint_phase3.py +++ b/tests/test_recorder_fingerprint_phase3.py @@ -4,6 +4,8 @@ from pathlib import Path +import numpy as np + from adaptive_rl.flight_testing.recorder import ( ExperimentConfigurationFingerprint, FlightRecorder, @@ -126,6 +128,60 @@ def test_dual_integrity_digests_and_record_tamper_detection(tmp_path: Path) -> N assert record.verify_record_digest() is False # result digest detects tampering! +def test_trajectories_preserved_and_covered_by_digest(tmp_path: Path) -> None: + """FlightRecord preserves full trajectories, and tampering with trajectory step invalidates record_digest.""" + from adaptive_rl.flight_testing.scenarios import ( + FlightScenarioDefinition, + FlightScenarioRunner, + ) + + runner = FlightScenarioRunner() + sc = FlightScenarioDefinition( + name="short_hover", + description="Short hover for trajectory test", + initial_position=np.array([5.0, 5.0, 5.0]), + waypoints=[np.array([5.0, 5.0, 5.0])], + max_steps=10, + ) + result = runner.run_scenario(sc) + assert len(result.telemetry) > 0 + + recorder = FlightRecorder(output_dir=tmp_path) + record = recorder.create_record( + experiment_id="trajectory_record_test", + physics_params={"mass": 1.0, "gravity": 9.81}, + scenario_results=[result], + ) + + assert "short_hover" in record.trajectories + assert len(record.trajectories["short_hover"]) == len(result.telemetry) + assert record.verify_config_fingerprint() is True + assert record.verify_record_digest() is True + + # Tamper with telemetry in trajectory: record_digest MUST be invalidated! + record.trajectories["short_hover"][0]["position"] = [999.0, 999.0, 999.0] + assert record.verify_record_digest() is False + + +def test_signature_distinction_from_digest(tmp_path: Path) -> None: + """Digital signature is an authenticated cryptographic signature, distinct from unkeyed digests.""" + recorder = FlightRecorder(output_dir=tmp_path) + record = recorder.create_record( + experiment_id="sig_test", + physics_params={"mass": 1.0}, + scenario_results=[], + signature="ed25519_sig_abc123", + ) + assert record.signature == "ed25519_sig_abc123" + assert record.config_fingerprint != record.signature + assert record.record_digest != record.signature + assert record.verify_record_digest() is True + + # Tampering with signature invalidates record_digest + record.signature = "tampered_signature" + assert record.verify_record_digest() is False + + def test_small_damping_and_inertia_float_precision() -> None: """Very small damping or inertia terms (< 1e-5) are preserved in canonical serialization.""" d = {"angular_damping": 1.25e-6, "mass": 1.0} From e0340f72c826627b66cf96ea8a0564497b02441b Mon Sep 17 00:00:00 2001 From: Aryan Saxena Date: Sun, 15 Nov 2026 20:41:15 +0530 Subject: [PATCH 2/7] fix(phase3): resolve residual defects across dynamics, estimation, planning, and provenance - Eliminate mutable dynamics state aliasing and calculate genuine inertial acceleration in IMU telemetry - Enforce authoritative mission completion requiring sequential waypoint traversal, position settling, velocity settling, and collision freedom - Preserve exact finite IEEE 754 precision in canonical JSON dumps and comprehensive scenario provenance hashing - Implement continuous swept-sphere dynamic obstacle collision detection with adaptive temporal subintervals - Implement closed-loop state-estimation-in-the-loop control contract and dedicated scenario - Enable arbitrary non-grid-aligned 3D A* start/goal coordinates with collision-free neighbor connection and validation - Couple stage-wise continuous first-order actuator lag across RK4 evaluation stages eliminating integration bias - Add full test suites covering all residual defect requirements with 528 passing tests --- src/adaptive_rl/environments/drone_6dof.py | 54 +++-- src/adaptive_rl/flight_testing/recorder.py | 56 ++++- src/adaptive_rl/flight_testing/scenarios.py | 206 +++++++++++++++--- .../navigation/dynamic_obstacles.py | 79 ++++++- src/adaptive_rl/navigation/planner.py | 65 ++++-- tests/test_actuator_aerodynamics.py | 26 +++ tests/test_fault_injection.py | 51 +++++ tests/test_flight_recorder.py | 4 +- tests/test_navigation_astar.py | 45 ++++ tests/test_navigation_dynamic_obstacles.py | 190 ++++++++++++++++ tests/test_recorder_fingerprint_phase3.py | 94 ++++++++ tests/test_sensor_estimation.py | 74 +++++++ 12 files changed, 863 insertions(+), 81 deletions(-) create mode 100644 tests/test_navigation_dynamic_obstacles.py diff --git a/src/adaptive_rl/environments/drone_6dof.py b/src/adaptive_rl/environments/drone_6dof.py index 8d5dc7f..d94bd16 100644 --- a/src/adaptive_rl/environments/drone_6dof.py +++ b/src/adaptive_rl/environments/drone_6dof.py @@ -568,28 +568,33 @@ def step_rk4(self, motor_thrusts: np.ndarray) -> DroneState6DOF: omega = self.state.angular_velocity.copy() for _ in range(self.substeps): - # Integrate actuator first-order response if enabled (exact analytical step response) + f_0 = self.actual_motor_thrusts.copy() + + # Continuous coupled actuator integration across RK4 evaluation stages if self.enable_motor_dynamics and self.motor_time_constant > 1e-9: - alpha = math.exp(-h / self.motor_time_constant) - self.actual_motor_thrusts = self.actual_motor_thrusts * alpha + f_cmd * ( - 1.0 - alpha + tau = self.motor_time_constant + alpha_half = math.exp(-0.5 * h / tau) + alpha_full = math.exp(-h / tau) + + f_k1 = f_0 + f_k2 = np.clip( + f_0 * alpha_half + f_cmd * (1.0 - alpha_half), 0.0, self.max_thrust_per_motor ) - self.actual_motor_thrusts = np.clip( - self.actual_motor_thrusts, 0.0, self.max_thrust_per_motor + f_k3 = f_k2 + f_k4 = np.clip( + f_0 * alpha_full + f_cmd * (1.0 - alpha_full), 0.0, self.max_thrust_per_motor ) + self.actual_motor_thrusts = f_k4.copy() else: - self.actual_motor_thrusts = np.clip(f_cmd, 0.0, self.max_thrust_per_motor) - - total_thrust, torques = self.compute_forces_and_torques( - self.actual_motor_thrusts, altitude=pos[2] - ) + f_clipped = np.clip(f_cmd, 0.0, self.max_thrust_per_motor) + f_k1 = f_k2 = f_k3 = f_k4 = f_clipped + self.actual_motor_thrusts = f_clipped.copy() - # k1 - k1_p, k1_v, k1_q, k1_w = self._state_derivatives( - pos, vel, quat, omega, total_thrust, torques - ) + # k1 (at t = 0) + t1, tau1 = self.compute_forces_and_torques(f_k1, altitude=pos[2]) + k1_p, k1_v, k1_q, k1_w = self._state_derivatives(pos, vel, quat, omega, t1, tau1) - # k2 + # k2 (at t = h/2) p2 = pos + 0.5 * h * k1_p v2 = vel + 0.5 * h * k1_v q2 = quat + 0.5 * h * k1_q @@ -597,9 +602,10 @@ def step_rk4(self, motor_thrusts: np.ndarray) -> DroneState6DOF: if q2_norm > 1e-12: q2 /= q2_norm w2 = omega + 0.5 * h * k1_w - k2_p, k2_v, k2_q, k2_w = self._state_derivatives(p2, v2, q2, w2, total_thrust, torques) + t2, tau2 = self.compute_forces_and_torques(f_k2, altitude=p2[2]) + k2_p, k2_v, k2_q, k2_w = self._state_derivatives(p2, v2, q2, w2, t2, tau2) - # k3 + # k3 (at t = h/2) p3 = pos + 0.5 * h * k2_p v3 = vel + 0.5 * h * k2_v q3 = quat + 0.5 * h * k2_q @@ -607,9 +613,10 @@ def step_rk4(self, motor_thrusts: np.ndarray) -> DroneState6DOF: if q3_norm > 1e-12: q3 /= q3_norm w3 = omega + 0.5 * h * k2_w - k3_p, k3_v, k3_q, k3_w = self._state_derivatives(p3, v3, q3, w3, total_thrust, torques) + t3, tau3 = self.compute_forces_and_torques(f_k3, altitude=p3[2]) + k3_p, k3_v, k3_q, k3_w = self._state_derivatives(p3, v3, q3, w3, t3, tau3) - # k4 + # k4 (at t = h) p4 = pos + h * k3_p v4 = vel + h * k3_v q4 = quat + h * k3_q @@ -617,7 +624,8 @@ def step_rk4(self, motor_thrusts: np.ndarray) -> DroneState6DOF: if q4_norm > 1e-12: q4 /= q4_norm w4 = omega + h * k3_w - k4_p, k4_v, k4_q, k4_w = self._state_derivatives(p4, v4, q4, w4, total_thrust, torques) + t4, tau4 = self.compute_forces_and_torques(f_k4, altitude=p4[2]) + k4_p, k4_v, k4_q, k4_w = self._state_derivatives(p4, v4, q4, w4, t4, tau4) # RK4 update pos += (h / 6.0) * (k1_p + 2.0 * k2_p + 2.0 * k3_p + k4_p) @@ -950,9 +958,9 @@ def _action_to_motor_thrusts(self, action: np.ndarray) -> np.ndarray: act_arr = np.clip(np.asarray(action, dtype=np.float64), -1.0, 1.0) return (act_arr + 1.0) * 0.5 * self.dynamics.max_thrust_per_motor - def _get_obs(self) -> np.ndarray: + def _get_obs(self, custom_state: Optional[DroneState6DOF] = None) -> np.ndarray: bx, by, bz = self.bounds - state = self.dynamics.state + state = custom_state if custom_state is not None else self.dynamics.state norm_pos = [state.position[0] / bx, state.position[1] / by, state.position[2] / bz] norm_vel = [ diff --git a/src/adaptive_rl/flight_testing/recorder.py b/src/adaptive_rl/flight_testing/recorder.py index c906cdb..5209737 100644 --- a/src/adaptive_rl/flight_testing/recorder.py +++ b/src/adaptive_rl/flight_testing/recorder.py @@ -48,29 +48,38 @@ def canonical_json_dumps(data: Any) -> str: - """Serialize dictionary or primitive to canonical JSON with sorted keys and stable float formatting. + """Serialize dictionary or primitive to canonical JSON with sorted keys and exact IEEE 754 float formatting. - Preserves 6 decimal places for standard values while preserving 12 significant digits for small - physical parameters (< 1e-5, such as damping or inertia terms) to prevent loss of precision. + Preserves full finite floating-point precision without lossy rounding, ensuring distinct + scientific configurations (such as sub-microgram mass or sub-millimeter geometry differences) + produce distinct cryptographic digests. """ def _sanitize(val: Any) -> Any: - if isinstance(val, (int, bool, str)) or val is None: + if isinstance(val, bool) or val is None: return val - if isinstance(val, float): - if not math.isfinite(val): - return str(val) - if abs(val) < 1e-5 and val != 0.0: - return round(val, 12) - return round(val, 6) + if isinstance(val, (int, np.integer)): + return int(val) + if isinstance(val, (float, np.floating)): + f_val = float(val) + if not math.isfinite(f_val): + if math.isnan(f_val): + return "NaN" + return "Infinity" if f_val > 0 else "-Infinity" + # Normalize -0.0 to 0.0 to guarantee deterministic hashing + return 0.0 if f_val == 0.0 else f_val + if isinstance(val, str): + return val + if isinstance(val, np.ndarray): + return [_sanitize(x) for x in val.tolist()] if isinstance(val, (list, tuple)): return [_sanitize(x) for x in val] + if hasattr(val, "to_dict"): + return _sanitize(val.to_dict()) if isinstance(val, dict): return { str(k): _sanitize(v) for k, v in sorted(val.items(), key=lambda item: str(item[0])) } - if hasattr(val, "to_dict"): - return _sanitize(val.to_dict()) return str(val) sanitized = _sanitize(data) @@ -110,6 +119,11 @@ class PhysicsSimulationFingerprint: body_frame_lidar: bool num_lidar_rays: int lidar_range: float + enable_motor_dynamics: bool = False + motor_time_constant: float = 0.0 + wind_vector: List[float] = field(default_factory=lambda: [0.0, 0.0, 0.0]) + enable_ground_effect: bool = False + quadratic_drag_coeff: float = 0.0 def to_dict(self) -> Dict[str, Any]: return asdict(self) @@ -127,6 +141,7 @@ class ExperimentConfigurationFingerprint: scenario_names: List[str] controller_type: str = "QuadrotorPIDController" controller_gains: Dict[str, float] = field(default_factory=dict) + scenarios: List[Dict[str, Any]] = field(default_factory=list) extra_parameters: Dict[str, Any] = field(default_factory=dict) def to_dict(self) -> Dict[str, Any]: @@ -137,6 +152,7 @@ def to_dict(self) -> Dict[str, Any]: "checkpoint_hash": self.checkpoint_hash, "random_seed": self.random_seed, "scenario_names": sorted(self.scenario_names), + "scenarios": self.scenarios, "controller_type": self.controller_type, "controller_gains": self.controller_gains, "extra_parameters": self.extra_parameters, @@ -240,6 +256,7 @@ def create_record( signature: Optional[str] = None, configuration: Optional[ExperimentConfigurationFingerprint | Dict[str, Any]] = None, trajectories: Optional[Dict[str, List[Dict[str, Any]]]] = None, + scenarios: Optional[Sequence[Any]] = None, ) -> FlightRecord: """Create a cryptographically fingerprinted FlightRecord from execution data.""" from datetime import datetime, timezone @@ -265,6 +282,11 @@ def create_record( body_frame_lidar=bool(physics_params.get("body_frame_lidar", False)), num_lidar_rays=int(physics_params.get("num_lidar_rays", 16)), lidar_range=float(physics_params.get("lidar_range", 20.0)), + enable_motor_dynamics=bool(physics_params.get("enable_motor_dynamics", False)), + motor_time_constant=float(physics_params.get("motor_time_constant", 0.0)), + wind_vector=[float(x) for x in physics_params.get("wind_vector", [0.0, 0.0, 0.0])], + enable_ground_effect=bool(physics_params.get("enable_ground_effect", False)), + quadratic_drag_coeff=float(physics_params.get("quadratic_drag_coeff", 0.0)), ) # Extract metrics and trajectories @@ -293,6 +315,15 @@ def create_record( else: chk_hash = "not_found" + # Serialize scenario provenance definitions + raw_scenarios = scenarios if scenarios is not None else physics_params.get("scenarios", []) + scenario_provenance: List[Dict[str, Any]] = [] + for sc in raw_scenarios: + if hasattr(sc, "to_dict"): + scenario_provenance.append(sc.to_dict()) + elif isinstance(sc, dict): + scenario_provenance.append(sc) + # Build comprehensive ExperimentConfigurationFingerprint if configuration is not None: if isinstance(configuration, ExperimentConfigurationFingerprint): @@ -315,6 +346,7 @@ def create_record( physics_params.get("controller_type", "QuadrotorPIDController") ), controller_gains=dict(physics_params.get("controller_gains", {})), + scenarios=scenario_provenance, extra_parameters=dict(physics_params.get("extra_parameters", {})), ) diff --git a/src/adaptive_rl/flight_testing/scenarios.py b/src/adaptive_rl/flight_testing/scenarios.py index abc6380..9bc1c56 100644 --- a/src/adaptive_rl/flight_testing/scenarios.py +++ b/src/adaptive_rl/flight_testing/scenarios.py @@ -301,10 +301,85 @@ class FlightScenarioDefinition: min_recovery_altitude: float = 0.8 enable_sensor_estimation: bool = False sensor_seed: Optional[int] = None + sensor_in_the_loop: bool = False max_duration_seconds: Optional[float] = None disturbance_fn: Optional[Callable[[int, DroneState6DOF], np.ndarray]] = None actuator_fault_fn: Optional[Callable[[int, np.ndarray], np.ndarray]] = None + def to_dict(self) -> Dict[str, Any]: + """Return a deterministic, fully serializable dictionary representation for cryptographic hashing.""" + obs_dicts = [ + {"center": [float(x) for x in obs.center], "radius": float(obs.radius)} + for obs in sorted( + self.obstacles, + key=lambda o: ( + float(o.center[0]), + float(o.center[1]), + float(o.center[2]), + float(o.radius), + ), + ) + ] + dyn_obs_dicts = [ + d.to_dict() + if hasattr(d, "to_dict") + else { + "center_initial": [float(x) for x in d.center_initial], + "radius": float(d.radius), + "velocity": [float(x) for x in d.velocity], + "bounds": [float(x) for x in d.bounds], + "motion_type": str(d.motion_type), + "oscillation_period": float(d.oscillation_period), + "oscillation_amplitude": [float(x) for x in d.oscillation_amplitude], + } + for d in self.dynamic_obstacles + ] + + fault_info: Dict[str, Any] = {} + if self.actuator_fault_fn is not None: + fn_name = getattr(self.actuator_fault_fn, "__name__", "custom_fault") + fault_info = {"name": fn_name} + if hasattr(self.actuator_fault_fn, "metadata"): + fault_info["metadata"] = getattr(self.actuator_fault_fn, "metadata") + elif self.actuator_fault_fn.__doc__: + fault_info["doc"] = self.actuator_fault_fn.__doc__.strip() + + dist_info: Dict[str, Any] = {} + if self.disturbance_fn is not None: + fn_name = getattr(self.disturbance_fn, "__name__", "custom_disturbance") + dist_info = {"name": fn_name} + if hasattr(self.disturbance_fn, "metadata"): + dist_info["metadata"] = getattr(self.disturbance_fn, "metadata") + elif self.disturbance_fn.__doc__: + dist_info["doc"] = self.disturbance_fn.__doc__.strip() + + return { + "name": self.name, + "description": self.description, + "initial_position": [float(x) for x in self.initial_position], + "initial_velocity": [float(x) for x in self.initial_velocity], + "initial_quaternion": [float(x) for x in self.initial_quaternion], + "initial_angular_velocity": [float(x) for x in self.initial_angular_velocity], + "waypoints": [[float(x) for x in w] for w in self.waypoints], + "obstacles": obs_dicts, + "dynamic_obstacles": dyn_obs_dicts, + "bounds": [float(x) for x in self.bounds], + "max_steps": self.max_steps, + "target_radius": float(self.target_radius), + "collision_radius": float(self.collision_radius), + "step_duration_sec": float(self.step_duration_sec), + "use_path_planner": self.use_path_planner, + "settling_velocity_threshold": float(self.settling_velocity_threshold), + "max_recovery_tilt_deg": float(self.max_recovery_tilt_deg), + "min_recovery_altitude": float(self.min_recovery_altitude), + "enable_sensor_estimation": self.enable_sensor_estimation, + "sensor_seed": self.sensor_seed, + "sensor_in_the_loop": self.sensor_in_the_loop, + "max_duration_seconds": self.max_duration_seconds, + "actuator_fault": fault_info, + "disturbance": dist_info, + } + def get_standard_flight_scenarios() -> List[FlightScenarioDefinition]: """Instantiate the 10 standard autonomous flight scenarios.""" @@ -624,6 +699,22 @@ def get_advanced_navigation_scenarios() -> List[FlightScenarioDefinition]: ) ) + # 19. State Estimation In-the-Loop Flight + scenarios.append( + FlightScenarioDefinition( + name="sensor_estimated_navigation", + description="Precision hover loiter driven entirely by sensor-fusion estimated state under IMU noise and drift.", + initial_position=np.array([10.0, 10.0, 5.0]), + waypoints=[np.array([10.0, 10.0, 5.0])], + obstacles=[], + max_steps=200, + target_radius=1.0, + enable_sensor_estimation=True, + sensor_in_the_loop=True, + sensor_seed=101, + ) + ) + return scenarios @@ -767,12 +858,22 @@ def run_scenario( # Sensor estimation avionics in-the-loop if enabled imu_sensor = None state_estimator = None - if scenario.enable_sensor_estimation: + sensor_rng = None + est_drone_state: Optional[DroneState6DOF] = None + if scenario.enable_sensor_estimation or scenario.sensor_in_the_loop: from adaptive_rl.sensors.estimation import ComplementaryFilter from adaptive_rl.sensors.imu import IMUSensor - imu_sensor = IMUSensor(seed=scenario.sensor_seed or 42) - state_estimator = ComplementaryFilter() + sensor_rng = np.random.default_rng(scenario.sensor_seed or 42) + imu_sensor = IMUSensor( + gravity=self.env.dynamics.gravity, seed=scenario.sensor_seed or 42 + ) + state_estimator = ComplementaryFilter(gravity=self.env.dynamics.gravity) + state_estimator.reset( + position=scenario.initial_position, + velocity=scenario.initial_velocity, + quaternion=scenario.initial_quaternion, + ) telemetry: List[FlightStepTelemetry] = [] pos_errors: List[float] = [] @@ -788,16 +889,24 @@ def run_scenario( step = 0 dt = self.env.dynamics.dt fault_ever_active = False + all_waypoints_completed = False while step < scenario.max_steps: step += 1 time_sec = step * dt current_state = self.env.dynamics.state + # Determine feedback state for closed-loop control (sensor-in-the-loop if active) + feedback_state = ( + est_drone_state + if (scenario.sensor_in_the_loop and est_drone_state is not None) + else current_state + ) + # Predictive dynamic obstacle avoidance via MissionManager if mission_manager is not None: current_target, _ = mission_manager.update( - drone_state=current_state, + drone_state=feedback_state, time_sec=time_sec, static_obstacles=scenario.obstacles, dynamic_obstacles=scenario.dynamic_obstacles, @@ -825,16 +934,16 @@ def run_scenario( omega_mag = float(np.linalg.norm(current_state.angular_velocity)) angular_speeds.append(omega_mag) - # Compute controller action + # Compute controller action using feedback_state (sensor-in-the-loop if active) if hasattr(ctrl, "compute_action"): action, ctrl_info = ctrl.compute_action( - current_state, + feedback_state, target_position=current_target, target_velocity=np.zeros(3), dt=dt, ) elif hasattr(ctrl, "predict"): - obs = self.env._get_obs() + obs = self.env._get_obs(custom_state=feedback_state) action, _ = ctrl.predict(obs, deterministic=True) else: action = np.zeros(4, dtype=np.float32) @@ -854,14 +963,17 @@ def run_scenario( if fault_active: fault_ever_active = True - # Step physical dynamics with Runge-Kutta 4 - old_pos = current_state.position.copy() + # Snapshot pre-integration physical state to prevent object aliasing + pre_state = self.env.dynamics.state.copy() + pre_velocity = pre_state.velocity.copy() + old_pos = pre_state.position.copy() - # External aerodynamic disturbance + # External aerodynamic disturbance applied to velocity if scenario.disturbance_fn is not None: - dist_force = scenario.disturbance_fn(step, current_state) - current_state.velocity += (dist_force / self.env.dynamics.mass) * dt + dist_force = scenario.disturbance_fn(step, pre_state) + self.env.dynamics.state.velocity += (dist_force / self.env.dynamics.mass) * dt + # Step physical dynamics with Runge-Kutta 4 new_state = self.env.dynamics.step_rk4(thrusts_actual) self.env._position = new_state.position.copy() self.env._current_step = step @@ -871,14 +983,35 @@ def run_scenario( est_vel = None pos_est_err = None att_est_err = None - if imu_sensor is not None and state_estimator is not None: - inertial_acc = (new_state.velocity - current_state.velocity) / dt - meas = imu_sensor.measure(current_state, inertial_acceleration=inertial_acc, dt=dt) - est_state = state_estimator.update(meas, dt=dt, true_state=new_state) + if imu_sensor is not None and state_estimator is not None and sensor_rng is not None: + # Calculate true net inertial acceleration between distinct pre and post states + inertial_acc = (new_state.velocity - pre_velocity) / dt + meas = imu_sensor.measure( + new_state, + inertial_acceleration=inertial_acc, + dt=dt, + timestamp=time_sec, + ) + # Avionics sensor fusion: IMU fused with noisy position and altitude observations + pos_obs_noise = sensor_rng.normal(0.0, 0.02, size=3) + noisy_pos_ref = new_state.position + pos_obs_noise + est_state = state_estimator.update( + meas, + dt=dt, + position_ref=noisy_pos_ref, + altitude_ref=float(noisy_pos_ref[2]), + true_state=new_state, + ) est_pos = est_state.position.copy() est_vel = est_state.velocity.copy() pos_est_err = est_state.position_error att_est_err = est_state.attitude_error_deg + est_drone_state = DroneState6DOF( + position=est_state.position.copy(), + velocity=est_state.velocity.copy(), + quaternion=est_state.quaternion.copy(), + angular_velocity=est_state.angular_velocity.copy(), + ) # Obstacle clearance check (combining static and dynamic obstacles) min_obs_d = float("inf") @@ -894,11 +1027,15 @@ def run_scenario( # Collision check coll, coll_type = self.env._check_collision(new_state.position, prev_pos=old_pos) - # Also check dynamic obstacles collisions + # Continuous swept dynamic obstacle collision check if not coll: for dyn_obs in scenario.dynamic_obstacles: - if dyn_obs.distance_to(new_state.position, time_sec) <= ( - dyn_obs.radius + scenario.collision_radius + if dyn_obs.check_continuous_collision( + prev_drone_pos=old_pos, + curr_drone_pos=new_state.position, + prev_time=time_sec - dt, + curr_time=time_sec, + vehicle_radius=scenario.collision_radius, ): coll = True coll_type = "obstacle" @@ -929,13 +1066,17 @@ def run_scenario( is_pos_settled = dist_to_final <= scenario.target_radius is_vel_settled = curr_speed <= scenario.settling_velocity_threshold - is_mission_completed = ( + all_waypoints_completed = ( (mission_manager.state == MissionState.ARRIVED) if mission_manager is not None - else (waypoint_idx == len(waypoints) - 1 and is_pos_settled) + else (waypoint_idx == len(waypoints) - 1 and is_at_curr_waypoint) ) - is_final_success = is_mission_completed and is_vel_settled and not is_collision + is_mission_completed = ( + all_waypoints_completed and is_pos_settled and is_vel_settled and not is_collision + ) + + is_final_success = is_mission_completed # Record telemetry telemetry.append( @@ -966,6 +1107,12 @@ def run_scenario( if coll: break + if ( + scenario.max_duration_seconds is not None + and time_sec >= scenario.max_duration_seconds + ): + break + if is_final_success and len(waypoints) > 1: outcome = "success" break @@ -984,6 +1131,9 @@ def run_scenario( is_pos_settled = final_pos_err <= scenario.target_radius is_vel_settled = final_speed <= scenario.settling_velocity_threshold + is_mission_completed = ( + all_waypoints_completed and is_pos_settled and is_vel_settled and not is_collision + ) # Authoritative, mutually exclusive outcome classification if fault_ever_active and ( @@ -996,17 +1146,21 @@ def run_scenario( outcome = "collision" if coll_type == "obstacle" else "boundary_breach" elif fault_ever_active: # Active actuator fault was injected: - # Requires mission completion, stable flight, altitude maintenance, and velocity settling + # Requires authoritative mission completion, stable flight, altitude maintenance, and velocity settling if ( - is_pos_settled - and is_vel_settled + is_mission_completed and max_tilt < scenario.max_recovery_tilt_deg and min_alt > scenario.min_recovery_altitude ): outcome = "recovered_degraded" + elif ( + scenario.max_duration_seconds is not None + and (step * dt) >= scenario.max_duration_seconds + ): + outcome = "timeout" else: outcome = "max_steps" - elif is_pos_settled and is_vel_settled and not is_collision: + elif is_mission_completed: outcome = "success" elif ( scenario.max_duration_seconds is not None diff --git a/src/adaptive_rl/navigation/dynamic_obstacles.py b/src/adaptive_rl/navigation/dynamic_obstacles.py index 729c2ee..1632e2e 100644 --- a/src/adaptive_rl/navigation/dynamic_obstacles.py +++ b/src/adaptive_rl/navigation/dynamic_obstacles.py @@ -61,6 +61,73 @@ def center_at(self, time_sec: float) -> np.ndarray: reflected[i] = b_max - (val - span) return reflected + def check_continuous_collision( + self, + prev_drone_pos: np.ndarray, + curr_drone_pos: np.ndarray, + prev_time: float, + curr_time: float, + vehicle_radius: float, + ) -> bool: + """Check continuous swept-sphere collision between vehicle and dynamic obstacle over [prev_time, curr_time]. + + Calculates minimum relative distance between moving vehicle segment and moving obstacle trajectory. + Handles arbitrary speeds, cross-trajectories, and boundary bounce reflections. + """ + p0 = np.asarray(prev_drone_pos, dtype=np.float64) + p1 = np.asarray(curr_drone_pos, dtype=np.float64) + t0 = float(prev_time) + t1 = float(curr_time) + dt = t1 - t0 + threshold = float(self.radius + vehicle_radius) + + if dt <= 1e-9: + c = self.center_at(t0) + return bool(np.linalg.norm(p1 - c) <= threshold) + + # Immediate endpoint checks + if np.linalg.norm(p0 - self.center_at(t0)) <= threshold: + return True + if np.linalg.norm(p1 - self.center_at(t1)) <= threshold: + return True + + # Adaptive subdivision to guarantee linear relative subsegment fidelity under high speed or bounce + v_obs_mag = float(np.linalg.norm(self.velocity)) + v_veh_est = float(np.linalg.norm(p1 - p0)) / dt + v_rel_max = max(1.0, v_obs_mag + v_veh_est) + # Substep duration ensures max relative displacement per subsegment <= half the threshold + sub_dt = min(0.01, threshold / (2.0 * v_rel_max)) + num_substeps = max(1, min(50, int(math.ceil(dt / sub_dt)))) + + for k in range(num_substeps): + frac_a = k / num_substeps + frac_b = (k + 1) / num_substeps + tau_a = t0 + frac_a * dt + tau_b = t0 + frac_b * dt + + pa = p0 + frac_a * (p1 - p0) + pb = p0 + frac_b * (p1 - p0) + ca = self.center_at(tau_a) + cb = self.center_at(tau_b) + + # Relative displacement vector r(s) = (pa - ca) + s * ((pb - cb) - (pa - ca)) + ra = pa - ca + rb = pb - cb + d_rel = rb - ra + d_sq = float(np.dot(d_rel, d_rel)) + + if d_sq < 1e-12: + min_dist = float(np.linalg.norm(ra)) + else: + s_opt = max(0.0, min(1.0, -float(np.dot(ra, d_rel)) / d_sq)) + closest_rel = ra + s_opt * d_rel + min_dist = float(np.linalg.norm(closest_rel)) + + if min_dist <= threshold: + return True + + return False + def distance_to(self, point: np.ndarray, time_sec: float) -> float: """Compute the Euclidean distance from a 3D point to the obstacle center at time t.""" c = self.center_at(time_sec) @@ -99,9 +166,11 @@ def predict_occupied_spheres( def to_dict(self) -> Dict[str, Any]: return { - "center_initial": [round(float(x), 4) for x in self.center_initial], - "radius": round(float(self.radius), 4), - "velocity": [round(float(x), 4) for x in self.velocity], - "motion_type": self.motion_type, - "bounds": list(self.bounds), + "center_initial": [float(x) for x in self.center_initial], + "radius": float(self.radius), + "velocity": [float(x) for x in self.velocity], + "bounds": [float(x) for x in self.bounds], + "motion_type": str(self.motion_type), + "oscillation_period": float(self.oscillation_period), + "oscillation_amplitude": [float(x) for x in self.oscillation_amplitude], } diff --git a/src/adaptive_rl/navigation/planner.py b/src/adaptive_rl/navigation/planner.py index 431588b..5a0ee53 100644 --- a/src/adaptive_rl/navigation/planner.py +++ b/src/adaptive_rl/navigation/planner.py @@ -304,18 +304,49 @@ def to_grid(p: np.ndarray) -> Tuple[int, int, int]: def to_world(coord: Tuple[int, int, int]) -> np.ndarray: return np.array([coord[0] * res, coord[1] * res, coord[2] * res], dtype=np.float64) - start_grid = to_grid(start_pt) + base_start = to_grid(start_pt) goal_grid = to_grid(goal_pt) + # 4. Search valid, collision-free candidate grid connections for start_pt + # Examines base grid cell and all 26 spatial neighbors + candidate_offsets = [(0, 0, 0)] + list(self._NEIGHBOR_OFFSETS) + valid_start_nodes: List[Tuple[Tuple[int, int, int], float]] = [] + for off in candidate_offsets: + cand_coord = (base_start[0] + off[0], base_start[1] + off[1], base_start[2] + off[2]) + cand_world = to_world(cand_coord) + v_pt, _ = self.is_point_valid(cand_world, obstacles) + if not v_pt: + continue + v_seg, _ = self.is_segment_valid(start_pt, cand_world, obstacles) + if not v_seg: + continue + dist_cand = float(np.linalg.norm(cand_world - start_pt)) + valid_start_nodes.append((cand_coord, dist_cand)) + + # If no neighboring grid node has a collision-free connection to start_pt, fail safely + if not valid_start_nodes: + elapsed = (time.perf_counter() - t0) * 1000.0 + return PlanningResult( + status="NO_PATH", + planning_time_ms=elapsed, + nodes_expanded=0, + ) + + valid_start_nodes.sort(key=lambda item: item[1]) + # Priority queue item: (f_score, g_score, grid_coord) open_set: List[Tuple[float, float, Tuple[int, int, int]]] = [] - h_start = float(np.linalg.norm(to_world(start_grid) - goal_pt)) - heapq.heappush(open_set, (h_start, 0.0, start_grid)) + came_from: Dict[Tuple[int, int, int], Optional[Tuple[int, int, int]]] = {} + g_scores: Dict[Tuple[int, int, int], float] = {} - came_from: Dict[Tuple[int, int, int], Tuple[int, int, int]] = {} - g_scores: Dict[Tuple[int, int, int], float] = {start_grid: 0.0} - visited: set[Tuple[int, int, int]] = set() + for cand_coord, d_start in valid_start_nodes: + cand_world = to_world(cand_coord) + h_score = float(np.linalg.norm(cand_world - goal_pt)) + g_scores[cand_coord] = d_start + came_from[cand_coord] = None + heapq.heappush(open_set, (d_start + h_score, d_start, cand_coord)) + visited: set[Tuple[int, int, int]] = set() goal_reached_coord: Optional[Tuple[int, int, int]] = None expansions = 0 @@ -328,8 +359,8 @@ def to_world(coord: Tuple[int, int, int]) -> np.ndarray: curr_world = to_world(current) - # Check if close enough to goal or line-of-sight to goal is open - if current == goal_grid or np.linalg.norm(curr_world - goal_pt) <= res: + # Check if line-of-sight to non-grid goal is open + if current == goal_grid or np.linalg.norm(curr_world - goal_pt) <= 2.0 * res: can_see_goal, _ = self.is_segment_valid(curr_world, goal_pt, obstacles) if can_see_goal: goal_reached_coord = current @@ -367,15 +398,23 @@ def to_world(coord: Tuple[int, int, int]) -> np.ndarray: nodes_expanded=expansions, ) - # 5. Reconstruct raw grid path + # 5. Reconstruct raw grid path with provable collision-free continuous connections raw_path: List[np.ndarray] = [goal_pt.copy()] - curr = goal_reached_coord - while curr in came_from: - raw_path.append(to_world(curr)) - curr = came_from[curr] + curr_step: Optional[Tuple[int, int, int]] = goal_reached_coord + while curr_step is not None: + raw_path.append(to_world(curr_step)) + curr_step = came_from.get(curr_step) raw_path.append(start_pt.copy()) raw_path.reverse() + # Deduplicate consecutive waypoints that are identical within tolerance + dedup_path: List[np.ndarray] = [] + for pt in raw_path: + if not dedup_path or np.linalg.norm(pt - dedup_path[-1]) > 1e-6: + dedup_path.append(pt) + if len(dedup_path) >= 2: + raw_path = dedup_path + # Compute raw length raw_len = sum( float(np.linalg.norm(raw_path[i + 1] - raw_path[i])) for i in range(len(raw_path) - 1) diff --git a/tests/test_actuator_aerodynamics.py b/tests/test_actuator_aerodynamics.py index ce1aa3b..612b335 100644 --- a/tests/test_actuator_aerodynamics.py +++ b/tests/test_actuator_aerodynamics.py @@ -240,3 +240,29 @@ def test_gym_env_domain_randomization_on_reset() -> None: _, info2 = env.reset(seed=202) mass2 = env.dynamics.mass assert mass1 != mass2 + + +def test_motor_coupled_velocity_integration_fidelity() -> None: + """Coupled stage-wise RK4 integration tracks exact analytical velocity integral across time steps and tau.""" + for tau in [0.02, 0.04, 0.08]: + for dt in [0.005, 0.01, 0.02]: + dyn = DroneDynamics6DOF( + mass=1.0, + dt=dt, + substeps=1, + enable_motor_dynamics=True, + motor_time_constant=tau, + linear_damping=0.0, + ) + dyn.reset(position=np.array([10.0, 10.0, 10.0]), velocity=np.zeros(3)) + hover_f = dyn.hover_thrust_per_motor + cmd_f = 4.0 + cmd = np.array([cmd_f, cmd_f, cmd_f, cmd_f]) + dyn.step_rk4(cmd) + + v_sim = dyn.state.velocity[2] + delta_f = 4.0 * (cmd_f - hover_f) + # Analytical v(dt) = (delta_F / m) * (dt + tau * (exp(-dt/tau) - 1)) + v_analytical = (delta_f / 1.0) * (dt + tau * (math.exp(-dt / tau) - 1.0)) + rel_err = abs(v_sim - v_analytical) / v_analytical + assert rel_err < 1e-3 diff --git a/tests/test_fault_injection.py b/tests/test_fault_injection.py index 0315d87..5edf2ae 100644 --- a/tests/test_fault_injection.py +++ b/tests/test_fault_injection.py @@ -168,3 +168,54 @@ def test_collision_precedence_over_max_steps() -> None: assert result.metrics.outcome in ("collision", "boundary_breach") assert result.metrics.outcome != "max_steps" assert result.metrics.success is False + + +def test_skipping_intermediate_waypoints_fails_mission() -> None: + """A vehicle located at the final coordinate without completing preceding waypoints is not SUCCESS.""" + + # Scenario: Start at (5, 5, 3), must visit W0=(20, 20, 10), then W1=(5, 5, 3). + # Stationary controller stays at (5, 5, 3). It never completed W0! + class HoverInPlaceController: + def compute_action( + self, state: Any, target_position: np.ndarray, target_velocity: np.ndarray, dt: float + ) -> Tuple[np.ndarray, Dict[str, Any]]: + # Zero normalized action = hover thrust + return np.zeros(4, dtype=np.float32), {} + + sc = FlightScenarioDefinition( + name="waypoint_skipping_test", + description="Verify all waypoints in sequence must be visited", + initial_position=np.array([5.0, 5.0, 3.0]), + waypoints=[ + np.array([20.0, 20.0, 10.0]), # Distant waypoint that must be visited first + np.array([5.0, 5.0, 3.0]), # Final destination matching start coordinate + ], + max_steps=30, + target_radius=1.2, + ) + + runner = FlightScenarioRunner() + result = runner.run_scenario(sc, controller=HoverInPlaceController()) + + # Distance to final waypoint is 0, but W0 was never completed: must NOT be success! + assert result.metrics.outcome != "success" + assert result.metrics.outcome == "max_steps" + assert result.metrics.success is False + + +def test_explicit_timeout_classification() -> None: + """When a scenario has max_duration_seconds and time expires before completion, outcome is timeout.""" + sc = FlightScenarioDefinition( + name="timeout_test", + description="Verify explicit timeout classification", + initial_position=np.array([5.0, 5.0, 5.0]), + waypoints=[np.array([25.0, 25.0, 10.0])], + max_steps=200, + max_duration_seconds=0.5, # Times out after 10 steps (0.5s at 0.05s/step) + ) + + runner = FlightScenarioRunner() + result = runner.run_scenario(sc) + + assert result.metrics.outcome == "timeout" + assert result.metrics.success is False diff --git a/tests/test_flight_recorder.py b/tests/test_flight_recorder.py index 9f9a5dd..a00ee70 100644 --- a/tests/test_flight_recorder.py +++ b/tests/test_flight_recorder.py @@ -25,8 +25,8 @@ def test_canonical_json_dumps_determinism() -> None: s1 = canonical_json_dumps(d1) s2 = canonical_json_dumps(d2) assert s1 == s2 - # Floats are rounded to 6 decimal places for stable hashing - assert "2.123457" in s1 + # Exact finite floating-point representation is preserved without lossy rounding + assert "2.12345678" in s1 h1 = compute_config_fingerprint(d1) h2 = compute_config_fingerprint(d2) diff --git a/tests/test_navigation_astar.py b/tests/test_navigation_astar.py index f37d6a6..c8fd5ee 100644 --- a/tests/test_navigation_astar.py +++ b/tests/test_navigation_astar.py @@ -303,3 +303,48 @@ def test_planner_multiple_resolutions() -> None: for i in range(len(res.waypoints) - 1): valid, _ = planner.is_segment_valid(res.waypoints[i], res.waypoints[i + 1], [obs]) assert valid is True + + +def test_planner_arbitrary_non_grid_aligned_coordinates() -> None: + """Planner handles arbitrary non-integer, non-grid-aligned coordinates across resolutions.""" + for res_val in [0.5, 1.0, 2.0]: + planner = AStarPlanner3D( + bounds=(20.0, 20.0, 10.0), resolution=res_val, vehicle_radius=0.5, safety_margin=0.2 + ) + start = np.array([2.37, 7.82, 4.19]) + goal = np.array([17.63, 8.44, 5.71]) + obs = ObstacleSphere3D(center=np.array([10.0, 8.0, 5.0]), radius=2.2) + + res = planner.plan(start, goal, [obs]) + assert res.status == "SUCCESS" + assert len(res.waypoints) >= 2 + np.testing.assert_allclose(res.waypoints[0], start, atol=1e-5) + np.testing.assert_allclose(res.waypoints[-1], goal, atol=1e-5) + + # Invariant: Every consecutive segment is strictly collision-free + for i in range(len(res.waypoints) - 1): + valid, clear = planner.is_segment_valid(res.waypoints[i], res.waypoints[i + 1], [obs]) + assert valid is True + assert clear >= 0.0 + + +def test_planner_non_grid_start_nearest_node_blocked_finds_neighbor() -> None: + """When the nearest rounded grid node is blocked, planner connects through a collision-free neighbor.""" + planner = AStarPlanner3D( + bounds=(20.0, 20.0, 10.0), resolution=2.0, vehicle_radius=0.4, safety_margin=0.1 + ) + # Grid resolution is 2.0. Base node for (4.8, 4.8, 4.0) is (4.0, 4.0, 4.0). + # Place obstacle at (4.0, 4.0, 4.0) with radius 0.5 (inflated radius 1.0). + # Node (4.0, 4.0, 4.0) is blocked. + # Start (4.8, 4.8, 4.0) is at distance sqrt(0.8^2 + 0.8^2) = 1.131 > 1.0 (valid). + obs = ObstacleSphere3D(center=np.array([4.0, 4.0, 4.0]), radius=0.5) + start = np.array([4.8, 4.8, 4.0]) + goal = np.array([12.0, 12.0, 4.0]) + + res = planner.plan(start, goal, [obs]) + assert res.status == "SUCCESS" + np.testing.assert_allclose(res.waypoints[0], start, atol=1e-5) + np.testing.assert_allclose(res.waypoints[-1], goal, atol=1e-5) + for i in range(len(res.waypoints) - 1): + valid, _ = planner.is_segment_valid(res.waypoints[i], res.waypoints[i + 1], [obs]) + assert valid is True diff --git a/tests/test_navigation_dynamic_obstacles.py b/tests/test_navigation_dynamic_obstacles.py new file mode 100644 index 0000000..86d5adf --- /dev/null +++ b/tests/test_navigation_dynamic_obstacles.py @@ -0,0 +1,190 @@ +"""Unit tests for continuous dynamic obstacle collision detection and predictive evasion (Phase III, Task 5).""" + +from __future__ import annotations + +import numpy as np + +from adaptive_rl.navigation.dynamic_obstacles import DynamicObstacleSphere3D +from adaptive_rl.navigation.mission import MissionManager + + +def test_stationary_drone_crossed_by_moving_obstacle() -> None: + """A stationary drone crossed by a moving obstacle between endpoints must register collision.""" + obs = DynamicObstacleSphere3D( + center_initial=np.array([0.0, 10.0, 5.0]), + radius=1.0, + velocity=np.array([20.0, 0.0, 0.0]), # Translates from x=0 to x=20 in 1 second + bounds=(50.0, 50.0, 20.0), + motion_type="bounce", + ) + drone_pos = np.array([10.0, 10.0, 5.0]) + vehicle_radius = 0.5 + + t0 = 0.0 + t1 = 1.0 + + # At t0=0, obs is at x=0 (dist=10 > 1.5) + # At t1=1, obs is at x=20 (dist=10 > 1.5) + # Neither endpoint registers collision + assert obs.distance_to(drone_pos, t0) > (obs.radius + vehicle_radius) + assert obs.distance_to(drone_pos, t1) > (obs.radius + vehicle_radius) + + # Continuous swept check must detect collision at t=0.5s when obstacle passes through x=10 + coll = obs.check_continuous_collision( + prev_drone_pos=drone_pos, + curr_drone_pos=drone_pos, + prev_time=t0, + curr_time=t1, + vehicle_radius=vehicle_radius, + ) + assert coll is True + + +def test_moving_drone_crossing_stationary_obstacle() -> None: + """A moving drone crossing a stationary obstacle between endpoints must register collision.""" + obs = DynamicObstacleSphere3D( + center_initial=np.array([10.0, 10.0, 5.0]), + radius=1.0, + velocity=np.zeros(3), + bounds=(50.0, 50.0, 20.0), + ) + vehicle_radius = 0.5 + + p0 = np.array([0.0, 10.0, 5.0]) + p1 = np.array([20.0, 10.0, 5.0]) + t0 = 0.0 + t1 = 1.0 + + assert obs.distance_to(p0, t0) > (obs.radius + vehicle_radius) + assert obs.distance_to(p1, t1) > (obs.radius + vehicle_radius) + + coll = obs.check_continuous_collision( + prev_drone_pos=p0, + curr_drone_pos=p1, + prev_time=t0, + curr_time=t1, + vehicle_radius=vehicle_radius, + ) + assert coll is True + + +def test_two_moving_objects_crossing_between_step_endpoints() -> None: + """Two moving objects crossing between step endpoints must register collision even if endpoints are clear.""" + # Drone moving along Y from (10, 0, 5) to (10, 20, 5) in 1s + p0 = np.array([10.0, 0.0, 5.0]) + p1 = np.array([10.0, 20.0, 5.0]) + + # Obstacle moving along X from (0, 10, 5) to (20, 10, 5) in 1s + obs = DynamicObstacleSphere3D( + center_initial=np.array([0.0, 10.0, 5.0]), + radius=0.8, + velocity=np.array([20.0, 0.0, 0.0]), + bounds=(50.0, 50.0, 20.0), + ) + vehicle_radius = 0.5 + t0 = 0.0 + t1 = 1.0 + + # At t0=0: drone=(10, 0, 5), obs=(0, 10, 5), dist = sqrt(100+100) = 14.14 > 1.3 + # At t1=1: drone=(10, 20, 5), obs=(20, 10, 5), dist = sqrt(100+100) = 14.14 > 1.3 + assert np.linalg.norm(p0 - obs.center_at(t0)) > 1.3 + assert np.linalg.norm(p1 - obs.center_at(t1)) > 1.3 + + # At t=0.5: drone=(10, 10, 5), obs=(10, 10, 5), collision! + coll = obs.check_continuous_collision( + prev_drone_pos=p0, + curr_drone_pos=p1, + prev_time=t0, + curr_time=t1, + vehicle_radius=vehicle_radius, + ) + assert coll is True + + +def test_high_speed_obstacle_swept_detection() -> None: + """High-speed obstacle (40 m/s) over a short interval (0.05s) does not tunnel through the aircraft.""" + obs = DynamicObstacleSphere3D( + center_initial=np.array([8.0, 10.0, 5.0]), + radius=0.6, + velocity=np.array([40.0, 0.0, 0.0]), # 2.0 meters displacement in 0.05s + bounds=(50.0, 50.0, 20.0), + ) + drone_pos = np.array([9.0, 10.0, 5.0]) + vehicle_radius = 0.3 + + t0 = 0.0 + t1 = 0.05 # Obstacle moves from x=8.0 to x=10.0; drone is at x=9.0 + + coll = obs.check_continuous_collision( + prev_drone_pos=drone_pos, + curr_drone_pos=drone_pos, + prev_time=t0, + curr_time=t1, + vehicle_radius=vehicle_radius, + ) + assert coll is True + + +def test_safe_parallel_trajectories_no_false_collision() -> None: + """Safe parallel trajectories with clearance never trigger false positive collision.""" + p0 = np.array([5.0, 0.0, 5.0]) + p1 = np.array([5.0, 20.0, 5.0]) + + obs = DynamicObstacleSphere3D( + center_initial=np.array([12.0, 0.0, 5.0]), # 7 meters lateral separation + radius=1.0, + velocity=np.array([0.0, 20.0, 0.0]), + bounds=(50.0, 50.0, 20.0), + ) + + coll = obs.check_continuous_collision( + prev_drone_pos=p0, + curr_drone_pos=p1, + prev_time=0.0, + curr_time=1.0, + vehicle_radius=0.5, + ) + assert coll is False + + +def test_dynamic_obstacle_predictive_evasion_replanning() -> None: + """MissionManager predictive trajectory checking detects collision and triggers autonomous replanning.""" + from adaptive_rl.environments.drone_6dof import DroneState6DOF + from adaptive_rl.navigation.planner import AStarPlanner3D + + # Target path from (5, 10, 5) to (25, 10, 5) + planner = AStarPlanner3D(bounds=(30.0, 30.0, 15.0), resolution=1.0) + mgr = MissionManager(planner=planner) + success = mgr.start_mission( + start_position=np.array([5.0, 10.0, 5.0]), + goals=[np.array([25.0, 10.0, 5.0])], + static_obstacles=[], + dynamic_obstacles=[], + ) + assert success is True + + # Dynamic obstacle crossing path at x=15, y=5 moving towards y=15 + dyn_obs = DynamicObstacleSphere3D( + center_initial=np.array([15.0, 5.0, 5.0]), + radius=1.5, + velocity=np.array([0.0, 2.0, 0.0]), + bounds=(30.0, 30.0, 15.0), + ) + + state = DroneState6DOF( + position=np.array([5.0, 10.0, 5.0]), + velocity=np.zeros(3), + quaternion=np.array([1.0, 0.0, 0.0, 0.0]), + angular_velocity=np.zeros(3), + ) + + # At t=2.0s, obstacle reaches y=9.0 (blocking the path at x=15, y=10) + current_target, telem = mgr.update( + drone_state=state, + time_sec=2.0, + static_obstacles=[], + dynamic_obstacles=[dyn_obs], + ) + + # MissionManager should detect the predicted intrusion and successfully replan + assert telem.replan_count >= 1 diff --git a/tests/test_recorder_fingerprint_phase3.py b/tests/test_recorder_fingerprint_phase3.py index ba1f4f6..fc9f722 100644 --- a/tests/test_recorder_fingerprint_phase3.py +++ b/tests/test_recorder_fingerprint_phase3.py @@ -187,3 +187,97 @@ def test_small_damping_and_inertia_float_precision() -> None: d = {"angular_damping": 1.25e-6, "mass": 1.0} serialized = canonical_json_dumps(d) assert "1.25e-06" in serialized or "0.00000125" in serialized or "1.25e-6" in serialized + + +def test_sub_microgram_mass_distinct_fingerprints() -> None: + """Distinct floating-point mass differences at sub-microgram scale (1e-9 kg) produce distinct digests.""" + d1 = {"mass": 1.0} + d2 = {"mass": 1.000000001} + fp1 = compute_config_fingerprint(d1) + fp2 = compute_config_fingerprint(d2) + assert fp1 != fp2 + + # Multiple serialization passes remain strictly deterministic + assert compute_config_fingerprint(d1) == fp1 + assert compute_config_fingerprint(d2) == fp2 + + +def test_sub_millimeter_obstacle_position_distinct_fingerprints() -> None: + """Sub-millimeter geometry shifts produce distinct cryptographic fingerprints.""" + from adaptive_rl.environments.drone import ObstacleSphere3D + from adaptive_rl.flight_testing.scenarios import FlightScenarioDefinition + + sc1 = FlightScenarioDefinition( + name="obs_sc", + description="Obstacle test", + initial_position=np.array([0.0, 0.0, 0.0]), + waypoints=[np.array([10.0, 10.0, 5.0])], + obstacles=[ObstacleSphere3D(center=np.array([5.0, 5.0, 5.0]), radius=2.0)], + ) + + sc2 = FlightScenarioDefinition( + name="obs_sc", + description="Obstacle test", + initial_position=np.array([0.0, 0.0, 0.0]), + waypoints=[np.array([10.0, 10.0, 5.0])], + obstacles=[ + ObstacleSphere3D(center=np.array([5.00005, 5.0, 5.0]), radius=2.0) + ], # 50 microns shift + ) + + fp1 = compute_config_fingerprint(sc1.to_dict()) + fp2 = compute_config_fingerprint(sc2.to_dict()) + assert fp1 != fp2 + + +def test_nan_inf_deterministic_handling() -> None: + """Non-finite floats (NaN, Inf, -Inf) are deterministically serialized and hashed.""" + d_nan = {"val": float("nan")} + d_inf = {"val": float("inf")} + d_neginf = {"val": float("-inf")} + + s_nan1 = canonical_json_dumps(d_nan) + s_nan2 = canonical_json_dumps(d_nan) + assert s_nan1 == s_nan2 + assert '"val":"NaN"' in s_nan1 + + s_inf = canonical_json_dumps(d_inf) + assert '"val":"Infinity"' in s_inf + + s_neginf = canonical_json_dumps(d_neginf) + assert '"val":"-Infinity"' in s_neginf + + +def test_scenario_provenance_invalidation() -> None: + """Changing waypoint, obstacle, or controller parameter invalidates scenario fingerprint.""" + from adaptive_rl.environments.drone import ObstacleSphere3D + from adaptive_rl.flight_testing.scenarios import FlightScenarioDefinition + + base_sc = FlightScenarioDefinition( + name="base_sc", + description="Base scenario", + initial_position=np.array([1.0, 2.0, 3.0]), + waypoints=[np.array([10.0, 10.0, 5.0]), np.array([20.0, 20.0, 8.0])], + obstacles=[ObstacleSphere3D(center=np.array([5.0, 5.0, 4.0]), radius=1.5)], + ) + fp_base = compute_config_fingerprint(base_sc.to_dict()) + + # 1. Waypoint shift + alt_wp = FlightScenarioDefinition( + name="base_sc", + description="Base scenario", + initial_position=np.array([1.0, 2.0, 3.0]), + waypoints=[np.array([10.0, 10.0, 5.0]), np.array([20.0, 20.0, 8.0001])], + obstacles=[ObstacleSphere3D(center=np.array([5.0, 5.0, 4.0]), radius=1.5)], + ) + assert compute_config_fingerprint(alt_wp.to_dict()) != fp_base + + # 2. Obstacle shift + alt_obs = FlightScenarioDefinition( + name="base_sc", + description="Base scenario", + initial_position=np.array([1.0, 2.0, 3.0]), + waypoints=[np.array([10.0, 10.0, 5.0]), np.array([20.0, 20.0, 8.0])], + obstacles=[ObstacleSphere3D(center=np.array([5.0, 5.0, 4.0001]), radius=1.5)], + ) + assert compute_config_fingerprint(alt_obs.to_dict()) != fp_base diff --git a/tests/test_sensor_estimation.py b/tests/test_sensor_estimation.py index f823d7b..6223f4f 100644 --- a/tests/test_sensor_estimation.py +++ b/tests/test_sensor_estimation.py @@ -113,3 +113,77 @@ def test_complementary_filter_position_altitude_tracking() -> None: est = filt.update(meas, dt=0.05, altitude_ref=5.0, true_state=true_state) assert math.isclose(est.position[2], 5.0, abs_tol=0.2) + + +def test_moving_aircraft_nonzero_inertial_acceleration_no_aliasing() -> None: + """Pre-integration state snapshotting ensures moving aircraft measures nonzero inertial acceleration.""" + from adaptive_rl.flight_testing.scenarios import ( + FlightScenarioDefinition, + FlightScenarioRunner, + ) + + runner = FlightScenarioRunner() + sc = FlightScenarioDefinition( + name="accel_test", + description="Verify nonzero acceleration without state object aliasing", + initial_position=np.array([5.0, 5.0, 5.0]), + initial_velocity=np.zeros(3), + waypoints=[np.array([10.0, 5.0, 5.0])], + max_steps=15, + enable_sensor_estimation=True, + sensor_seed=42, + ) + + result = runner.run_scenario(sc) + assert len(result.telemetry) == 15 + + # During the first few steps while accelerating towards x=10, velocity changes + # and estimated states must be recorded + v_changes = [ + float(np.linalg.norm(result.telemetry[i].velocity - result.telemetry[i - 1].velocity)) + for i in range(1, len(result.telemetry)) + ] + assert max(v_changes) > 1e-4 + + # Verify that telemetry has valid estimation error fields + for t in result.telemetry: + assert t.estimated_position is not None + assert t.estimated_velocity is not None + assert t.position_estimation_error is not None + assert t.attitude_estimation_error_deg is not None + + +def test_imu_rotational_body_frame_gyroscope() -> None: + """Gyroscope correctly measures body-frame angular rate under arbitrary attitude orientation.""" + q_pitch45 = euler_to_quaternion(roll=0.0, pitch=math.radians(45.0), yaw=0.0) + omega_body = np.array([0.5, -0.3, 1.2]) + + imu = IMUSensor(gravity=9.81, accel_noise_std=0.0, gyro_noise_std=0.0, bias_drift_std=0.0) + state = DroneState6DOF( + position=np.array([0.0, 0.0, 5.0]), + velocity=np.zeros(3), + quaternion=q_pitch45, + angular_velocity=omega_body, + ) + + meas = imu.measure(state, inertial_acceleration=np.zeros(3), dt=0.01) + np.testing.assert_allclose(meas.gyroscope, omega_body, atol=1e-5) + + +def test_sensor_in_the_loop_flight_execution() -> None: + """Closed loop flight driven entirely by estimated state successfully tracks waypoints.""" + from adaptive_rl.flight_testing.scenarios import ( + FlightScenarioRunner, + get_scenario, + ) + + runner = FlightScenarioRunner() + sc = get_scenario("sensor_estimated_navigation") + assert sc.sensor_in_the_loop is True + assert sc.enable_sensor_estimation is True + + result = runner.run_scenario(sc) + assert result.metrics.outcome == "success" + assert result.metrics.final_position_error <= sc.target_radius + final_speed = float(np.linalg.norm(result.telemetry[-1].velocity)) + assert final_speed <= sc.settling_velocity_threshold From 292afb37bd9974d7b68e5e4260292b11943b5198 Mon Sep 17 00:00:00 2001 From: Aryan Saxena Date: Sun, 15 Nov 2026 21:17:20 +0530 Subject: [PATCH 3/7] fix(phase3): enforce sequential waypoints during replan, conservative dynamic collision, complete provenance inference, and branch CI --- .github/workflows/ci.yml | 4 +- src/adaptive_rl/flight_testing/__init__.py | 4 + src/adaptive_rl/flight_testing/recorder.py | 59 +++++- src/adaptive_rl/flight_testing/scenarios.py | 112 +++++++++-- .../navigation/dynamic_obstacles.py | 78 ++++++-- src/adaptive_rl/navigation/mission.py | 72 ++++++- tests/test_navigation_dynamic_obstacles.py | 186 ++++++++++++++++++ tests/test_recorder_fingerprint_phase3.py | 120 +++++++++++ 8 files changed, 590 insertions(+), 45 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 6f0a8eb..d6d373e 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -2,9 +2,9 @@ name: CI on: push: - branches: [main] + branches: [main, "feat/**", "fix/**"] pull_request: - branches: [main] + branches: [main, "feat/**", "fix/**"] permissions: contents: read diff --git a/src/adaptive_rl/flight_testing/__init__.py b/src/adaptive_rl/flight_testing/__init__.py index 7095f6b..4f4aabe 100644 --- a/src/adaptive_rl/flight_testing/__init__.py +++ b/src/adaptive_rl/flight_testing/__init__.py @@ -3,6 +3,7 @@ from __future__ import annotations from adaptive_rl.flight_testing.recorder import ( + ExperimentConfigurationFingerprint, ExperimentMetadata, FlightRecord, FlightRecorder, @@ -16,6 +17,7 @@ FlightScenarioResult, FlightScenarioRunner, FlightStepTelemetry, + extract_callable_provenance, get_advanced_navigation_scenarios, get_all_flight_scenarios, get_scenario, @@ -23,6 +25,7 @@ ) __all__ = [ + "ExperimentConfigurationFingerprint", "ExperimentMetadata", "FlightRecord", "FlightRecorder", @@ -34,6 +37,7 @@ "PhysicsSimulationFingerprint", "canonical_json_dumps", "compute_config_fingerprint", + "extract_callable_provenance", "get_advanced_navigation_scenarios", "get_all_flight_scenarios", "get_scenario", diff --git a/src/adaptive_rl/flight_testing/recorder.py b/src/adaptive_rl/flight_testing/recorder.py index 5209737..d18f96a 100644 --- a/src/adaptive_rl/flight_testing/recorder.py +++ b/src/adaptive_rl/flight_testing/recorder.py @@ -143,6 +143,7 @@ class ExperimentConfigurationFingerprint: controller_gains: Dict[str, float] = field(default_factory=dict) scenarios: List[Dict[str, Any]] = field(default_factory=list) extra_parameters: Dict[str, Any] = field(default_factory=dict) + provenance_complete: bool = True def to_dict(self) -> Dict[str, Any]: return { @@ -156,6 +157,7 @@ def to_dict(self) -> Dict[str, Any]: "controller_type": self.controller_type, "controller_gains": self.controller_gains, "extra_parameters": self.extra_parameters, + "provenance_complete": self.provenance_complete, } @@ -196,6 +198,7 @@ class FlightRecord: trajectories: Dict[str, List[Dict[str, Any]]] = field(default_factory=dict) configuration: Optional[ExperimentConfigurationFingerprint] = None aggregate_summary: Dict[str, Any] = field(default_factory=dict) + provenance_complete: bool = True signature: Optional[str] = None # Optional asymmetric HMAC / digital signature def to_dict(self, include_digest: bool = True) -> Dict[str, Any]: @@ -211,6 +214,7 @@ def to_dict(self, include_digest: bool = True) -> Dict[str, Any]: "scenario_metrics": [m.to_dict() for m in self.scenario_metrics], "trajectories": self.trajectories, "aggregate_summary": self.aggregate_summary, + "provenance_complete": self.provenance_complete, "signature": self.signature, } if self.configuration is not None: @@ -316,13 +320,47 @@ def create_record( chk_hash = "not_found" # Serialize scenario provenance definitions - raw_scenarios = scenarios if scenarios is not None else physics_params.get("scenarios", []) scenario_provenance: List[Dict[str, Any]] = [] - for sc in raw_scenarios: - if hasattr(sc, "to_dict"): - scenario_provenance.append(sc.to_dict()) - elif isinstance(sc, dict): - scenario_provenance.append(sc) + provenance_complete: bool = True + + raw_scenarios = ( + scenarios if scenarios is not None else physics_params.get("scenarios", None) + ) + + if raw_scenarios is not None: + for sc in raw_scenarios: + if hasattr(sc, "to_dict"): + scenario_provenance.append(sc.to_dict()) + elif isinstance(sc, dict): + scenario_provenance.append(sc) + else: + # Automatically infer scenario definitions from scenario_results + for r in scenario_results: + sc_def = getattr(r, "scenario_definition", None) + if sc_def is not None and hasattr(sc_def, "to_dict"): + scenario_provenance.append(sc_def.to_dict()) + else: + name = getattr(r, "scenario_name", None) + if name: + try: + from adaptive_rl.flight_testing.scenarios import get_scenario + + sc_obj = get_scenario(name) + scenario_provenance.append(sc_obj.to_dict()) + except Exception: + provenance_complete = False + else: + provenance_complete = False + + scenario_provenance.sort(key=lambda s: str(s.get("name", ""))) + + # Verify whether all scenario_names have corresponding provenance + expected_scenario_names = set(m.scenario_name for m in metrics_list) + found_scenario_names = set(s.get("name") for s in scenario_provenance if "name" in s) + if expected_scenario_names and not expected_scenario_names.issubset(found_scenario_names): + provenance_complete = False + if not expected_scenario_names and not scenario_provenance: + provenance_complete = True # Build comprehensive ExperimentConfigurationFingerprint if configuration is not None: @@ -348,6 +386,7 @@ def create_record( controller_gains=dict(physics_params.get("controller_gains", {})), scenarios=scenario_provenance, extra_parameters=dict(physics_params.get("extra_parameters", {})), + provenance_complete=provenance_complete, ) config_hash = compute_config_fingerprint(exp_config.to_dict()) @@ -412,9 +451,10 @@ def create_record( "random_seed": random_seed, "scenario_metrics": [m.to_dict() for m in metrics_list], "trajectories": collected_trajectories, - "configuration": exp_config.to_dict(), "aggregate_summary": agg_summary, + "provenance_complete": provenance_complete, "signature": signature, + "configuration": exp_config.to_dict(), } rec_digest = compute_record_digest(record_pre) @@ -432,6 +472,7 @@ def create_record( trajectories=collected_trajectories, configuration=exp_config, aggregate_summary=agg_summary, + provenance_complete=provenance_complete, signature=signature, ) @@ -545,7 +586,9 @@ def load_record_json(filepath: Path | str) -> FlightRecord: scenario_names=list(cfg_data.get("scenario_names", [])), controller_type=str(cfg_data.get("controller_type", "QuadrotorPIDController")), controller_gains=dict(cfg_data.get("controller_gains", {})), + scenarios=list(cfg_data.get("scenarios", [])), extra_parameters=dict(cfg_data.get("extra_parameters", {})), + provenance_complete=bool(cfg_data.get("provenance_complete", True)), ) else: configuration = ExperimentConfigurationFingerprint( @@ -555,6 +598,7 @@ def load_record_json(filepath: Path | str) -> FlightRecord: checkpoint_hash=data.get("checkpoint_hash"), random_seed=data.get("random_seed"), scenario_names=sorted([m.scenario_name for m in metrics_list]), + provenance_complete=bool(data.get("provenance_complete", True)), ) record = FlightRecord( @@ -571,6 +615,7 @@ def load_record_json(filepath: Path | str) -> FlightRecord: trajectories=trajectories, configuration=configuration, aggregate_summary=data.get("aggregate_summary", {}), + provenance_complete=bool(data.get("provenance_complete", True)), signature=data.get("signature"), ) diff --git a/src/adaptive_rl/flight_testing/scenarios.py b/src/adaptive_rl/flight_testing/scenarios.py index 9bc1c56..f49491a 100644 --- a/src/adaptive_rl/flight_testing/scenarios.py +++ b/src/adaptive_rl/flight_testing/scenarios.py @@ -36,6 +36,7 @@ from __future__ import annotations import csv +import hashlib import json import logging import math @@ -166,6 +167,7 @@ class FlightScenarioResult: description: str metrics: FlightScenarioMetrics telemetry: List[FlightStepTelemetry] = field(default_factory=list) + scenario_definition: Optional[FlightScenarioDefinition] = None def to_dict(self, include_telemetry: bool = True) -> Dict[str, Any]: data: Dict[str, Any] = { @@ -173,6 +175,8 @@ def to_dict(self, include_telemetry: bool = True) -> Dict[str, Any]: "description": self.description, "metrics": self.metrics.to_dict(), } + if self.scenario_definition is not None: + data["scenario_definition"] = self.scenario_definition.to_dict() if include_telemetry: data["telemetry"] = [step.to_dict() for step in self.telemetry] return data @@ -273,6 +277,80 @@ def export_telemetry_csv(self, filepath: Path | str) -> Path: return path +def extract_callable_provenance(fn: Any) -> Dict[str, Any]: + """Extract a stable, deterministic provenance record for a callable. + + Captures function identity, module, qualified name, explicit metadata, + custom attributes, closure variables, and a SHA-256 digest of bytecode and constants. + Guarantees different callback configurations generate different digests. + """ + if fn is None: + return {} + + info: Dict[str, Any] = { + "name": getattr(fn, "__name__", type(fn).__name__), + "qualname": getattr(fn, "__qualname__", getattr(fn, "__name__", str(type(fn)))), + "callable_type": type(fn).__name__, + } + if hasattr(fn, "__module__") and fn.__module__: + info["module"] = str(fn.__module__) + + # Explicit metadata or docstring + if hasattr(fn, "metadata"): + meta = getattr(fn, "metadata") + if isinstance(meta, dict): + info["metadata"] = {str(k): v for k, v in sorted(meta.items())} + else: + info["metadata"] = str(meta) + elif getattr(fn, "__doc__", None): + info["doc"] = fn.__doc__.strip() + + # Custom attributes on the callable + if hasattr(fn, "__dict__"): + custom_attrs = { + k: v for k, v in fn.__dict__.items() if not k.startswith("__") and k != "metadata" + } + if custom_attrs: + info["attributes"] = {str(k): str(v) for k, v in sorted(custom_attrs.items())} + + # Closure variables + closure_vars: Dict[str, Any] = {} + if hasattr(fn, "__closure__") and fn.__closure__ is not None and hasattr(fn, "__code__"): + co_freevars = getattr(fn.__code__, "co_freevars", ()) + for var_name, cell in zip(co_freevars, fn.__closure__): + try: + contents = cell.cell_contents + if isinstance(contents, (int, float, bool, str)): + closure_vars[var_name] = contents + elif isinstance(contents, np.ndarray): + closure_vars[var_name] = contents.tolist() + elif isinstance(contents, (list, tuple)): + closure_vars[var_name] = list(contents) + elif isinstance(contents, dict): + closure_vars[var_name] = {str(k): v for k, v in sorted(contents.items())} + else: + closure_vars[var_name] = str(contents) + except ValueError: + closure_vars[var_name] = "" + if closure_vars: + info["closure"] = {k: closure_vars[k] for k in sorted(closure_vars.keys())} + + # Bytecode and constants digest + if hasattr(fn, "__code__"): + code = fn.__code__ + hasher = hashlib.sha256() + hasher.update(getattr(code, "co_code", b"")) + co_consts = getattr(code, "co_consts", ()) + for const in co_consts: + if hasattr(const, "co_code"): + hasher.update(const.co_code) + else: + hasher.update(str(const).encode("utf-8", errors="replace")) + info["code_hash"] = hasher.hexdigest() + + return info + + @dataclass class FlightScenarioDefinition: """Specification of an autonomous flight validation scenario.""" @@ -337,21 +415,11 @@ def to_dict(self) -> Dict[str, Any]: fault_info: Dict[str, Any] = {} if self.actuator_fault_fn is not None: - fn_name = getattr(self.actuator_fault_fn, "__name__", "custom_fault") - fault_info = {"name": fn_name} - if hasattr(self.actuator_fault_fn, "metadata"): - fault_info["metadata"] = getattr(self.actuator_fault_fn, "metadata") - elif self.actuator_fault_fn.__doc__: - fault_info["doc"] = self.actuator_fault_fn.__doc__.strip() + fault_info = extract_callable_provenance(self.actuator_fault_fn) dist_info: Dict[str, Any] = {} if self.disturbance_fn is not None: - fn_name = getattr(self.disturbance_fn, "__name__", "custom_disturbance") - dist_info = {"name": fn_name} - if hasattr(self.disturbance_fn, "metadata"): - dist_info["metadata"] = getattr(self.disturbance_fn, "metadata") - elif self.disturbance_fn.__doc__: - dist_info["doc"] = self.disturbance_fn.__doc__.strip() + dist_info = extract_callable_provenance(self.disturbance_fn) return { "name": self.name, @@ -840,6 +908,7 @@ def run_scenario( description=scenario.description, metrics=metrics, telemetry=[], + scenario_definition=scenario, ) waypoints = [w.copy() for w in mission_manager.active_path] else: @@ -1061,13 +1130,20 @@ def run_scenario( ctrl.set_target(current_target) # Final arrival and settling check - dist_to_final = float(np.linalg.norm(waypoints[-1] - new_state.position)) + final_coord = ( + scenario.waypoints[-1] if scenario.waypoints else scenario.initial_position + ) + dist_to_final = float(np.linalg.norm(final_coord - new_state.position)) curr_speed = float(np.linalg.norm(new_state.velocity)) is_pos_settled = dist_to_final <= scenario.target_radius is_vel_settled = curr_speed <= scenario.settling_velocity_threshold all_waypoints_completed = ( - (mission_manager.state == MissionState.ARRIVED) + ( + mission_manager.state == MissionState.ARRIVED + and mission_manager.current_mission_waypoint_idx + >= len(mission_manager.mission_waypoints) + ) if mission_manager is not None else (waypoint_idx == len(waypoints) - 1 and is_at_curr_waypoint) ) @@ -1120,7 +1196,12 @@ def run_scenario( outcome = "success" break - final_pos_err = float(np.linalg.norm(waypoints[-1] - self.env._position)) + final_pos_err = float( + np.linalg.norm( + (scenario.waypoints[-1] if scenario.waypoints else scenario.initial_position) + - self.env._position + ) + ) final_speed = float(np.linalg.norm(self.env.dynamics.state.velocity)) max_tilt = max(att_errors_deg) if att_errors_deg else 0.0 min_alt = ( @@ -1206,6 +1287,7 @@ def run_scenario( description=scenario.description, metrics=metrics, telemetry=telemetry, + scenario_definition=scenario, ) def run_all_scenarios( diff --git a/src/adaptive_rl/navigation/dynamic_obstacles.py b/src/adaptive_rl/navigation/dynamic_obstacles.py index 1632e2e..6d64dae 100644 --- a/src/adaptive_rl/navigation/dynamic_obstacles.py +++ b/src/adaptive_rl/navigation/dynamic_obstacles.py @@ -36,6 +36,14 @@ def __post_init__(self) -> None: self.velocity = np.asarray(self.velocity, dtype=np.float64) self.oscillation_amplitude = np.asarray(self.oscillation_amplitude, dtype=np.float64) + def max_speed(self) -> float: + """Calculate the theoretical maximum translation speed of the obstacle.""" + if self.motion_type == "oscillate": + omega = 2.0 * math.pi / max(0.1, self.oscillation_period) + amp_mag = float(np.linalg.norm(self.oscillation_amplitude)) + return omega * amp_mag + return float(np.linalg.norm(self.velocity)) + def center_at(self, time_sec: float) -> np.ndarray: """Compute the deterministic 3D position of the obstacle at a given simulation time.""" t = float(time_sec) @@ -72,7 +80,7 @@ def check_continuous_collision( """Check continuous swept-sphere collision between vehicle and dynamic obstacle over [prev_time, curr_time]. Calculates minimum relative distance between moving vehicle segment and moving obstacle trajectory. - Handles arbitrary speeds, cross-trajectories, and boundary bounce reflections. + Conservative collision evaluation accounts for secant chord approximation errors and high speeds. """ p0 = np.asarray(prev_drone_pos, dtype=np.float64) p1 = np.asarray(curr_drone_pos, dtype=np.float64) @@ -91,13 +99,27 @@ def check_continuous_collision( if np.linalg.norm(p1 - self.center_at(t1)) <= threshold: return True - # Adaptive subdivision to guarantee linear relative subsegment fidelity under high speed or bounce - v_obs_mag = float(np.linalg.norm(self.velocity)) + # Adaptive subdivision based on valid maximum speed + v_obs_mag = self.max_speed() v_veh_est = float(np.linalg.norm(p1 - p0)) / dt v_rel_max = max(1.0, v_obs_mag + v_veh_est) - # Substep duration ensures max relative displacement per subsegment <= half the threshold + + # Substep duration ensures relative displacement per subsegment <= half the threshold sub_dt = min(0.01, threshold / (2.0 * v_rel_max)) - num_substeps = max(1, min(50, int(math.ceil(dt / sub_dt)))) + max_substeps = 500 + desired_substeps = max(1, int(math.ceil(dt / sub_dt))) + num_substeps = min(max_substeps, desired_substeps) + + # Theoretical secant approximation error bound eps_sec over subsegment tau_step + tau_step = dt / num_substeps + if self.motion_type == "oscillate": + omega = 2.0 * math.pi / max(0.1, self.oscillation_period) + amp_mag = float(np.linalg.norm(self.oscillation_amplitude)) + eps_sec = (1.0 / 8.0) * (tau_step**2) * (omega**2) * amp_mag + else: + eps_sec = v_obs_mag * tau_step + + effective_threshold = threshold + eps_sec for k in range(num_substeps): frac_a = k / num_substeps @@ -123,7 +145,17 @@ def check_continuous_collision( closest_rel = ra + s_opt * d_rel min_dist = float(np.linalg.norm(closest_rel)) - if min_dist <= threshold: + if min_dist <= effective_threshold: + return True + + # When computational limits are reached (desired_substeps > max_substeps), + # fail conservatively if swept bounding volumes intersect. + if desired_substeps > max_substeps: + mid_drone = 0.5 * (p0 + p1) + mid_obs = 0.5 * (self.center_at(t0) + self.center_at(t1)) + envelope_dist = float(np.linalg.norm(mid_drone - mid_obs)) + swept_bound = threshold + 0.5 * v_rel_max * dt + if envelope_dist <= swept_bound: return True return False @@ -158,11 +190,35 @@ def predict_occupied_spheres( horizon_sec: float = 3.0, dt: float = 0.5, ) -> List[ObstacleSphere3D]: - """Generate static spherical obstacle approximations covering the predicted trajectory horizon.""" - return [ - ObstacleSphere3D(center=pt, radius=self.radius) - for _, pt in self.predict_trajectory(t_start, horizon_sec, dt) - ] + """Generate static spherical obstacle approximations covering the predicted continuous swept volume. + + Guarantees that consecutive spheres overlap with spacing <= 1.2 * radius, + leaving zero gaps in the predicted flight corridor. + """ + raw_traj = self.predict_trajectory(t_start, horizon_sec, dt) + if not raw_traj: + return [] + + spheres: List[ObstacleSphere3D] = [] + max_spacing = max(0.05, 1.2 * self.radius) + + for i in range(len(raw_traj) - 1): + t_curr, c_curr = raw_traj[i] + t_next, c_next = raw_traj[i + 1] + seg_dist = float(np.linalg.norm(c_next - c_curr)) + n_sub = max(1, int(math.ceil(seg_dist / max_spacing))) + + for j in range(n_sub): + alpha = j / float(n_sub) + t_sample = t_curr + alpha * (t_next - t_curr) + c_sample = self.center_at(t_sample) + spheres.append(ObstacleSphere3D(center=c_sample, radius=self.radius)) + + # Include the final horizon endpoint + t_last, _ = raw_traj[-1] + spheres.append(ObstacleSphere3D(center=self.center_at(t_last), radius=self.radius)) + + return spheres def to_dict(self) -> Dict[str, Any]: return { diff --git a/src/adaptive_rl/navigation/mission.py b/src/adaptive_rl/navigation/mission.py index 6e807ae..82cc109 100644 --- a/src/adaptive_rl/navigation/mission.py +++ b/src/adaptive_rl/navigation/mission.py @@ -45,12 +45,16 @@ class MissionTelemetry: distance_to_final_goal: float replan_count: int min_clearance_observed: float + mission_waypoint_index: int = 0 + total_mission_waypoints: int = 0 def to_dict(self) -> Dict[str, Any]: return { "state": self.state.value, "active_waypoint_index": self.active_waypoint_index, "total_waypoints": self.total_waypoints, + "mission_waypoint_index": self.mission_waypoint_index, + "total_mission_waypoints": self.total_mission_waypoints, "current_target": [round(float(x), 4) for x in self.current_target], "distance_to_target": round(self.distance_to_target, 4), "distance_to_final_goal": round(self.distance_to_final_goal, 4), @@ -78,6 +82,7 @@ def __init__( self.state: MissionState = MissionState.IDLE self.mission_waypoints: List[np.ndarray] = [] + self.current_mission_waypoint_idx: int = 0 self.active_path: List[np.ndarray] = [] self.active_path_idx: int = 0 self.replan_count: int = 0 @@ -99,6 +104,7 @@ def start_mission( if not self.mission_waypoints: raise ValueError("Mission must contain at least one destination waypoint.") + self.current_mission_waypoint_idx = 0 self.final_goal = self.mission_waypoints[-1].copy() self.replan_count = 0 self.min_clearance_observed = float("inf") @@ -154,11 +160,11 @@ def update( if clear < self.min_clearance_observed: self.min_clearance_observed = clear - # Current target waypoint + # Current target waypoint along active path if self.active_path_idx < len(self.active_path): current_target = self.active_path[self.active_path_idx] else: - current_target = self.active_path[-1] + current_target = self.active_path[-1] if self.active_path else pos.copy() dist_to_target = float(np.linalg.norm(current_target - pos)) @@ -179,26 +185,69 @@ def update( ) self.state = MissionState.REPLANNING self.replan_count += 1 - # Replan from current position to the destination - dest = self.mission_waypoints[-1] if self.mission_waypoints else current_target - replan_res = self.planner.plan(pos, dest, eval_obs) - if replan_res.status == "SUCCESS" and replan_res.waypoints: - self.active_path = replan_res.waypoints - self.active_path_idx = 1 if len(replan_res.waypoints) > 1 else 0 + + # Replan sequentially through all remaining mission waypoints to preserve order + remaining_goals = self.mission_waypoints[self.current_mission_waypoint_idx :] + if not remaining_goals: + remaining_goals = [ + self.final_goal if self.final_goal is not None else current_target + ] + + replan_path: List[np.ndarray] = [pos.copy()] + curr_pt = pos.copy() + replan_ok = True + + for goal in remaining_goals: + seg_plan = self.planner.plan(curr_pt, goal, eval_obs) + if seg_plan.status != "SUCCESS" or not seg_plan.waypoints: + replan_ok = False + break + for wp in seg_plan.waypoints[1:]: + replan_path.append(wp.copy()) + curr_pt = goal.copy() + + if replan_ok and len(replan_path) > 1: + self.active_path = replan_path + self.active_path_idx = 1 current_target = self.active_path[self.active_path_idx] + dist_to_target = float(np.linalg.norm(current_target - pos)) self.state = MissionState.EXECUTING else: # Safe stop in place (hover hold) while dynamic obstacle clears the corridor + # Preserve safe hover/hold without falsely advancing mission current_target = pos.copy() return current_target, self.get_telemetry(drone_state) - # Advance waypoint if reached + # Advance active path waypoint if reached if dist_to_target <= self.waypoint_acceptance_radius: if self.active_path_idx < len(self.active_path) - 1: self.active_path_idx += 1 current_target = self.active_path[self.active_path_idx] + + # Advance high-level mission waypoints sequentially as reached + while self.current_mission_waypoint_idx < len(self.mission_waypoints): + dist_to_mwp = float( + np.linalg.norm(self.mission_waypoints[self.current_mission_waypoint_idx] - pos) + ) + if dist_to_mwp <= self.waypoint_acceptance_radius: + self.current_mission_waypoint_idx += 1 else: - self.state = MissionState.ARRIVED + break + + # Mission arrival check: all mission waypoints must be completed in order + all_mission_wps_reached = self.current_mission_waypoint_idx >= len(self.mission_waypoints) + final_goal_reached = ( + float( + np.linalg.norm( + (self.mission_waypoints[-1] if self.mission_waypoints else current_target) - pos + ) + ) + <= self.waypoint_acceptance_radius + ) + at_end_of_path = self.active_path_idx >= len(self.active_path) - 1 + + if all_mission_wps_reached and final_goal_reached and at_end_of_path: + self.state = MissionState.ARRIVED return current_target, self.get_telemetry(drone_state) @@ -215,6 +264,7 @@ def trigger_return_to_home( self.active_path = res.waypoints self.active_path_idx = 1 if len(res.waypoints) > 1 else 0 self.mission_waypoints = [self.home_position.copy()] + self.current_mission_waypoint_idx = 0 self.final_goal = self.home_position.copy() self.state = MissionState.EXECUTING return True @@ -243,4 +293,6 @@ def get_telemetry(self, drone_state: DroneState6DOF) -> MissionTelemetry: distance_to_final_goal=dist_final, replan_count=self.replan_count, min_clearance_observed=self.min_clearance_observed, + mission_waypoint_index=self.current_mission_waypoint_idx, + total_mission_waypoints=len(self.mission_waypoints), ) diff --git a/tests/test_navigation_dynamic_obstacles.py b/tests/test_navigation_dynamic_obstacles.py index 86d5adf..42ab0c5 100644 --- a/tests/test_navigation_dynamic_obstacles.py +++ b/tests/test_navigation_dynamic_obstacles.py @@ -188,3 +188,189 @@ def test_dynamic_obstacle_predictive_evasion_replanning() -> None: # MissionManager should detect the predicted intrusion and successfully replan assert telem.replan_count >= 1 + + +def test_sequential_waypoints_preserved_during_dynamic_replan() -> None: + """Dynamic replan must preserve all remaining sequential mission waypoints in order.""" + from adaptive_rl.environments.drone_6dof import DroneState6DOF + from adaptive_rl.navigation.planner import AStarPlanner3D + + planner = AStarPlanner3D(bounds=(30.0, 30.0, 15.0), resolution=1.0) + mgr = MissionManager(planner=planner, waypoint_acceptance_radius=1.0) + + # 3 sequential waypoints + wp1 = np.array([5.0, 10.0, 5.0]) + wp2 = np.array([15.0, 10.0, 5.0]) + wp3 = np.array([25.0, 10.0, 5.0]) + + ok = mgr.start_mission( + start_position=np.array([2.0, 10.0, 5.0]), + goals=[wp1, wp2, wp3], + static_obstacles=[], + ) + assert ok is True + assert mgr.current_mission_waypoint_idx == 0 + assert len(mgr.mission_waypoints) == 3 + + # Dynamic obstacle blocks path segment between drone and wp1 + dyn_obs = DynamicObstacleSphere3D( + center_initial=np.array([3.5, 10.0, 5.0]), + radius=1.0, + velocity=np.zeros(3), + ) + + state = DroneState6DOF( + position=np.array([2.0, 10.0, 5.0]), + velocity=np.zeros(3), + quaternion=np.array([1.0, 0.0, 0.0, 0.0]), + angular_velocity=np.zeros(3), + ) + + _, telem = mgr.update( + drone_state=state, + time_sec=0.0, + static_obstacles=[], + dynamic_obstacles=[dyn_obs], + ) + + # Should have replanned + assert telem.replan_count >= 1 + # Active path must end at final waypoint wp3 + assert np.allclose(mgr.active_path[-1], wp3) + # The replanned path MUST pass through wp1 before reaching wp2 and wp3 + dists_wp1 = [np.linalg.norm(p - wp1) for p in mgr.active_path] + dists_wp2 = [np.linalg.norm(p - wp2) for p in mgr.active_path] + dists_wp3 = [np.linalg.norm(p - wp3) for p in mgr.active_path] + idx1 = int(np.argmin(dists_wp1)) + idx2 = int(np.argmin(dists_wp2)) + idx3 = int(np.argmin(dists_wp3)) + assert dists_wp1[idx1] <= 1.0, "Replan did not include wp1!" + assert dists_wp2[idx2] <= 1.0, "Replan did not include wp2!" + assert dists_wp3[idx3] <= 1.0, "Replan did not include wp3!" + assert idx1 < idx2 < idx3, "Waypoints in active path are out of order!" + assert mgr.current_mission_waypoint_idx == 0 + + +def test_mission_cannot_arrive_skipping_intermediate_waypoints() -> None: + """Drone reaching final waypoint coordinates without visiting intermediate waypoints cannot achieve ARRIVED.""" + from adaptive_rl.environments.drone_6dof import DroneState6DOF + from adaptive_rl.navigation.mission import MissionState + from adaptive_rl.navigation.planner import AStarPlanner3D + + planner = AStarPlanner3D(bounds=(30.0, 30.0, 15.0), resolution=1.0) + mgr = MissionManager(planner=planner, waypoint_acceptance_radius=1.0) + + wp1 = np.array([5.0, 10.0, 5.0]) + wp2 = np.array([15.0, 10.0, 5.0]) + wp3 = np.array([25.0, 10.0, 5.0]) + + mgr.start_mission( + start_position=np.array([2.0, 10.0, 5.0]), + goals=[wp1, wp2, wp3], + static_obstacles=[], + ) + + # Teleport directly to wp3 without touching wp1 or wp2 + state_at_final = DroneState6DOF( + position=wp3.copy(), + velocity=np.zeros(3), + quaternion=np.array([1.0, 0.0, 0.0, 0.0]), + angular_velocity=np.zeros(3), + ) + + _, telem = mgr.update( + drone_state=state_at_final, + time_sec=1.0, + static_obstacles=[], + ) + + # Mission must NOT be ARRIVED because intermediate waypoints were skipped + assert mgr.state != MissionState.ARRIVED + assert telem.state != MissionState.ARRIVED + assert mgr.current_mission_waypoint_idx < 3 + + +def test_failed_replan_hovers_safely_without_advancing_mission() -> None: + """When replanning fails (e.g. completely boxed in), drone holds position safely and does not advance.""" + from adaptive_rl.environments.drone import ObstacleSphere3D + from adaptive_rl.environments.drone_6dof import DroneState6DOF + from adaptive_rl.navigation.mission import MissionState + from adaptive_rl.navigation.planner import AStarPlanner3D + + planner = AStarPlanner3D(bounds=(30.0, 30.0, 15.0), resolution=1.0) + mgr = MissionManager(planner=planner, waypoint_acceptance_radius=1.0) + + wp1 = np.array([10.0, 10.0, 5.0]) + mgr.start_mission( + start_position=np.array([5.0, 10.0, 5.0]), + goals=[wp1], + static_obstacles=[], + ) + + pos = np.array([5.0, 10.0, 5.0]) + blocking_dyn = DynamicObstacleSphere3D( + center_initial=np.array([6.0, 10.0, 5.0]), + radius=2.0, + velocity=np.zeros(3), + ) + goal_blocking = ObstacleSphere3D(center=wp1, radius=2.0) + + state = DroneState6DOF( + position=pos.copy(), + velocity=np.zeros(3), + quaternion=np.array([1.0, 0.0, 0.0, 0.0]), + angular_velocity=np.zeros(3), + ) + + target, _ = mgr.update( + drone_state=state, + time_sec=0.0, + static_obstacles=[goal_blocking], + dynamic_obstacles=[blocking_dyn], + ) + + # Target must be safe hover at current position + assert np.allclose(target, pos) + assert mgr.state == MissionState.REPLANNING + assert mgr.current_mission_waypoint_idx == 0 + + +def test_oscillating_obstacle_speed_bound_and_collision_detection() -> None: + """Oscillating obstacle with zero linear velocity calculates valid speed bound and detects swept collision.""" + obs = DynamicObstacleSphere3D( + center_initial=np.array([10.0, 10.0, 5.0]), + radius=1.0, + velocity=np.zeros(3), # Zero linear velocity + motion_type="oscillate", + oscillation_period=2.0, # omega = 2*pi / 2 = pi + oscillation_amplitude=np.array([10.0, 0.0, 0.0]), # Amplitude 10m + ) + max_spd = obs.max_speed() + assert abs(max_spd - (np.pi * 10.0)) < 1e-4 + + coll = obs.check_continuous_collision( + prev_drone_pos=np.array([19.8, 10.0, 5.0]), + curr_drone_pos=np.array([19.8, 10.0, 5.0]), + prev_time=0.4, + curr_time=0.6, + vehicle_radius=0.5, + ) + assert coll is True + + +def test_predict_occupied_spheres_zero_gaps() -> None: + """Predictive occupied spheres cover swept volume densely with spacing <= 1.2 * radius (zero gaps).""" + obs = DynamicObstacleSphere3D( + center_initial=np.array([5.0, 5.0, 5.0]), + radius=1.0, + velocity=np.array([10.0, 0.0, 0.0]), # Moves 10 m/s + bounds=(50.0, 50.0, 20.0), + ) + spheres = obs.predict_occupied_spheres(t_start=0.0, horizon_sec=2.0, dt=0.5) + assert len(spheres) > 5 + + for i in range(len(spheres) - 1): + dist = float(np.linalg.norm(spheres[i + 1].center - spheres[i].center)) + assert dist <= 1.2 * obs.radius + 1e-4, ( + f"Gap detected at index {i}: dist={dist} > {1.2 * obs.radius}" + ) diff --git a/tests/test_recorder_fingerprint_phase3.py b/tests/test_recorder_fingerprint_phase3.py index fc9f722..350c356 100644 --- a/tests/test_recorder_fingerprint_phase3.py +++ b/tests/test_recorder_fingerprint_phase3.py @@ -281,3 +281,123 @@ def test_scenario_provenance_invalidation() -> None: obstacles=[ObstacleSphere3D(center=np.array([5.0, 5.0, 4.0001]), radius=1.5)], ) assert compute_config_fingerprint(alt_obs.to_dict()) != fp_base + + +def test_create_record_infers_scenario_provenance_automatically(tmp_path: Path) -> None: + """FlightRecorder.create_record automatically infers scenario provenance from scenario_results.""" + from adaptive_rl.flight_testing.scenarios import ( + FlightScenarioDefinition, + FlightScenarioRunner, + ) + + runner = FlightScenarioRunner() + sc = FlightScenarioDefinition( + name="test_inferred_scenario", + description="Test scenario for provenance inference", + initial_position=np.array([1.0, 2.0, 3.0]), + waypoints=[np.array([1.0, 2.0, 3.0])], + max_steps=5, + ) + res = runner.run_scenario(sc) + assert res.scenario_definition is not None + + recorder = FlightRecorder(output_dir=tmp_path) + record = recorder.create_record( + experiment_id="test_infer_exp", + physics_params={"mass": 1.0, "gravity": 9.81}, + scenario_results=[res], # No scenarios explicitly passed! + ) + + assert record.configuration is not None + assert len(record.configuration.scenarios) == 1 + assert record.configuration.scenarios[0]["name"] == "test_inferred_scenario" + assert record.configuration.provenance_complete is True + assert record.verify_config_fingerprint() is True + assert record.verify_record_digest() is True + + +def test_incomplete_scenario_provenance_flagged(tmp_path: Path) -> None: + """When scenario results cannot be resolved to definitions, provenance_complete is False.""" + recorder = FlightRecorder(output_dir=tmp_path) + unknown_metric = FlightScenarioMetrics( + scenario_name="unknown_custom_scenario_xyz", + success=True, + outcome="success", + duration_steps=10, + duration_seconds=0.5, + final_position_error=0.01, + mean_position_error=0.02, + max_position_error=0.05, + rms_position_error=0.02, + mean_velocity_error=0.01, + max_velocity_error=0.02, + rms_velocity_error=0.01, + mean_attitude_error_deg=0.5, + max_attitude_error_deg=1.0, + mean_angular_speed=0.1, + max_angular_speed=0.2, + initial_goal_distance=1.0, + final_goal_distance=0.01, + min_obstacle_clearance=-1.0, + collision_count=0, + control_effort=10.0, + execution_wall_time_ms=5.0, + ) + + record = recorder.create_record( + experiment_id="test_incomplete_exp", + physics_params={"mass": 1.0, "gravity": 9.81}, + scenario_results=[unknown_metric], + ) + + assert record.configuration is not None + assert record.configuration.provenance_complete is False + assert record.provenance_complete is False + + +def test_callable_provenance_contract_closure_and_code() -> None: + """Callable provenance captures closure variables, bytecode hash, and explicit metadata.""" + from adaptive_rl.flight_testing.scenarios import ( + FlightScenarioDefinition, + extract_callable_provenance, + ) + + def make_thrust_fault(factor: float, motor_idx: int): + def fault(step: int, thrusts: np.ndarray) -> np.ndarray: + t = thrusts.copy() + t[motor_idx] *= factor + return t + + return fault + + fault_50 = make_thrust_fault(0.5, 0) + fault_70 = make_thrust_fault(0.7, 0) + + prov_50 = extract_callable_provenance(fault_50) + prov_70 = extract_callable_provenance(fault_70) + + # Different closure values must yield different provenance + assert prov_50["closure"]["factor"] == 0.5 + assert prov_70["closure"]["factor"] == 0.7 + assert prov_50 != prov_70 + + # Scenarios using these distinct faults must yield distinct cryptographic fingerprints + sc1 = FlightScenarioDefinition( + name="sc1", + description="Sc 1", + initial_position=np.array([0.0, 0.0, 1.0]), + actuator_fault_fn=fault_50, + ) + sc2 = FlightScenarioDefinition( + name="sc1", + description="Sc 1", + initial_position=np.array([0.0, 0.0, 1.0]), + actuator_fault_fn=fault_70, + ) + assert compute_config_fingerprint(sc1.to_dict()) != compute_config_fingerprint(sc2.to_dict()) + + # Metadata inclusion + fault_50.metadata = {"subsystem": "avionics_esc_1"} # type: ignore[attr-defined] + prov_meta = extract_callable_provenance(fault_50) + assert prov_meta.get("metadata") == {"subsystem": "avionics_esc_1"} + assert prov_meta != prov_50 From d64e86adbf8fa75edcb0a8b83c2a5e3699c6bd69 Mon Sep 17 00:00:00 2001 From: Aryan Saxena Date: Sun, 15 Nov 2026 21:43:35 +0530 Subject: [PATCH 4/7] fix(phase3): harden predictive dynamic occupancy, complete callable provenance, and enforce provenance consistency --- src/adaptive_rl/flight_testing/recorder.py | 65 ++++- src/adaptive_rl/flight_testing/scenarios.py | 219 ++++++++++++++-- .../navigation/dynamic_obstacles.py | 95 +++++-- tests/test_navigation_dynamic_obstacles.py | 239 ++++++++++++++++++ tests/test_recorder_fingerprint_phase3.py | 218 ++++++++++++++++ 5 files changed, 783 insertions(+), 53 deletions(-) diff --git a/src/adaptive_rl/flight_testing/recorder.py b/src/adaptive_rl/flight_testing/recorder.py index d18f96a..860eace 100644 --- a/src/adaptive_rl/flight_testing/recorder.py +++ b/src/adaptive_rl/flight_testing/recorder.py @@ -226,6 +226,9 @@ def to_dict(self, include_digest: bool = True) -> Dict[str, Any]: def verify_config_fingerprint(self) -> bool: """Verify that the stored config_fingerprint matches the configuration.""" if self.configuration is not None: + # Inconsistent provenance check: top-level and configuration flags must agree + if self.provenance_complete != self.configuration.provenance_complete: + return False expected = compute_config_fingerprint(self.configuration.to_dict()) else: expected = compute_config_fingerprint(self.physics.to_dict()) @@ -237,6 +240,9 @@ def verify_fingerprint(self) -> bool: def verify_record_digest(self) -> bool: """Verify that the stored record_digest matches the SHA-256 digest of record contents.""" + if self.configuration is not None: + if self.provenance_complete != self.configuration.provenance_complete: + return False payload = self.to_dict(include_digest=False) expected = compute_record_digest(payload) return self.record_digest == expected @@ -330,15 +336,29 @@ def create_record( if raw_scenarios is not None: for sc in raw_scenarios: if hasattr(sc, "to_dict"): - scenario_provenance.append(sc.to_dict()) + d = sc.to_dict() + scenario_provenance.append(d) + if ( + getattr(sc, "provenance_complete", True) is False + or d.get("provenance_complete") is False + ): + provenance_complete = False elif isinstance(sc, dict): scenario_provenance.append(sc) + if sc.get("provenance_complete") is False: + provenance_complete = False else: # Automatically infer scenario definitions from scenario_results for r in scenario_results: sc_def = getattr(r, "scenario_definition", None) if sc_def is not None and hasattr(sc_def, "to_dict"): - scenario_provenance.append(sc_def.to_dict()) + d = sc_def.to_dict() + scenario_provenance.append(d) + if ( + getattr(sc_def, "provenance_complete", True) is False + or d.get("provenance_complete") is False + ): + provenance_complete = False else: name = getattr(r, "scenario_name", None) if name: @@ -346,7 +366,13 @@ def create_record( from adaptive_rl.flight_testing.scenarios import get_scenario sc_obj = get_scenario(name) - scenario_provenance.append(sc_obj.to_dict()) + d = sc_obj.to_dict() + scenario_provenance.append(d) + if ( + getattr(sc_obj, "provenance_complete", True) is False + or d.get("provenance_complete") is False + ): + provenance_complete = False except Exception: provenance_complete = False else: @@ -370,7 +396,34 @@ def create_record( exp_config = ExperimentConfigurationFingerprint(**configuration) else: raise ValueError(f"Invalid configuration type: {type(configuration)}") + + # Validate explicitly supplied configuration against executed scenarios: + cfg_valid = True + if expected_scenario_names: + cfg_names = set(exp_config.scenario_names) + if not expected_scenario_names.issubset(cfg_names): + cfg_valid = False + + cfg_sc_names = set( + s.get("name") + for s in exp_config.scenarios + if isinstance(s, dict) and "name" in s + ) + if not expected_scenario_names.issubset(cfg_sc_names): + cfg_valid = False + + for sc in exp_config.scenarios: + if isinstance(sc, dict) and sc.get("provenance_complete") is False: + cfg_valid = False + + # Define ONE authoritative provenance completeness result + authoritative_complete = ( + provenance_complete and cfg_valid and bool(exp_config.provenance_complete) + ) + # Synchronize configuration flag so top-level and configuration always agree + exp_config.provenance_complete = authoritative_complete else: + authoritative_complete = provenance_complete exp_config = ExperimentConfigurationFingerprint( physics=physics, environment_bounds=list( @@ -386,7 +439,7 @@ def create_record( controller_gains=dict(physics_params.get("controller_gains", {})), scenarios=scenario_provenance, extra_parameters=dict(physics_params.get("extra_parameters", {})), - provenance_complete=provenance_complete, + provenance_complete=authoritative_complete, ) config_hash = compute_config_fingerprint(exp_config.to_dict()) @@ -452,7 +505,7 @@ def create_record( "scenario_metrics": [m.to_dict() for m in metrics_list], "trajectories": collected_trajectories, "aggregate_summary": agg_summary, - "provenance_complete": provenance_complete, + "provenance_complete": authoritative_complete, "signature": signature, "configuration": exp_config.to_dict(), } @@ -472,7 +525,7 @@ def create_record( trajectories=collected_trajectories, configuration=exp_config, aggregate_summary=agg_summary, - provenance_complete=provenance_complete, + provenance_complete=authoritative_complete, signature=signature, ) diff --git a/src/adaptive_rl/flight_testing/scenarios.py b/src/adaptive_rl/flight_testing/scenarios.py index f49491a..479d7ea 100644 --- a/src/adaptive_rl/flight_testing/scenarios.py +++ b/src/adaptive_rl/flight_testing/scenarios.py @@ -35,11 +35,15 @@ from __future__ import annotations +import builtins import csv +import functools import hashlib +import inspect import json import logging import math +import re import time from dataclasses import asdict, dataclass, field from pathlib import Path @@ -277,16 +281,111 @@ def export_telemetry_csv(self, filepath: Path | str) -> Path: return path +def _safe_provenance_value(val: Any) -> Tuple[Any, bool]: + """Deterministically serialize a value for provenance hashing without leaking memory addresses. + + Returns (serialized_value, is_complete). + """ + if val is None or isinstance(val, bool): + return val, True + if isinstance(val, (int, np.integer)): + return int(val), True + if isinstance(val, (float, np.floating)): + f_val = float(val) + if not math.isfinite(f_val): + return str(f_val), True + return 0.0 if f_val == 0.0 else f_val, True + if isinstance(val, str): + # Sanitize any hex memory addresses (e.g., '0x7f...', '<... at 0x...>') + if re.search(r"\bat 0x[0-9a-fA-F]+\b", val) or re.search(r"\b0x[0-9a-fA-F]{6,16}\b", val): + cleaned = re.sub(r"\bat 0x[0-9a-fA-F]+\b", "", val) + cleaned = re.sub(r"\b0x[0-9a-fA-F]{6,16}\b", "", cleaned).strip() + return cleaned, False + return val, True + if isinstance(val, np.ndarray): + items, ok = _safe_provenance_value(val.tolist()) + return items, ok + if isinstance(val, (list, tuple)): + list_results = [_safe_provenance_value(x) for x in val] + return [r[0] for r in list_results], all(r[1] for r in list_results) + if isinstance(val, dict): + dict_results = { + str(k): _safe_provenance_value(v) + for k, v in sorted(val.items(), key=lambda item: str(item[0])) + } + return {k: r[0] for k, r in dict_results.items()}, all(r[1] for r in dict_results.values()) + if hasattr(val, "to_dict") and callable(getattr(val, "to_dict")): + try: + return _safe_provenance_value(val.to_dict()) + except Exception: + return f"unserializable:{type(val).__name__}", False + if hasattr(val, "metadata"): + meta = getattr(val, "metadata") + if meta is not None: + return _safe_provenance_value(meta) + if inspect.ismodule(val): + return f"module:{val.__name__}", True + if inspect.isfunction(val) or inspect.isclass(val): + return ( + f"callable:{getattr(val, '__qualname__', getattr(val, '__name__', str(type(val))))}", + True, + ) + if hasattr(val, "__dict__"): + try: + custom_attrs = {k: v for k, v in val.__dict__.items() if not k.startswith("__")} + if custom_attrs: + return _safe_provenance_value(custom_attrs) + except Exception: + pass + return f"object:{type(val).__qualname__}", False + + s = str(val) + if re.search(r"\bat 0x[0-9a-fA-F]+\b", s) or re.search(r"\b0x[0-9a-fA-F]{6,16}\b", s): + cleaned = re.sub(r"\bat 0x[0-9a-fA-F]+\b", "", s).strip() + return cleaned, False + return s, False + + def extract_callable_provenance(fn: Any) -> Dict[str, Any]: """Extract a stable, deterministic provenance record for a callable. - Captures function identity, module, qualified name, explicit metadata, - custom attributes, closure variables, and a SHA-256 digest of bytecode and constants. + Captures function identity, module, qualified name, function defaults, keyword defaults, + referenced global dependencies, closure variables, custom attributes, explicit metadata, + and a SHA-256 digest of code object bytecode, constants, and referenced names. Guarantees different callback configurations generate different digests. """ if fn is None: return {} + provenance_complete = True + + # Unwrap functools.partial + partial_args: List[Any] = [] + partial_keywords: Dict[str, Any] = {} + if isinstance(fn, functools.partial): + partial_args, ok_args = _safe_provenance_value(fn.args) + partial_keywords, ok_kw = _safe_provenance_value(fn.keywords or {}) + if not (ok_args and ok_kw): + provenance_complete = False + fn = fn.func + + # Determine underlying code object and callable type + is_callable_instance = False + if inspect.isfunction(fn) or inspect.ismethod(fn): + code = getattr(fn, "__code__", None) + fn_globals = getattr(fn, "__globals__", {}) + target_fn = fn + elif hasattr(fn, "__call__"): + is_callable_instance = True + call_method = fn.__call__ + code = getattr(call_method, "__code__", None) + fn_globals = getattr(call_method, "__globals__", {}) + target_fn = call_method + else: + code = getattr(fn, "__code__", None) + fn_globals = getattr(fn, "__globals__", {}) + target_fn = fn + info: Dict[str, Any] = { "name": getattr(fn, "__name__", type(fn).__name__), "qualname": getattr(fn, "__qualname__", getattr(fn, "__name__", str(type(fn)))), @@ -295,49 +394,96 @@ def extract_callable_provenance(fn: Any) -> Dict[str, Any]: if hasattr(fn, "__module__") and fn.__module__: info["module"] = str(fn.__module__) - # Explicit metadata or docstring + if is_callable_instance: + info["callable_class"] = f"{type(fn).__module__}.{type(fn).__qualname__}" + + if partial_args: + info["partial_args"] = partial_args + if partial_keywords: + info["partial_keywords"] = partial_keywords + + # Function positional defaults and keyword-only defaults + defaults = getattr(target_fn, "__defaults__", None) + if defaults is not None: + ser_defaults, ok_def = _safe_provenance_value(defaults) + info["defaults"] = ser_defaults + if not ok_def: + provenance_complete = False + + kwdefaults = getattr(target_fn, "__kwdefaults__", None) + if kwdefaults is not None: + ser_kwdefaults, ok_kwdef = _safe_provenance_value(kwdefaults) + info["kwdefaults"] = ser_kwdefaults + if not ok_kwdef: + provenance_complete = False + + # Explicit metadata contract if hasattr(fn, "metadata"): meta = getattr(fn, "metadata") - if isinstance(meta, dict): - info["metadata"] = {str(k): v for k, v in sorted(meta.items())} - else: - info["metadata"] = str(meta) + ser_meta, ok_meta = _safe_provenance_value(meta) + info["metadata"] = ser_meta + if isinstance(meta, dict) and meta.get("provenance_complete") is False: + provenance_complete = False + elif not ok_meta: + provenance_complete = False elif getattr(fn, "__doc__", None): info["doc"] = fn.__doc__.strip() - # Custom attributes on the callable - if hasattr(fn, "__dict__"): + # Custom attributes on the callable instance or function + custom_obj = fn if is_callable_instance else target_fn + if hasattr(custom_obj, "__dict__"): custom_attrs = { - k: v for k, v in fn.__dict__.items() if not k.startswith("__") and k != "metadata" + k: v + for k, v in custom_obj.__dict__.items() + if not k.startswith("__") and k != "metadata" } if custom_attrs: - info["attributes"] = {str(k): str(v) for k, v in sorted(custom_attrs.items())} + ser_attrs, ok_attrs = _safe_provenance_value(custom_attrs) + info["attributes"] = ser_attrs + if not ok_attrs: + provenance_complete = False # Closure variables closure_vars: Dict[str, Any] = {} - if hasattr(fn, "__closure__") and fn.__closure__ is not None and hasattr(fn, "__code__"): - co_freevars = getattr(fn.__code__, "co_freevars", ()) - for var_name, cell in zip(co_freevars, fn.__closure__): + if hasattr(target_fn, "__closure__") and target_fn.__closure__ is not None and code is not None: + co_freevars = getattr(code, "co_freevars", ()) + for var_name, cell in zip(co_freevars, target_fn.__closure__): try: contents = cell.cell_contents - if isinstance(contents, (int, float, bool, str)): - closure_vars[var_name] = contents - elif isinstance(contents, np.ndarray): - closure_vars[var_name] = contents.tolist() - elif isinstance(contents, (list, tuple)): - closure_vars[var_name] = list(contents) - elif isinstance(contents, dict): - closure_vars[var_name] = {str(k): v for k, v in sorted(contents.items())} - else: - closure_vars[var_name] = str(contents) + ser_val, ok_val = _safe_provenance_value(contents) + closure_vars[var_name] = ser_val + if not ok_val: + provenance_complete = False except ValueError: closure_vars[var_name] = "" if closure_vars: info["closure"] = {k: closure_vars[k] for k in sorted(closure_vars.keys())} - # Bytecode and constants digest - if hasattr(fn, "__code__"): - code = fn.__code__ + # Global dependencies referenced in code.co_names + if code is not None and fn_globals: + co_names = getattr(code, "co_names", ()) + builtin_names = set(dir(builtins)) + ref_globals: Dict[str, Any] = {} + for name in sorted(set(co_names)): + if name in fn_globals and name not in builtin_names: + g_val = fn_globals[name] + ser_g, ok_g = _safe_provenance_value(g_val) + ref_globals[name] = ser_g + if not ok_g: + provenance_complete = False + if ref_globals: + info["referenced_globals"] = ref_globals + + # Code object metadata and digest + if code is not None: + info["code_metadata"] = { + "co_argcount": code.co_argcount, + "co_kwonlyargcount": getattr(code, "co_kwonlyargcount", 0), + "co_flags": code.co_flags, + "co_names": list(code.co_names), + "co_varnames": list(code.co_varnames), + } + hasher = hashlib.sha256() hasher.update(getattr(code, "co_code", b"")) co_consts = getattr(code, "co_consts", ()) @@ -346,8 +492,13 @@ def extract_callable_provenance(fn: Any) -> Dict[str, Any]: hasher.update(const.co_code) else: hasher.update(str(const).encode("utf-8", errors="replace")) + for name in getattr(code, "co_names", ()): + hasher.update(name.encode("utf-8", errors="replace")) + for vname in getattr(code, "co_varnames", ()): + hasher.update(vname.encode("utf-8", errors="replace")) info["code_hash"] = hasher.hexdigest() + info["provenance_complete"] = provenance_complete return info @@ -384,6 +535,19 @@ class FlightScenarioDefinition: disturbance_fn: Optional[Callable[[int, DroneState6DOF], np.ndarray]] = None actuator_fault_fn: Optional[Callable[[int, np.ndarray], np.ndarray]] = None + @property + def provenance_complete(self) -> bool: + """Indicates whether all behavior-affecting components have complete provenance.""" + if self.actuator_fault_fn is not None: + fault_info = extract_callable_provenance(self.actuator_fault_fn) + if fault_info.get("provenance_complete") is False: + return False + if self.disturbance_fn is not None: + dist_info = extract_callable_provenance(self.disturbance_fn) + if dist_info.get("provenance_complete") is False: + return False + return True + def to_dict(self) -> Dict[str, Any]: """Return a deterministic, fully serializable dictionary representation for cryptographic hashing.""" obs_dicts = [ @@ -446,6 +610,7 @@ def to_dict(self) -> Dict[str, Any]: "max_duration_seconds": self.max_duration_seconds, "actuator_fault": fault_info, "disturbance": dist_info, + "provenance_complete": self.provenance_complete, } diff --git a/src/adaptive_rl/navigation/dynamic_obstacles.py b/src/adaptive_rl/navigation/dynamic_obstacles.py index 6d64dae..17620d2 100644 --- a/src/adaptive_rl/navigation/dynamic_obstacles.py +++ b/src/adaptive_rl/navigation/dynamic_obstacles.py @@ -32,9 +32,47 @@ class DynamicObstacleSphere3D: ) def __post_init__(self) -> None: + self.radius = float(self.radius) + if not math.isfinite(self.radius) or self.radius <= 0.0: + raise ValueError( + f"Obstacle radius must be strictly positive and finite, got {self.radius}" + ) + + if self.motion_type not in ("bounce", "oscillate"): + raise ValueError( + f"Unsupported motion_type: '{self.motion_type}'. Must be 'bounce' or 'oscillate'" + ) + + self.oscillation_period = float(self.oscillation_period) + if not math.isfinite(self.oscillation_period) or self.oscillation_period <= 0.0: + raise ValueError( + f"Oscillation period must be strictly positive and finite, got {self.oscillation_period}" + ) + self.center_initial = np.asarray(self.center_initial, dtype=np.float64) + if self.center_initial.shape != (3,) or not np.all(np.isfinite(self.center_initial)): + raise ValueError( + f"center_initial must be a finite 3D vector, got {self.center_initial}" + ) + self.velocity = np.asarray(self.velocity, dtype=np.float64) + if self.velocity.shape != (3,) or not np.all(np.isfinite(self.velocity)): + raise ValueError(f"velocity must be a finite 3D vector, got {self.velocity}") + self.oscillation_amplitude = np.asarray(self.oscillation_amplitude, dtype=np.float64) + if self.oscillation_amplitude.shape != (3,) or not np.all( + np.isfinite(self.oscillation_amplitude) + ): + raise ValueError( + f"oscillation_amplitude must be a finite 3D vector, got {self.oscillation_amplitude}" + ) + + if len(self.bounds) != 3 or any( + not math.isfinite(b) or b <= 2.0 * self.radius for b in self.bounds + ): + raise ValueError( + f"Arena bounds must all exceed 2 * radius ({2.0 * self.radius}), got {self.bounds}" + ) def max_speed(self) -> float: """Calculate the theoretical maximum translation speed of the obstacle.""" @@ -189,34 +227,51 @@ def predict_occupied_spheres( t_start: float, horizon_sec: float = 3.0, dt: float = 0.5, + max_spheres: int = 2000, ) -> List[ObstacleSphere3D]: """Generate static spherical obstacle approximations covering the predicted continuous swept volume. Guarantees that consecutive spheres overlap with spacing <= 1.2 * radius, leaving zero gaps in the predicted flight corridor. + Uses a mathematically derived upper bound on the distance travelled across the horizon + (v_max * horizon_sec) rather than discrete endpoint displacement. """ - raw_traj = self.predict_trajectory(t_start, horizon_sec, dt) - if not raw_traj: - return [] + t_start = float(t_start) + horizon_sec = float(horizon_sec) + if horizon_sec <= 0.0: + return [ObstacleSphere3D(center=self.center_at(t_start), radius=self.radius)] + + v_max = self.max_speed() + max_spacing = 1.2 * self.radius + + # If obstacle is essentially stationary, a single sphere at initial position suffices + if v_max <= 1e-9: + return [ObstacleSphere3D(center=self.center_at(t_start), radius=self.radius)] + + # Total distance upper bound across the entire prediction horizon + # For any trajectory c(t), arc length s <= integral of ||c'(t)|| dt <= v_max * horizon_sec + total_dist_bound = v_max * horizon_sec + + # Required number of temporal steps to guarantee center spacing <= 1.2 * radius everywhere + needed_steps = max(1, int(math.ceil(total_dist_bound / max_spacing))) + + # Handle computational limits conservatively: + # If needed_steps exceeds max_spheres, we clamp the number of evaluations, but we + # CONSERVATIVELY inflate the sphere radius so that adjacent spheres still overlap + # without leaving any gaps in the predicted flight corridor. + if needed_steps > max_spheres: + actual_steps = max_spheres + step_dt = horizon_sec / actual_steps + step_disp_bound = v_max * step_dt + eff_radius = max(self.radius, step_disp_bound / 1.2) + else: + actual_steps = needed_steps + eff_radius = self.radius spheres: List[ObstacleSphere3D] = [] - max_spacing = max(0.05, 1.2 * self.radius) - - for i in range(len(raw_traj) - 1): - t_curr, c_curr = raw_traj[i] - t_next, c_next = raw_traj[i + 1] - seg_dist = float(np.linalg.norm(c_next - c_curr)) - n_sub = max(1, int(math.ceil(seg_dist / max_spacing))) - - for j in range(n_sub): - alpha = j / float(n_sub) - t_sample = t_curr + alpha * (t_next - t_curr) - c_sample = self.center_at(t_sample) - spheres.append(ObstacleSphere3D(center=c_sample, radius=self.radius)) - - # Include the final horizon endpoint - t_last, _ = raw_traj[-1] - spheres.append(ObstacleSphere3D(center=self.center_at(t_last), radius=self.radius)) + for i in range(actual_steps + 1): + t = t_start + (i / actual_steps) * horizon_sec + spheres.append(ObstacleSphere3D(center=self.center_at(t), radius=eff_radius)) return spheres diff --git a/tests/test_navigation_dynamic_obstacles.py b/tests/test_navigation_dynamic_obstacles.py index 42ab0c5..5a14c57 100644 --- a/tests/test_navigation_dynamic_obstacles.py +++ b/tests/test_navigation_dynamic_obstacles.py @@ -374,3 +374,242 @@ def test_predict_occupied_spheres_zero_gaps() -> None: assert dist <= 1.2 * obs.radius + 1e-4, ( f"Gap detected at index {i}: dist={dist} > {1.2 * obs.radius}" ) + + +def test_obstacle_parameter_validation() -> None: + """DynamicObstacleSphere3D validates positive radius, valid bounds, positive period, and motion type.""" + import pytest + + # Non-positive radius + with pytest.raises(ValueError, match="strictly positive"): + DynamicObstacleSphere3D(center_initial=np.array([10.0, 10.0, 5.0]), radius=0.0) + with pytest.raises(ValueError, match="strictly positive"): + DynamicObstacleSphere3D(center_initial=np.array([10.0, 10.0, 5.0]), radius=-0.5) + + # Invalid bounds <= 2 * radius + with pytest.raises(ValueError, match=r"Arena bounds must all exceed 2 \* radius"): + DynamicObstacleSphere3D( + center_initial=np.array([10.0, 10.0, 5.0]), + radius=5.0, + bounds=(10.0, 30.0, 20.0), # bounds[0] = 10 <= 2 * 5 + ) + + # Invalid motion type + with pytest.raises(ValueError, match="Unsupported motion_type"): + DynamicObstacleSphere3D( + center_initial=np.array([10.0, 10.0, 5.0]), + radius=1.0, + motion_type="teleport", + ) + + # Non-positive oscillation period + with pytest.raises(ValueError, match="Oscillation period must be strictly positive"): + DynamicObstacleSphere3D( + center_initial=np.array([10.0, 10.0, 5.0]), + radius=1.0, + motion_type="oscillate", + oscillation_period=0.0, + ) + + +def test_predict_occupied_spheres_period_equal_sampling_interval() -> None: + """Obstacle oscillating with period equal to sampling interval does not lose intermediate swept path.""" + T = 0.5 + obs = DynamicObstacleSphere3D( + center_initial=np.array([10.0, 10.0, 5.0]), + radius=0.5, + velocity=np.zeros(3), + motion_type="oscillate", + oscillation_period=T, + oscillation_amplitude=np.array([4.0, 0.0, 0.0]), + bounds=(30.0, 30.0, 15.0), + ) + # Samples at t=0 and t=0.5 are both at x=10.0 (endpoint displacement is 0.0!) + assert np.allclose(obs.center_at(0.0), obs.center_at(T)) + + spheres = obs.predict_occupied_spheres(t_start=0.0, horizon_sec=T, dt=T) + assert len(spheres) > 5 + + x_coords = [s.center[0] for s in spheres] + # At t = T/4 = 0.125s, x reaches 14.0; at t = 3T/4 = 0.375s, x reaches 6.0 + assert max(x_coords) >= 13.5, f"Peak amplitude not captured: {max(x_coords)}" + assert min(x_coords) <= 6.5, f"Trough amplitude not captured: {min(x_coords)}" + + # Ensure no gaps between consecutive spheres + for i in range(len(spheres) - 1): + dist = float(np.linalg.norm(spheres[i + 1].center - spheres[i].center)) + assert dist <= 1.2 * spheres[i].radius + 1e-4 + + +def test_predict_occupied_spheres_multiple_cycles_between_samples() -> None: + """Multiple full oscillation cycles occurring between samples are fully captured without gaps.""" + obs = DynamicObstacleSphere3D( + center_initial=np.array([10.0, 10.0, 5.0]), + radius=0.5, + velocity=np.zeros(3), + motion_type="oscillate", + oscillation_period=0.1, # 10 full cycles in 1.0 second! + oscillation_amplitude=np.array([5.0, 0.0, 0.0]), + bounds=(30.0, 30.0, 15.0), + ) + spheres = obs.predict_occupied_spheres(t_start=0.0, horizon_sec=1.0, dt=1.0) + assert len(spheres) > 20 + + x_coords = [s.center[0] for s in spheres] + assert max(x_coords) >= 14.5 + assert min(x_coords) <= 5.5 + + for i in range(len(spheres) - 1): + dist = float(np.linalg.norm(spheres[i + 1].center - spheres[i].center)) + assert dist <= 1.2 * spheres[i].radius + 1e-4 + + +def test_predict_occupied_spheres_bounce_returning_to_start() -> None: + """Bouncing obstacle reflecting and returning to its start position preserves its full swept path.""" + obs = DynamicObstacleSphere3D( + center_initial=np.array([2.0, 10.0, 5.0]), + radius=1.0, + velocity=np.array( + [28.0, 0.0, 0.0] + ), # Travels 28m in 1s, bounces, travels back in 1s (cycle=2s) + bounds=(30.0, 30.0, 15.0), + motion_type="bounce", + ) + c0 = obs.center_at(0.0) + c2 = obs.center_at(2.0) + assert np.allclose(c0, c2) + + spheres = obs.predict_occupied_spheres(t_start=0.0, horizon_sec=2.0, dt=2.0) + x_coords = [s.center[0] for s in spheres] + assert max(x_coords) >= 27.0 + assert min(x_coords) <= 3.0 + + for i in range(len(spheres) - 1): + dist = float(np.linalg.norm(spheres[i + 1].center - spheres[i].center)) + assert dist <= 1.2 * spheres[i].radius + 1e-4 + + +def test_predict_occupied_spheres_tiny_radius_no_floor_violation() -> None: + """Very small positive obstacle radius (0.01m) strictly satisfies <= 1.2 * radius spacing without floor violation.""" + obs = DynamicObstacleSphere3D( + center_initial=np.array([10.0, 10.0, 5.0]), + radius=0.01, # 1 cm radius + velocity=np.array([0.1, 0.0, 0.0]), # 10 cm/s + bounds=(30.0, 30.0, 15.0), + ) + spheres = obs.predict_occupied_spheres(t_start=0.0, horizon_sec=0.2, dt=0.2) + assert len(spheres) >= 2 + for i in range(len(spheres) - 1): + dist = float(np.linalg.norm(spheres[i + 1].center - spheres[i].center)) + assert dist <= 1.2 * obs.radius + 1e-6, ( + f"Spacing {dist} exceeded 1.2 * radius ({1.2 * obs.radius})" + ) + + +def test_predict_occupied_spheres_computational_limit_conservative() -> None: + """When needed steps exceed max_spheres, radius is expanded conservatively so no gaps occur.""" + obs = DynamicObstacleSphere3D( + center_initial=np.array([5.0, 5.0, 5.0]), + radius=0.001, # 1 mm radius + velocity=np.array([50.0, 0.0, 0.0]), # 50 m/s + bounds=(100.0, 100.0, 50.0), + ) + spheres = obs.predict_occupied_spheres(t_start=0.0, horizon_sec=1.0, dt=1.0, max_spheres=100) + assert len(spheres) == 101 + + eff_r = spheres[0].radius + assert eff_r > obs.radius + for i in range(len(spheres) - 1): + dist = float(np.linalg.norm(spheres[i + 1].center - spheres[i].center)) + assert dist <= 1.2 * eff_r + 1e-4, ( + f"Gap opened under computational limit: dist={dist} > 1.2 * eff_r" + ) + + +def test_end_to_end_repeated_replanning_sequential_progression() -> None: + """P2: End-to-end mission with repeated dynamic obstacle interruptions preserves sequential progression.""" + from adaptive_rl.environments.drone_6dof import DroneState6DOF + from adaptive_rl.navigation.mission import MissionManager, MissionState + from adaptive_rl.navigation.planner import AStarPlanner3D + + planner = AStarPlanner3D( + bounds=(30.0, 30.0, 15.0), + resolution=1.0, + vehicle_radius=0.4, + safety_margin=0.2, + ) + mgr = MissionManager(planner=planner, waypoint_acceptance_radius=0.8) + + # 3 sequential high-level waypoints + wp1 = np.array([10.0, 10.0, 5.0]) + wp2 = np.array([20.0, 10.0, 5.0]) + wp3 = np.array([28.0, 10.0, 5.0]) + + ok = mgr.start_mission( + start_position=np.array([2.0, 10.0, 5.0]), + goals=[wp1, wp2, wp3], + static_obstacles=[], + ) + assert ok is True + assert mgr.current_mission_waypoint_idx == 0 + + # 1. Obstacle 1 appears during segment 1 (at x=6.0, y=10.0) + obs1 = DynamicObstacleSphere3D( + center_initial=np.array([6.0, 10.0, 5.0]), + radius=0.6, + velocity=np.zeros(3), + ) + drone_state = DroneState6DOF( + position=np.array([2.0, 10.0, 5.0]), + velocity=np.zeros(3), + quaternion=np.array([1.0, 0.0, 0.0, 0.0]), + angular_velocity=np.zeros(3), + ) + tgt, telem = mgr.update( + drone_state, time_sec=0.1, static_obstacles=[], dynamic_obstacles=[obs1] + ) + assert telem.replan_count == 1 + assert mgr.state == MissionState.EXECUTING + assert mgr.current_mission_waypoint_idx == 0 + + # Fly through replanned path to reach wp1 + while mgr.current_mission_waypoint_idx == 0: + drone_state.position = mgr.active_path[mgr.active_path_idx].copy() + tgt, telem = mgr.update( + drone_state, time_sec=1.0, static_obstacles=[], dynamic_obstacles=[obs1] + ) + assert mgr.current_mission_waypoint_idx == 1 # Successfully reached wp1! + + # 2. Obstacle 2 appears during segment 2 (at x=15.0, y=10.0) + obs2 = DynamicObstacleSphere3D( + center_initial=np.array([15.0, 10.0, 5.0]), + radius=0.6, + velocity=np.zeros(3), + ) + tgt, telem = mgr.update( + drone_state, time_sec=2.0, static_obstacles=[], dynamic_obstacles=[obs1, obs2] + ) + assert telem.replan_count == 2 + assert mgr.state == MissionState.EXECUTING + assert mgr.current_mission_waypoint_idx == 1 # Still on segment to wp2 + + # Fly through replanned path to reach wp2 + while mgr.current_mission_waypoint_idx == 1: + drone_state.position = mgr.active_path[mgr.active_path_idx].copy() + tgt, telem = mgr.update( + drone_state, time_sec=3.0, static_obstacles=[], dynamic_obstacles=[obs1, obs2] + ) + assert mgr.current_mission_waypoint_idx == 2 # Successfully reached wp2! + + # 3. Proceed along path to wp3 + while mgr.current_mission_waypoint_idx < 3: + drone_state.position = mgr.active_path[mgr.active_path_idx].copy() + tgt, telem = mgr.update( + drone_state, time_sec=4.0, static_obstacles=[], dynamic_obstacles=[obs1, obs2] + ) + + # Arrived at final waypoint wp3 + assert mgr.current_mission_waypoint_idx == 3 + assert mgr.state == MissionState.ARRIVED + assert telem.state == MissionState.ARRIVED + assert telem.replan_count == 2 diff --git a/tests/test_recorder_fingerprint_phase3.py b/tests/test_recorder_fingerprint_phase3.py index 350c356..8ff737c 100644 --- a/tests/test_recorder_fingerprint_phase3.py +++ b/tests/test_recorder_fingerprint_phase3.py @@ -401,3 +401,221 @@ def fault(step: int, thrusts: np.ndarray) -> np.ndarray: prov_meta = extract_callable_provenance(fault_50) assert prov_meta.get("metadata") == {"subsystem": "avionics_esc_1"} assert prov_meta != prov_50 + + +GLOBAL_SCALING_FACTOR = 1.0 + + +def test_callable_provenance_defaults_and_kwdefaults() -> None: + """Functions with identical code but different default or kwdefault arguments produce distinct provenance.""" + from adaptive_rl.flight_testing.scenarios import extract_callable_provenance + + # 1. Positional defaults + def fault_a(step: int, thrusts: np.ndarray, factor: float = 0.5) -> np.ndarray: + return thrusts * factor + + def fault_b(step: int, thrusts: np.ndarray, factor: float = 0.8) -> np.ndarray: + return thrusts * factor + + prov_a = extract_callable_provenance(fault_a) + prov_b = extract_callable_provenance(fault_b) + assert prov_a["defaults"] == [0.5] + assert prov_b["defaults"] == [0.8] + assert prov_a != prov_b + + # 2. Keyword-only defaults + def fault_kw1(step: int, thrusts: np.ndarray, *, scale: float = 1.5) -> np.ndarray: + return thrusts * scale + + def fault_kw2(step: int, thrusts: np.ndarray, *, scale: float = 2.5) -> np.ndarray: + return thrusts * scale + + prov_kw1 = extract_callable_provenance(fault_kw1) + prov_kw2 = extract_callable_provenance(fault_kw2) + assert prov_kw1["kwdefaults"] == {"scale": 1.5} + assert prov_kw2["kwdefaults"] == {"scale": 2.5} + assert prov_kw1 != prov_kw2 + + +def test_callable_provenance_referenced_globals() -> None: + """Functions referencing different global values produce distinct provenance.""" + global GLOBAL_SCALING_FACTOR + from adaptive_rl.flight_testing.scenarios import extract_callable_provenance + + def global_scaled_fault(step: int, thrusts: np.ndarray) -> np.ndarray: + return thrusts * GLOBAL_SCALING_FACTOR + + GLOBAL_SCALING_FACTOR = 1.0 + prov_g1 = extract_callable_provenance(global_scaled_fault) + assert prov_g1["referenced_globals"]["GLOBAL_SCALING_FACTOR"] == 1.0 + + GLOBAL_SCALING_FACTOR = 2.5 + prov_g2 = extract_callable_provenance(global_scaled_fault) + assert prov_g2["referenced_globals"]["GLOBAL_SCALING_FACTOR"] == 2.5 + assert prov_g1 != prov_g2 + + +def test_callable_objects_provenance() -> None: + """Callable class instances capture class identity, instance configuration, and code metadata.""" + from adaptive_rl.flight_testing.scenarios import extract_callable_provenance + + class ActuatorLossModel: + def __init__(self, motor_id: int, efficiency: float) -> None: + self.motor_id = motor_id + self.efficiency = efficiency + + def __call__(self, step: int, thrusts: np.ndarray) -> np.ndarray: + t = thrusts.copy() + t[self.motor_id] *= self.efficiency + return t + + model1 = ActuatorLossModel(motor_id=0, efficiency=0.4) + model2 = ActuatorLossModel(motor_id=0, efficiency=0.9) + + prov1 = extract_callable_provenance(model1) + prov2 = extract_callable_provenance(model2) + + assert "ActuatorLossModel" in prov1["callable_class"] + assert prov1["attributes"]["efficiency"] == 0.4 + assert prov2["attributes"]["efficiency"] == 0.9 + assert prov1["provenance_complete"] is True + assert prov2["provenance_complete"] is True + assert prov1 != prov2 + + +def test_unsupported_non_serializable_callable_state_marked_incomplete() -> None: + """Callable with non-serializable state is marked incomplete without leaking memory addresses.""" + import threading + + from adaptive_rl.flight_testing.scenarios import extract_callable_provenance + + class ComplexFault: + def __init__(self) -> None: + self.lock = threading.Lock() # Non-serializable + + def __call__(self, step: int, thrusts: np.ndarray) -> np.ndarray: + return thrusts + + fault = ComplexFault() + prov = extract_callable_provenance(fault) + + assert prov["provenance_complete"] is False + # Verify no memory address leaks + serialized = canonical_json_dumps(prov) + assert "0x" not in serialized + + +def test_deterministic_repeated_callable_serialization() -> None: + """Repeated calls to extract_callable_provenance produce identical deterministic digests.""" + from adaptive_rl.flight_testing.scenarios import extract_callable_provenance + + def sample_fault(step: int, thrusts: np.ndarray, gain: float = 0.5) -> np.ndarray: + return thrusts * gain + + p1 = extract_callable_provenance(sample_fault) + p2 = extract_callable_provenance(sample_fault) + assert canonical_json_dumps(p1) == canonical_json_dumps(p2) + + +def test_explicit_configuration_missing_scenario_definitions_marked_incomplete( + tmp_path: Path, +) -> None: + """Explicit configuration missing scenario definitions is marked incomplete.""" + from adaptive_rl.flight_testing.scenarios import ( + FlightScenarioDefinition, + FlightScenarioRunner, + ) + + runner = FlightScenarioRunner() + sc1 = FlightScenarioDefinition( + name="sc1", + description="1", + initial_position=np.array([1.0, 1.0, 1.0]), + max_steps=2, + ) + sc2 = FlightScenarioDefinition( + name="sc2", + description="2", + initial_position=np.array([2.0, 2.0, 2.0]), + max_steps=2, + ) + res1 = runner.run_scenario(sc1) + res2 = runner.run_scenario(sc2) + + physics = PhysicsSimulationFingerprint( + mass=1.0, + gravity=9.81, + arm_length=0.2, + arm_span=0.4, + rotor_offset_d=0.1414, + rotor_radius=0.09, + max_thrust_per_motor=5.0, + torque_to_thrust_ratio=0.01, + linear_damping=0.1, + angular_damping=0.02, + inertia=[0.01, 0.01, 0.02], + dt=0.05, + substeps=5, + substep_dt=0.01, + body_frame_lidar=False, + num_lidar_rays=16, + lidar_range=20.0, + ) + + # Supply explicit configuration that only covers sc1, but scenario_results has [res1, res2] + cfg = ExperimentConfigurationFingerprint( + physics=physics, + environment_bounds=[30.0, 30.0, 15.0], + algorithm_name="test", + checkpoint_hash=None, + random_seed=42, + scenario_names=["sc1"], # Missing sc2! + scenarios=[sc1.to_dict()], + provenance_complete=True, # Caller falsely claims complete! + ) + + recorder = FlightRecorder(output_dir=tmp_path) + record = recorder.create_record( + experiment_id="missing_sc_test", + physics_params={"mass": 1.0, "gravity": 9.81}, + scenario_results=[res1, res2], + configuration=cfg, + ) + + # Must be marked incomplete! + assert record.provenance_complete is False + assert record.configuration.provenance_complete is False + assert record.verify_config_fingerprint() is True + + +def test_explicit_configuration_conflicting_completeness_detected(tmp_path: Path) -> None: + """Record tampering with conflicting top-level and configuration completeness fails verification.""" + from adaptive_rl.flight_testing.scenarios import ( + FlightScenarioDefinition, + FlightScenarioRunner, + ) + + runner = FlightScenarioRunner() + sc = FlightScenarioDefinition( + name="sc_ok", + description="ok", + initial_position=np.array([1.0, 1.0, 1.0]), + max_steps=2, + ) + res = runner.run_scenario(sc) + + recorder = FlightRecorder(output_dir=tmp_path) + record = recorder.create_record( + experiment_id="tamper_test", + physics_params={"mass": 1.0, "gravity": 9.81}, + scenario_results=[res], + ) + assert record.verify_config_fingerprint() is True + assert record.verify_record_digest() is True + + # Tamper with top-level flag to disagree with configuration flag + record.provenance_complete = not record.configuration.provenance_complete + + # Both verifications must catch the inconsistency! + assert record.verify_config_fingerprint() is False + assert record.verify_record_digest() is False From dace7e0f60aba6f52b0bc4343494ef88e3a59b2e Mon Sep 17 00:00:00 2001 From: Aryan Saxena Date: Sun, 15 Nov 2026 22:12:59 +0530 Subject: [PATCH 5/7] fix(phase3): harden predictive occupancy parameter safety, validate arena bounds, complete recursive provenance, and verify 6-DOF continuous collision --- src/adaptive_rl/environments/drone_6dof.py | 6 + src/adaptive_rl/flight_testing/recorder.py | 27 +- src/adaptive_rl/flight_testing/scenarios.py | 101 +++++-- .../navigation/dynamic_obstacles.py | 200 ++++++++++---- tests/test_navigation_dynamic_obstacles.py | 256 +++++++++++++++++- tests/test_recorder_fingerprint_phase3.py | 200 ++++++++++++++ 6 files changed, 710 insertions(+), 80 deletions(-) diff --git a/src/adaptive_rl/environments/drone_6dof.py b/src/adaptive_rl/environments/drone_6dof.py index d94bd16..986c1a1 100644 --- a/src/adaptive_rl/environments/drone_6dof.py +++ b/src/adaptive_rl/environments/drone_6dof.py @@ -392,6 +392,7 @@ def __init__( quaternion=np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64), angular_velocity=np.zeros(3, dtype=np.float64), ) + self.last_substep_positions: List[np.ndarray] = [self.state.position.copy()] @property def arm_span(self) -> float: @@ -567,6 +568,8 @@ def step_rk4(self, motor_thrusts: np.ndarray) -> DroneState6DOF: quat = self.state.quaternion.copy() omega = self.state.angular_velocity.copy() + substep_positions = [pos.copy()] + for _ in range(self.substeps): f_0 = self.actual_motor_thrusts.copy() @@ -636,6 +639,9 @@ def step_rk4(self, motor_thrusts: np.ndarray) -> DroneState6DOF: quat /= q_norm omega += (h / 6.0) * (k1_w + 2.0 * k2_w + 2.0 * k3_w + k4_w) + substep_positions.append(pos.copy()) + + self.last_substep_positions = substep_positions self.state.position = pos self.state.velocity = vel self.state.quaternion = quat diff --git a/src/adaptive_rl/flight_testing/recorder.py b/src/adaptive_rl/flight_testing/recorder.py index 860eace..7d4ffb8 100644 --- a/src/adaptive_rl/flight_testing/recorder.py +++ b/src/adaptive_rl/flight_testing/recorder.py @@ -388,6 +388,11 @@ def create_record( if not expected_scenario_names and not scenario_provenance: provenance_complete = True + # Map executed scenario definitions by name + exec_sc_by_name = { + s.get("name"): s for s in scenario_provenance if isinstance(s, dict) and "name" in s + } + # Build comprehensive ExperimentConfigurationFingerprint if configuration is not None: if isinstance(configuration, ExperimentConfigurationFingerprint): @@ -404,14 +409,28 @@ def create_record( if not expected_scenario_names.issubset(cfg_names): cfg_valid = False - cfg_sc_names = set( - s.get("name") + cfg_sc_by_name = { + s.get("name"): s for s in exp_config.scenarios if isinstance(s, dict) and "name" in s - ) - if not expected_scenario_names.issubset(cfg_sc_names): + } + if not expected_scenario_names.issubset(cfg_sc_by_name.keys()): cfg_valid = False + # Compare canonical serialized scenario definitions (not only names) + for sc_name in expected_scenario_names: + exec_def = exec_sc_by_name.get(sc_name) + cfg_def = cfg_sc_by_name.get(sc_name) + if exec_def is None or cfg_def is None: + cfg_valid = False + break + # Normalize comparison: compare behavior-affecting content + exec_content = {k: v for k, v in exec_def.items() if k != "provenance_complete"} + cfg_content = {k: v for k, v in cfg_def.items() if k != "provenance_complete"} + if canonical_json_dumps(exec_content) != canonical_json_dumps(cfg_content): + cfg_valid = False + break + for sc in exp_config.scenarios: if isinstance(sc, dict) and sc.get("provenance_complete") is False: cfg_valid = False diff --git a/src/adaptive_rl/flight_testing/scenarios.py b/src/adaptive_rl/flight_testing/scenarios.py index 479d7ea..ded6c63 100644 --- a/src/adaptive_rl/flight_testing/scenarios.py +++ b/src/adaptive_rl/flight_testing/scenarios.py @@ -281,11 +281,14 @@ def export_telemetry_csv(self, filepath: Path | str) -> Path: return path -def _safe_provenance_value(val: Any) -> Tuple[Any, bool]: +def _safe_provenance_value(val: Any, _visited: Optional[set[int]] = None) -> Tuple[Any, bool]: """Deterministically serialize a value for provenance hashing without leaking memory addresses. Returns (serialized_value, is_complete). """ + if _visited is None: + _visited = set() + if val is None or isinstance(val, bool): return val, True if isinstance(val, (int, np.integer)): @@ -303,38 +306,73 @@ def _safe_provenance_value(val: Any) -> Tuple[Any, bool]: return cleaned, False return val, True if isinstance(val, np.ndarray): - items, ok = _safe_provenance_value(val.tolist()) + items, ok = _safe_provenance_value(val.tolist(), _visited=_visited) return items, ok if isinstance(val, (list, tuple)): - list_results = [_safe_provenance_value(x) for x in val] + list_results = [_safe_provenance_value(x, _visited=_visited) for x in val] return [r[0] for r in list_results], all(r[1] for r in list_results) if isinstance(val, dict): dict_results = { - str(k): _safe_provenance_value(v) + str(k): _safe_provenance_value(v, _visited=_visited) for k, v in sorted(val.items(), key=lambda item: str(item[0])) } return {k: r[0] for k, r in dict_results.items()}, all(r[1] for r in dict_results.values()) if hasattr(val, "to_dict") and callable(getattr(val, "to_dict")): try: - return _safe_provenance_value(val.to_dict()) + return _safe_provenance_value(val.to_dict(), _visited=_visited) except Exception: return f"unserializable:{type(val).__name__}", False if hasattr(val, "metadata"): meta = getattr(val, "metadata") if meta is not None: - return _safe_provenance_value(meta) + return _safe_provenance_value(meta, _visited=_visited) if inspect.ismodule(val): return f"module:{val.__name__}", True - if inspect.isfunction(val) or inspect.isclass(val): - return ( - f"callable:{getattr(val, '__qualname__', getattr(val, '__name__', str(type(val))))}", - True, - ) + + # Referenced helper function or method + if inspect.isfunction(val) or inspect.ismethod(val): + if id(val) in _visited: + qual = getattr(val, "__qualname__", getattr(val, "__name__", str(val))) + return {"recursion_ref": qual}, True + fn_prov = extract_callable_provenance(val, _visited=set(_visited)) + return fn_prov, fn_prov.get("provenance_complete", True) + + # Class dependency + if inspect.isclass(val): + if id(val) in _visited: + qual = getattr(val, "__qualname__", getattr(val, "__name__", str(val))) + return {"recursion_ref": qual}, True + _visited.add(id(val)) + mod = getattr(val, "__module__", "") + qual = getattr(val, "__qualname__", getattr(val, "__name__", str(val))) + if mod == "builtins" or mod.startswith("numpy") or mod.startswith("scipy"): + return f"class:{mod}.{qual}", True + # User/custom class: fingerprint class attributes + cls_attrs: Dict[str, Any] = {} + cls_complete = True + for k, v in sorted(val.__dict__.items(), key=lambda item: str(item[0])): + if not k.startswith("__"): + ser_v, ok_v = _safe_provenance_value(v, _visited=_visited) + cls_attrs[k] = ser_v + if not ok_v: + cls_complete = False + return {"class": f"{mod}.{qual}", "attributes": cls_attrs}, cls_complete + + # Callable class instance + if callable(val) and hasattr(val, "__call__"): + if id(val) in _visited: + return {"recursion_ref": type(val).__qualname__}, True + inst_prov = extract_callable_provenance(val, _visited=set(_visited)) + return inst_prov, inst_prov.get("provenance_complete", True) + if hasattr(val, "__dict__"): + if id(val) in _visited: + return {"recursion_ref": type(val).__qualname__}, True + _visited.add(id(val)) try: custom_attrs = {k: v for k, v in val.__dict__.items() if not k.startswith("__")} if custom_attrs: - return _safe_provenance_value(custom_attrs) + return _safe_provenance_value(custom_attrs, _visited=_visited) except Exception: pass return f"object:{type(val).__qualname__}", False @@ -346,25 +384,37 @@ def _safe_provenance_value(val: Any) -> Tuple[Any, bool]: return s, False -def extract_callable_provenance(fn: Any) -> Dict[str, Any]: +def extract_callable_provenance(fn: Any, _visited: Optional[set[int]] = None) -> Dict[str, Any]: """Extract a stable, deterministic provenance record for a callable. Captures function identity, module, qualified name, function defaults, keyword defaults, - referenced global dependencies, closure variables, custom attributes, explicit metadata, - and a SHA-256 digest of code object bytecode, constants, and referenced names. + referenced global dependencies (recursively), closure variables, custom attributes, + explicit metadata, and a SHA-256 digest of code object bytecode, constants, and referenced names. Guarantees different callback configurations generate different digests. """ if fn is None: return {} + if _visited is None: + _visited = set() + + if id(fn) in _visited: + return { + "recursion_ref": getattr( + fn, "__qualname__", getattr(fn, "__name__", type(fn).__name__) + ), + "provenance_complete": True, + } + _visited.add(id(fn)) + provenance_complete = True # Unwrap functools.partial partial_args: List[Any] = [] partial_keywords: Dict[str, Any] = {} if isinstance(fn, functools.partial): - partial_args, ok_args = _safe_provenance_value(fn.args) - partial_keywords, ok_kw = _safe_provenance_value(fn.keywords or {}) + partial_args, ok_args = _safe_provenance_value(fn.args, _visited=set(_visited)) + partial_keywords, ok_kw = _safe_provenance_value(fn.keywords or {}, _visited=set(_visited)) if not (ok_args and ok_kw): provenance_complete = False fn = fn.func @@ -405,14 +455,14 @@ def extract_callable_provenance(fn: Any) -> Dict[str, Any]: # Function positional defaults and keyword-only defaults defaults = getattr(target_fn, "__defaults__", None) if defaults is not None: - ser_defaults, ok_def = _safe_provenance_value(defaults) + ser_defaults, ok_def = _safe_provenance_value(defaults, _visited=set(_visited)) info["defaults"] = ser_defaults if not ok_def: provenance_complete = False kwdefaults = getattr(target_fn, "__kwdefaults__", None) if kwdefaults is not None: - ser_kwdefaults, ok_kwdef = _safe_provenance_value(kwdefaults) + ser_kwdefaults, ok_kwdef = _safe_provenance_value(kwdefaults, _visited=set(_visited)) info["kwdefaults"] = ser_kwdefaults if not ok_kwdef: provenance_complete = False @@ -420,7 +470,7 @@ def extract_callable_provenance(fn: Any) -> Dict[str, Any]: # Explicit metadata contract if hasattr(fn, "metadata"): meta = getattr(fn, "metadata") - ser_meta, ok_meta = _safe_provenance_value(meta) + ser_meta, ok_meta = _safe_provenance_value(meta, _visited=set(_visited)) info["metadata"] = ser_meta if isinstance(meta, dict) and meta.get("provenance_complete") is False: provenance_complete = False @@ -438,7 +488,7 @@ def extract_callable_provenance(fn: Any) -> Dict[str, Any]: if not k.startswith("__") and k != "metadata" } if custom_attrs: - ser_attrs, ok_attrs = _safe_provenance_value(custom_attrs) + ser_attrs, ok_attrs = _safe_provenance_value(custom_attrs, _visited=set(_visited)) info["attributes"] = ser_attrs if not ok_attrs: provenance_complete = False @@ -450,7 +500,7 @@ def extract_callable_provenance(fn: Any) -> Dict[str, Any]: for var_name, cell in zip(co_freevars, target_fn.__closure__): try: contents = cell.cell_contents - ser_val, ok_val = _safe_provenance_value(contents) + ser_val, ok_val = _safe_provenance_value(contents, _visited=set(_visited)) closure_vars[var_name] = ser_val if not ok_val: provenance_complete = False @@ -459,7 +509,7 @@ def extract_callable_provenance(fn: Any) -> Dict[str, Any]: if closure_vars: info["closure"] = {k: closure_vars[k] for k in sorted(closure_vars.keys())} - # Global dependencies referenced in code.co_names + # Global dependencies referenced in code.co_names (recursively fingerprinted) if code is not None and fn_globals: co_names = getattr(code, "co_names", ()) builtin_names = set(dir(builtins)) @@ -467,7 +517,7 @@ def extract_callable_provenance(fn: Any) -> Dict[str, Any]: for name in sorted(set(co_names)): if name in fn_globals and name not in builtin_names: g_val = fn_globals[name] - ser_g, ok_g = _safe_provenance_value(g_val) + ser_g, ok_g = _safe_provenance_value(g_val, _visited=set(_visited)) ref_globals[name] = ser_g if not ok_g: provenance_complete = False @@ -1263,6 +1313,7 @@ def run_scenario( coll, coll_type = self.env._check_collision(new_state.position, prev_pos=old_pos) # Continuous swept dynamic obstacle collision check if not coll: + intermediate_positions = getattr(self.env.dynamics, "last_substep_positions", None) for dyn_obs in scenario.dynamic_obstacles: if dyn_obs.check_continuous_collision( prev_drone_pos=old_pos, @@ -1270,6 +1321,7 @@ def run_scenario( prev_time=time_sec - dt, curr_time=time_sec, vehicle_radius=scenario.collision_radius, + intermediate_drone_positions=intermediate_positions, ): coll = True coll_type = "obstacle" @@ -1445,6 +1497,7 @@ def run_scenario( collision_count=1 if is_collision else 0, control_effort=float(np.sum(control_efforts)) if control_efforts else 0.0, execution_wall_time_ms=elapsed_ms, + replan_count=mission_manager.replan_count if mission_manager is not None else 0, ) return FlightScenarioResult( diff --git a/src/adaptive_rl/navigation/dynamic_obstacles.py b/src/adaptive_rl/navigation/dynamic_obstacles.py index 17620d2..d763080 100644 --- a/src/adaptive_rl/navigation/dynamic_obstacles.py +++ b/src/adaptive_rl/navigation/dynamic_obstacles.py @@ -8,7 +8,7 @@ import math from dataclasses import dataclass, field -from typing import Any, Dict, List, Tuple +from typing import Any, Dict, List, Optional, Tuple import numpy as np @@ -74,10 +74,23 @@ def __post_init__(self) -> None: f"Arena bounds must all exceed 2 * radius ({2.0 * self.radius}), got {self.bounds}" ) + if self.motion_type == "oscillate": + for i in range(3): + b_min = self.radius + b_max = self.bounds[i] - self.radius + min_c = self.center_initial[i] - abs(self.oscillation_amplitude[i]) + max_c = self.center_initial[i] + abs(self.oscillation_amplitude[i]) + if min_c < b_min or max_c > b_max: + raise ValueError( + f"Oscillation envelope along axis {i} [{min_c}, {max_c}] exceeds " + f"permitted obstacle center region [{b_min}, {b_max}] for arena bounds {self.bounds} " + f"and radius {self.radius}" + ) + def max_speed(self) -> float: """Calculate the theoretical maximum translation speed of the obstacle.""" if self.motion_type == "oscillate": - omega = 2.0 * math.pi / max(0.1, self.oscillation_period) + omega = 2.0 * math.pi / self.oscillation_period amp_mag = float(np.linalg.norm(self.oscillation_amplitude)) return omega * amp_mag return float(np.linalg.norm(self.velocity)) @@ -86,7 +99,7 @@ def center_at(self, time_sec: float) -> np.ndarray: """Compute the deterministic 3D position of the obstacle at a given simulation time.""" t = float(time_sec) if self.motion_type == "oscillate": - omega = 2.0 * math.pi / max(0.1, self.oscillation_period) + omega = 2.0 * math.pi / self.oscillation_period offset = self.oscillation_amplitude * math.sin(omega * t) return self.center_initial + offset @@ -96,7 +109,7 @@ def center_at(self, time_sec: float) -> np.ndarray: for i in range(3): b_min = self.radius b_max = self.bounds[i] - self.radius - span = max(0.1, b_max - b_min) + span = b_max - b_min # Normalized cycle in [0, 2*span] val = (pos[i] - b_min) % (2.0 * span) if val < 0.0: @@ -107,39 +120,33 @@ def center_at(self, time_sec: float) -> np.ndarray: reflected[i] = b_max - (val - span) return reflected - def check_continuous_collision( + def _check_subsegment_collision( self, - prev_drone_pos: np.ndarray, - curr_drone_pos: np.ndarray, - prev_time: float, - curr_time: float, - vehicle_radius: float, + p0: np.ndarray, + p1: np.ndarray, + t0: float, + t1: float, + threshold: float, + veh_acc_max: float = 30.0, ) -> bool: - """Check continuous swept-sphere collision between vehicle and dynamic obstacle over [prev_time, curr_time]. - - Calculates minimum relative distance between moving vehicle segment and moving obstacle trajectory. - Conservative collision evaluation accounts for secant chord approximation errors and high speeds. - """ - p0 = np.asarray(prev_drone_pos, dtype=np.float64) - p1 = np.asarray(curr_drone_pos, dtype=np.float64) - t0 = float(prev_time) - t1 = float(curr_time) + """Evaluate continuous swept-sphere collision across a single linear vehicle segment [t0, t1].""" + p0_arr = np.asarray(p0, dtype=np.float64) + p1_arr = np.asarray(p1, dtype=np.float64) dt = t1 - t0 - threshold = float(self.radius + vehicle_radius) if dt <= 1e-9: c = self.center_at(t0) - return bool(np.linalg.norm(p1 - c) <= threshold) + return bool(np.linalg.norm(p1_arr - c) <= threshold) # Immediate endpoint checks - if np.linalg.norm(p0 - self.center_at(t0)) <= threshold: + if np.linalg.norm(p0_arr - self.center_at(t0)) <= threshold: return True - if np.linalg.norm(p1 - self.center_at(t1)) <= threshold: + if np.linalg.norm(p1_arr - self.center_at(t1)) <= threshold: return True # Adaptive subdivision based on valid maximum speed v_obs_mag = self.max_speed() - v_veh_est = float(np.linalg.norm(p1 - p0)) / dt + v_veh_est = float(np.linalg.norm(p1_arr - p0_arr)) / dt v_rel_max = max(1.0, v_obs_mag + v_veh_est) # Substep duration ensures relative displacement per subsegment <= half the threshold @@ -148,16 +155,20 @@ def check_continuous_collision( desired_substeps = max(1, int(math.ceil(dt / sub_dt))) num_substeps = min(max_substeps, desired_substeps) - # Theoretical secant approximation error bound eps_sec over subsegment tau_step + # Theoretical curvature error bounds over subsegment tau_step: + # 1. Obstacle curvature bound: tau_step = dt / num_substeps if self.motion_type == "oscillate": - omega = 2.0 * math.pi / max(0.1, self.oscillation_period) + omega = 2.0 * math.pi / self.oscillation_period amp_mag = float(np.linalg.norm(self.oscillation_amplitude)) - eps_sec = (1.0 / 8.0) * (tau_step**2) * (omega**2) * amp_mag + eps_obs = (1.0 / 8.0) * (tau_step**2) * (omega**2) * amp_mag else: - eps_sec = v_obs_mag * tau_step + eps_obs = 0.0 + + # 2. 6-DOF Vehicle curvature deviation bound from chord (||p''(t)|| <= veh_acc_max): + eps_veh = (1.0 / 8.0) * (tau_step**2) * veh_acc_max - effective_threshold = threshold + eps_sec + effective_threshold = threshold + eps_obs + eps_veh for k in range(num_substeps): frac_a = k / num_substeps @@ -165,8 +176,8 @@ def check_continuous_collision( tau_a = t0 + frac_a * dt tau_b = t0 + frac_b * dt - pa = p0 + frac_a * (p1 - p0) - pb = p0 + frac_b * (p1 - p0) + pa = p0_arr + frac_a * (p1_arr - p0_arr) + pb = p0_arr + frac_b * (p1_arr - p0_arr) ca = self.center_at(tau_a) cb = self.center_at(tau_b) @@ -189,15 +200,59 @@ def check_continuous_collision( # When computational limits are reached (desired_substeps > max_substeps), # fail conservatively if swept bounding volumes intersect. if desired_substeps > max_substeps: - mid_drone = 0.5 * (p0 + p1) + mid_drone = 0.5 * (p0_arr + p1_arr) mid_obs = 0.5 * (self.center_at(t0) + self.center_at(t1)) envelope_dist = float(np.linalg.norm(mid_drone - mid_obs)) - swept_bound = threshold + 0.5 * v_rel_max * dt + swept_bound = effective_threshold + 0.5 * v_rel_max * dt if envelope_dist <= swept_bound: return True return False + def check_continuous_collision( + self, + prev_drone_pos: np.ndarray, + curr_drone_pos: np.ndarray, + prev_time: float, + curr_time: float, + vehicle_radius: float, + intermediate_drone_positions: Optional[List[np.ndarray]] = None, + ) -> bool: + """Check continuous swept-sphere collision between vehicle and dynamic obstacle over [prev_time, curr_time]. + + Calculates relative clearance between vehicle trajectory and moving obstacle trajectory. + Supports actual 6-DOF intermediate RK4 substep positions when available, and bounds + trajectory curvature using physical acceleration limits (eps_veh <= 1/8 * a_max * dt^2). + """ + threshold = float(self.radius + vehicle_radius) + t0 = float(prev_time) + t1 = float(curr_time) + dt = t1 - t0 + + if dt <= 1e-9: + c = self.center_at(t0) + return bool(np.linalg.norm(np.asarray(curr_drone_pos) - c) <= threshold) + + if intermediate_drone_positions and len(intermediate_drone_positions) >= 2: + num_segs = len(intermediate_drone_positions) - 1 + for j in range(num_segs): + p_a = intermediate_drone_positions[j] + p_b = intermediate_drone_positions[j + 1] + t_a = t0 + (j / num_segs) * dt + t_b = t0 + ((j + 1) / num_segs) * dt + if self._check_subsegment_collision(p_a, p_b, t_a, t_b, threshold): + return True + return False + + # If intermediate positions are not provided, evaluate the single chord with full-interval vehicle curvature bound + return self._check_subsegment_collision( + np.asarray(prev_drone_pos), + np.asarray(curr_drone_pos), + t0, + t1, + threshold, + ) + def distance_to(self, point: np.ndarray, time_sec: float) -> float: """Compute the Euclidean distance from a 3D point to the obstacle center at time t.""" c = self.center_at(time_sec) @@ -235,33 +290,84 @@ def predict_occupied_spheres( leaving zero gaps in the predicted flight corridor. Uses a mathematically derived upper bound on the distance travelled across the horizon (v_max * horizon_sec) rather than discrete endpoint displacement. + + Parameters + ---------- + t_start : float + Start simulation time for the prediction horizon (finite float). + horizon_sec : float + Prediction horizon duration in seconds (non-negative finite float). + dt : float + Nominal discrete sampling interval (strictly positive finite float). + Note: The modern algorithm computes optimal continuous swept subdivisions + based on v_max and 1.2 * R overlap; dt is validated and preserved for + backwards compatibility. + max_spheres : int + Strict upper bound on the number of returned ObstacleSphere3D instances + (strictly positive integer >= 1). If computational limits clamp the + sphere count, the sphere radii are conservatively inflated to guarantee + zero gaps in the predicted flight corridor. """ - t_start = float(t_start) - horizon_sec = float(horizon_sec) - if horizon_sec <= 0.0: - return [ObstacleSphere3D(center=self.center_at(t_start), radius=self.radius)] + if isinstance(max_spheres, bool) or not isinstance(max_spheres, int) or max_spheres <= 0: + raise ValueError( + f"max_spheres must be a strictly positive integer (>= 1), got {max_spheres!r}" + ) + + if ( + isinstance(t_start, bool) + or not isinstance(t_start, (int, float)) + or not math.isfinite(t_start) + ): + raise ValueError(f"t_start must be a finite float, got {t_start!r}") + + if ( + isinstance(horizon_sec, bool) + or not isinstance(horizon_sec, (int, float)) + or not math.isfinite(horizon_sec) + ): + raise ValueError(f"horizon_sec must be a finite float, got {horizon_sec!r}") + + if horizon_sec < 0.0: + raise ValueError(f"horizon_sec must be non-negative, got {horizon_sec!r}") + + if dt is not None: + if ( + isinstance(dt, bool) + or not isinstance(dt, (int, float)) + or not math.isfinite(dt) + or dt <= 0.0 + ): + raise ValueError(f"dt must be a strictly positive finite float, got {dt!r}") + + t_start_f = float(t_start) + horizon_f = float(horizon_sec) + + if horizon_f <= 0.0: + return [ObstacleSphere3D(center=self.center_at(t_start_f), radius=self.radius)] v_max = self.max_speed() max_spacing = 1.2 * self.radius # If obstacle is essentially stationary, a single sphere at initial position suffices if v_max <= 1e-9: - return [ObstacleSphere3D(center=self.center_at(t_start), radius=self.radius)] + return [ObstacleSphere3D(center=self.center_at(t_start_f), radius=self.radius)] # Total distance upper bound across the entire prediction horizon # For any trajectory c(t), arc length s <= integral of ||c'(t)|| dt <= v_max * horizon_sec - total_dist_bound = v_max * horizon_sec + total_dist_bound = v_max * horizon_f # Required number of temporal steps to guarantee center spacing <= 1.2 * radius everywhere needed_steps = max(1, int(math.ceil(total_dist_bound / max_spacing))) + needed_spheres = needed_steps + 1 + + if needed_spheres > max_spheres: + if max_spheres == 1: + t_mid = t_start_f + 0.5 * horizon_f + eff_radius = max(self.radius, self.radius + 0.5 * total_dist_bound) + return [ObstacleSphere3D(center=self.center_at(t_mid), radius=eff_radius)] - # Handle computational limits conservatively: - # If needed_steps exceeds max_spheres, we clamp the number of evaluations, but we - # CONSERVATIVELY inflate the sphere radius so that adjacent spheres still overlap - # without leaving any gaps in the predicted flight corridor. - if needed_steps > max_spheres: - actual_steps = max_spheres - step_dt = horizon_sec / actual_steps + actual_steps = max_spheres - 1 + step_dt = horizon_f / actual_steps step_disp_bound = v_max * step_dt eff_radius = max(self.radius, step_disp_bound / 1.2) else: @@ -270,7 +376,7 @@ def predict_occupied_spheres( spheres: List[ObstacleSphere3D] = [] for i in range(actual_steps + 1): - t = t_start + (i / actual_steps) * horizon_sec + t = t_start_f + (i / actual_steps) * horizon_f spheres.append(ObstacleSphere3D(center=self.center_at(t), radius=eff_radius)) return spheres diff --git a/tests/test_navigation_dynamic_obstacles.py b/tests/test_navigation_dynamic_obstacles.py index 5a14c57..9ec1943 100644 --- a/tests/test_navigation_dynamic_obstacles.py +++ b/tests/test_navigation_dynamic_obstacles.py @@ -338,19 +338,19 @@ def test_failed_replan_hovers_safely_without_advancing_mission() -> None: def test_oscillating_obstacle_speed_bound_and_collision_detection() -> None: """Oscillating obstacle with zero linear velocity calculates valid speed bound and detects swept collision.""" obs = DynamicObstacleSphere3D( - center_initial=np.array([10.0, 10.0, 5.0]), + center_initial=np.array([15.0, 10.0, 5.0]), radius=1.0, velocity=np.zeros(3), # Zero linear velocity motion_type="oscillate", oscillation_period=2.0, # omega = 2*pi / 2 = pi - oscillation_amplitude=np.array([10.0, 0.0, 0.0]), # Amplitude 10m + oscillation_amplitude=np.array([10.0, 0.0, 0.0]), # Amplitude 10m; envelope [5.0, 25.0] ) max_spd = obs.max_speed() assert abs(max_spd - (np.pi * 10.0)) < 1e-4 coll = obs.check_continuous_collision( - prev_drone_pos=np.array([19.8, 10.0, 5.0]), - curr_drone_pos=np.array([19.8, 10.0, 5.0]), + prev_drone_pos=np.array([24.8, 10.0, 5.0]), + curr_drone_pos=np.array([24.8, 10.0, 5.0]), prev_time=0.4, curr_time=0.6, vehicle_radius=0.5, @@ -515,7 +515,7 @@ def test_predict_occupied_spheres_computational_limit_conservative() -> None: bounds=(100.0, 100.0, 50.0), ) spheres = obs.predict_occupied_spheres(t_start=0.0, horizon_sec=1.0, dt=1.0, max_spheres=100) - assert len(spheres) == 101 + assert len(spheres) == 100 eff_r = spheres[0].radius assert eff_r > obs.radius @@ -613,3 +613,249 @@ def test_end_to_end_repeated_replanning_sequential_progression() -> None: assert mgr.state == MissionState.ARRIVED assert telem.state == MissionState.ARRIVED assert telem.replan_count == 2 + + +def test_predict_occupied_spheres_parameter_safety() -> None: + """predict_occupied_spheres enforces strict parameter validation and never fails open.""" + import pytest + + obs = DynamicObstacleSphere3D( + center_initial=np.array([10.0, 10.0, 5.0]), + radius=1.0, + velocity=np.array([5.0, 0.0, 0.0]), + bounds=(30.0, 30.0, 15.0), + ) + + # 1. Invalid max_spheres (zero, negative, float, bool, non-finite) + for bad_limit in [0, -1, -100, 10.5, True, False, float("inf"), float("nan"), "100"]: + with pytest.raises(ValueError): + obs.predict_occupied_spheres(t_start=0.0, horizon_sec=2.0, max_spheres=bad_limit) # type: ignore[arg-type] + + # 2. Valid max_spheres boundary limits: 1, 2, large + s1 = obs.predict_occupied_spheres(t_start=0.0, horizon_sec=2.0, max_spheres=1) + assert len(s1) == 1 + assert s1[0].radius >= obs.radius # Inflated conservatively to cover the trajectory + + s2 = obs.predict_occupied_spheres(t_start=0.0, horizon_sec=2.0, max_spheres=2) + assert len(s2) == 2 + assert s2[0].radius >= obs.radius + + s_large = obs.predict_occupied_spheres(t_start=0.0, horizon_sec=2.0, max_spheres=10000) + assert len(s_large) <= 10000 + assert len(s_large) > 2 + + # 3. Invalid t_start and horizon_sec + with pytest.raises(ValueError, match="t_start must be a finite float"): + obs.predict_occupied_spheres(t_start=float("nan"), horizon_sec=2.0) + with pytest.raises(ValueError, match="t_start must be a finite float"): + obs.predict_occupied_spheres(t_start=float("inf"), horizon_sec=2.0) + with pytest.raises(ValueError, match="t_start must be a finite float"): + obs.predict_occupied_spheres(t_start="0.0", horizon_sec=2.0) # type: ignore[arg-type] + + with pytest.raises(ValueError, match="horizon_sec must be non-negative"): + obs.predict_occupied_spheres(t_start=0.0, horizon_sec=-1.0) + with pytest.raises(ValueError, match="horizon_sec must be a finite float"): + obs.predict_occupied_spheres(t_start=0.0, horizon_sec=float("inf")) + + # 4. Zero-length horizon returns exactly 1 sphere at t_start + s_zero = obs.predict_occupied_spheres(t_start=1.5, horizon_sec=0.0) + assert len(s_zero) == 1 + np.testing.assert_allclose(s_zero[0].center, obs.center_at(1.5)) + assert s_zero[0].radius == obs.radius + + # 5. dt validation contract + with pytest.raises(ValueError, match="dt must be a strictly positive finite float"): + obs.predict_occupied_spheres(t_start=0.0, horizon_sec=1.0, dt=-0.5) + with pytest.raises(ValueError, match="dt must be a strictly positive finite float"): + obs.predict_occupied_spheres(t_start=0.0, horizon_sec=1.0, dt=0.0) + with pytest.raises(ValueError, match="dt must be a strictly positive finite float"): + obs.predict_occupied_spheres(t_start=0.0, horizon_sec=1.0, dt=float("inf")) + + +def test_narrow_valid_arena_bounce_no_artificial_floor() -> None: + """Narrow arena with span < 0.1m reflects deterministically within [R, bounds - R].""" + # bounds[0] = 1.05m, radius = 0.5m -> b_min = 0.5m, b_max = 0.55m -> span = 0.05m (< 0.1m) + obs_pos = DynamicObstacleSphere3D( + center_initial=np.array([0.5, 10.0, 5.0]), + radius=0.5, + velocity=np.array([1.0, 0.0, 0.0]), # Positive velocity + bounds=(1.05, 30.0, 15.0), + motion_type="bounce", + ) + + # Test positive velocity over multiple reflections + for step in range(100): + t = step * 0.01 + c = obs_pos.center_at(t) + # Center x must strictly stay in [0.5, 0.55] + assert 0.5 - 1e-9 <= c[0] <= 0.55 + 1e-9, f"Violation at t={t}: c[0]={c[0]}" + + # Test negative velocity + obs_neg = DynamicObstacleSphere3D( + center_initial=np.array([0.55, 10.0, 5.0]), + radius=0.5, + velocity=np.array([-1.0, 0.0, 0.0]), # Negative velocity + bounds=(1.05, 30.0, 15.0), + motion_type="bounce", + ) + for step in range(100): + t = step * 0.01 + c = obs_neg.center_at(t) + assert 0.5 - 1e-9 <= c[0] <= 0.55 + 1e-9, f"Violation at t={t}: c[0]={c[0]}" + + +def test_oscillation_envelope_validation() -> None: + """Oscillation envelope exceeding permitted center region raises ValueError.""" + import pytest + + # Permitted center region for radius=1.0, bounds=(30, 30, 15) is [1.0, 29.0] x [1.0, 29.0] x [1.0, 14.0] + # Valid oscillation approaching boundary: + # center_initial = 25.0, amplitude = 4.0 -> envelope [21.0, 29.0] <= 29.0 -> VALID + obs_valid = DynamicObstacleSphere3D( + center_initial=np.array([25.0, 10.0, 5.0]), + radius=1.0, + bounds=(30.0, 30.0, 15.0), + motion_type="oscillate", + oscillation_amplitude=np.array([4.0, 0.0, 0.0]), + ) + assert obs_valid is not None + + # Invalid oscillation exceeding upper boundary: + # center_initial = 26.0, amplitude = 4.0 -> envelope [22.0, 30.0] > 29.0 -> INVALID + with pytest.raises(ValueError, match="Oscillation envelope along axis 0"): + DynamicObstacleSphere3D( + center_initial=np.array([26.0, 10.0, 5.0]), + radius=1.0, + bounds=(30.0, 30.0, 15.0), + motion_type="oscillate", + oscillation_amplitude=np.array([4.0, 0.0, 0.0]), + ) + + # Invalid oscillation exceeding lower boundary: + # center_initial = 4.0, amplitude = 4.0 -> envelope [0.0, 8.0], 0.0 < 1.0 -> INVALID + with pytest.raises(ValueError, match="Oscillation envelope along axis 0"): + DynamicObstacleSphere3D( + center_initial=np.array([4.0, 10.0, 5.0]), + radius=1.0, + bounds=(30.0, 30.0, 15.0), + motion_type="oscillate", + oscillation_amplitude=np.array([4.0, 0.0, 0.0]), + ) + + +def test_continuous_collision_curved_vehicle_trajectory() -> None: + """Continuous collision evaluates actual curved 6-DOF vehicle path between endpoints.""" + vehicle_radius = 0.3 + t0 = 0.0 + t1 = 1.0 + + # Consider a case where straight chord is CLEAR, but curved trajectory curves INTO the obstacle: + # Obstacle is at (10.0, 11.0, 5.0) with radius 0.5m. Threshold = 0.5 + 0.3 = 0.8m. + # Straight chord: from (8.0, 10.0, 5.0) to (12.0, 10.0, 5.0). + # Closest distance on chord to (10, 11, 5) is 1.0m > 0.8m (CLEAR!) + obs_offset = DynamicObstacleSphere3D( + center_initial=np.array([10.0, 11.0, 5.0]), + radius=0.5, + velocity=np.zeros(3), + bounds=(50.0, 50.0, 20.0), + ) + # Threshold = 0.5 + 0.3 = 0.8m + p_start = np.array([8.0, 10.0, 5.0]) + p_end = np.array([12.0, 10.0, 5.0]) + + # Straight chord: y is constantly 10.0, obstacle y is 11.0, dist = 1.0m > 0.8m + # But curved trajectory bulges along y to y = 10.7m! Closest dist = 0.3m <= 0.8m! + curved_positions = [ + np.array([8.0, 10.0, 5.0]), + np.array([9.0, 10.4, 5.0]), + np.array([10.0, 10.7, 5.0]), # Curved intrusion! Dist = 0.3m <= 0.8m + np.array([11.0, 10.4, 5.0]), + np.array([12.0, 10.0, 5.0]), + ] + + coll_curved = obs_offset.check_continuous_collision( + prev_drone_pos=p_start, + curr_drone_pos=p_end, + prev_time=t0, + curr_time=t1, + vehicle_radius=vehicle_radius, + intermediate_drone_positions=curved_positions, + ) + assert coll_curved is True, "Curved trajectory intrusion must be detected!" + + # Safe clearance curved trajectory that remains > 1.5m away + safe_positions = [ + np.array([8.0, 9.0, 5.0]), + np.array([9.0, 9.2, 5.0]), + np.array([10.0, 9.3, 5.0]), + np.array([11.0, 9.2, 5.0]), + np.array([12.0, 9.0, 5.0]), + ] + coll_safe = obs_offset.check_continuous_collision( + prev_drone_pos=safe_positions[0], + curr_drone_pos=safe_positions[-1], + prev_time=t0, + curr_time=t1, + vehicle_radius=vehicle_radius, + intermediate_drone_positions=safe_positions, + ) + assert coll_safe is False, "Safe clearance curved trajectory must not trigger collision!" + + +def test_real_closed_loop_flight_scenario_repeated_replanning() -> None: + """Real closed-loop mission with 6-DOF dynamics, PID control, moving obstacles, and 2 replans.""" + from adaptive_rl.flight_testing.scenarios import ( + FlightScenarioDefinition, + FlightScenarioRunner, + ) + + # 3 sequential waypoints across the arena + wp1 = np.array([5.0, 5.0, 4.0]) + wp2 = np.array([11.0, 5.0, 4.0]) + wp3 = np.array([17.0, 5.0, 4.0]) + + # Dynamic obstacle 1: moving across segment 1 (x=3) from y=2 to y=6 + obs1 = DynamicObstacleSphere3D( + center_initial=np.array([3.0, 2.0, 4.0]), + radius=0.7, + velocity=np.array([0.0, 0.4, 0.0]), + bounds=(30.0, 30.0, 15.0), + ) + + # Dynamic obstacle 2: moving across segment 2 (x=8) from y=8 to y=3 + obs2 = DynamicObstacleSphere3D( + center_initial=np.array([8.0, 8.0, 4.0]), + radius=0.7, + velocity=np.array([0.0, -0.4, 0.0]), + bounds=(30.0, 30.0, 15.0), + ) + + sc = FlightScenarioDefinition( + name="closed_loop_sequential_replanning_mission", + description="Closed-loop 6-DOF PID flight through 3 sequential waypoints with dynamic obstacle evasion", + initial_position=np.array([1.0, 5.0, 4.0]), + waypoints=[wp1, wp2, wp3], + dynamic_obstacles=[obs1, obs2], + use_path_planner=True, + collision_radius=0.25, + target_radius=1.5, + settling_velocity_threshold=1.5, + max_steps=650, + ) + + runner = FlightScenarioRunner() + result = runner.run_scenario(sc) + + # Assertions on real flight execution: + # 1. Controller-driven actual 6-DOF motion occurred: + assert len(result.telemetry) > 20 + assert result.metrics.control_effort > 0.0 + + # 2. Sequential waypoints were achieved in order: + assert result.metrics.replan_count >= 2 + assert result.metrics.outcome == "success" + assert result.metrics.success is True + assert result.metrics.collision_count == 0 + + # 3. Final position error within target radius: + assert result.metrics.final_position_error <= sc.target_radius diff --git a/tests/test_recorder_fingerprint_phase3.py b/tests/test_recorder_fingerprint_phase3.py index 8ff737c..3fd76b8 100644 --- a/tests/test_recorder_fingerprint_phase3.py +++ b/tests/test_recorder_fingerprint_phase3.py @@ -619,3 +619,203 @@ def test_explicit_configuration_conflicting_completeness_detected(tmp_path: Path # Both verifications must catch the inconsistency! assert record.verify_config_fingerprint() is False assert record.verify_record_digest() is False + + +# Global helpers for referenced callable dependency testing +def _global_helper_v1(x: float) -> float: + return x * 2.0 + + +def _global_helper_v2(x: float) -> float: + return x * 10.0 + + +def test_referenced_global_helper_code_change_alters_provenance() -> None: + """Changing a referenced global helper's code changes the parent callable's provenance digest.""" + from adaptive_rl.flight_testing.scenarios import extract_callable_provenance + + # Function referencing global helper + def parent_fn(x: float) -> float: + return _global_helper_v1(x) + + def parent_fn_v2(x: float) -> float: + return _global_helper_v2(x) + + prov1 = extract_callable_provenance(parent_fn) + prov2 = extract_callable_provenance(parent_fn_v2) + + # Both parent functions have identical names, signatures, but different referenced helpers + assert canonical_json_dumps(prov1) != canonical_json_dumps(prov2) + assert ( + prov1["referenced_globals"]["_global_helper_v1"]["code_hash"] + != (prov2["referenced_globals"]["_global_helper_v2"]["code_hash"]) + ) + + +def test_referenced_global_helper_defaults_and_closures_alter_provenance() -> None: + """Changing helper default arguments or closure variables alters the parent callback's provenance.""" + from adaptive_rl.flight_testing.scenarios import extract_callable_provenance + + def make_parent(gain: float): + def inner_helper(val: float, scale: float = 1.0) -> float: + return val * scale * gain + + def parent_cb(x: float) -> float: + return inner_helper(x) + + return parent_cb + + cb1 = make_parent(gain=2.0) + cb2 = make_parent(gain=5.0) + + p1 = extract_callable_provenance(cb1) + p2 = extract_callable_provenance(cb2) + + assert canonical_json_dumps(p1) != canonical_json_dumps(p2) + + +def _module_recursive_factorial(n: int) -> int: + return 1 if n <= 1 else n * _module_recursive_factorial(n - 1) + + +def test_recursive_callable_dependencies_terminate_safely() -> None: + """Self-recursive and mutually recursive callable dependencies terminate deterministically without error.""" + from adaptive_rl.flight_testing.scenarios import extract_callable_provenance + + prov_fact = extract_callable_provenance(_module_recursive_factorial) + assert prov_fact["provenance_complete"] is True + # The referenced global '_module_recursive_factorial' contains a recursion reference + assert "_module_recursive_factorial" in prov_fact["referenced_globals"] + assert "recursion_ref" in prov_fact["referenced_globals"]["_module_recursive_factorial"] + + +def test_explicit_configuration_content_mismatch_marked_incomplete(tmp_path: Path) -> None: + """Explicit configuration with matching scenario names but differing scenario definitions is marked incomplete.""" + from adaptive_rl.flight_testing.scenarios import ( + FlightScenarioDefinition, + FlightScenarioRunner, + ) + + runner = FlightScenarioRunner() + sc_real = FlightScenarioDefinition( + name="hover_scenario", + description="real execution", + initial_position=np.array([1.0, 1.0, 1.0]), + max_steps=5, + ) + res = runner.run_scenario(sc_real) + + physics = PhysicsSimulationFingerprint( + mass=1.0, + gravity=9.81, + arm_length=0.2, + arm_span=0.4, + rotor_offset_d=0.1414, + rotor_radius=0.09, + max_thrust_per_motor=5.0, + torque_to_thrust_ratio=0.01, + linear_damping=0.1, + angular_damping=0.02, + inertia=[0.01, 0.01, 0.02], + dt=0.05, + substeps=5, + substep_dt=0.01, + body_frame_lidar=False, + num_lidar_rays=16, + lidar_range=20.0, + ) + + # Construct false configuration: same name "hover_scenario", but different initial_position [9.0, 9.0, 9.0]! + sc_fake = FlightScenarioDefinition( + name="hover_scenario", + description="falsified execution", + initial_position=np.array([9.0, 9.0, 9.0]), + max_steps=5, + ) + + cfg = ExperimentConfigurationFingerprint( + physics=physics, + environment_bounds=[30.0, 30.0, 15.0], + algorithm_name="test", + checkpoint_hash=None, + random_seed=42, + scenario_names=["hover_scenario"], + scenarios=[sc_fake.to_dict()], + provenance_complete=True, + ) + + recorder = FlightRecorder(output_dir=tmp_path) + record = recorder.create_record( + experiment_id="content_mismatch_test", + physics_params={"mass": 1.0, "gravity": 9.81}, + scenario_results=[res], + configuration=cfg, + ) + + # Content mismatch MUST result in provenance_complete = False! + assert record.provenance_complete is False + assert record.configuration.provenance_complete is False + assert record.verify_config_fingerprint() is True + + +def test_explicit_configuration_fully_equivalent_preserved(tmp_path: Path) -> None: + """Explicit configuration with matching scenario content preserves provenance_complete = True.""" + from adaptive_rl.flight_testing.scenarios import ( + FlightScenarioDefinition, + FlightScenarioRunner, + ) + + runner = FlightScenarioRunner() + sc = FlightScenarioDefinition( + name="hover_scenario_equiv", + description="legitimate execution", + initial_position=np.array([2.0, 2.0, 2.0]), + max_steps=5, + ) + res = runner.run_scenario(sc) + + physics = PhysicsSimulationFingerprint( + mass=1.0, + gravity=9.81, + arm_length=0.2, + arm_span=0.4, + rotor_offset_d=0.1414, + rotor_radius=0.09, + max_thrust_per_motor=5.0, + torque_to_thrust_ratio=0.01, + linear_damping=0.1, + angular_damping=0.02, + inertia=[0.01, 0.01, 0.02], + dt=0.05, + substeps=5, + substep_dt=0.01, + body_frame_lidar=False, + num_lidar_rays=16, + lidar_range=20.0, + ) + + # Legitimate matching configuration + cfg = ExperimentConfigurationFingerprint( + physics=physics, + environment_bounds=[30.0, 30.0, 15.0], + algorithm_name="test", + checkpoint_hash=None, + random_seed=42, + scenario_names=["hover_scenario_equiv"], + scenarios=[sc.to_dict()], + provenance_complete=True, + ) + + recorder = FlightRecorder(output_dir=tmp_path) + record = recorder.create_record( + experiment_id="content_match_test", + physics_params={"mass": 1.0, "gravity": 9.81}, + scenario_results=[res], + configuration=cfg, + ) + + # Legitimate match MUST preserve provenance_complete = True! + assert record.provenance_complete is True + assert record.configuration.provenance_complete is True + assert record.verify_config_fingerprint() is True + assert record.verify_record_digest() is True From 65c58656d20e542bb814a7833ec2f29d310158f5 Mon Sep 17 00:00:00 2001 From: Aryan Saxena Date: Sun, 15 Nov 2026 22:49:38 +0530 Subject: [PATCH 6/7] fix(phase3): close swept volume coverage, defensible curvature, curved static/boundary safety, and recursive provenance --- src/adaptive_rl/environments/drone_6dof.py | 143 +++++++++-- src/adaptive_rl/flight_testing/recorder.py | 68 ++--- src/adaptive_rl/flight_testing/scenarios.py | 130 ++++++++-- .../navigation/dynamic_obstacles.py | 105 +++++--- tests/test_navigation_dynamic_obstacles.py | 242 ++++++++++++++++++ tests/test_recorder_fingerprint_phase3.py | 165 ++++++++++++ 6 files changed, 732 insertions(+), 121 deletions(-) diff --git a/src/adaptive_rl/environments/drone_6dof.py b/src/adaptive_rl/environments/drone_6dof.py index 986c1a1..e9f3658 100644 --- a/src/adaptive_rl/environments/drone_6dof.py +++ b/src/adaptive_rl/environments/drone_6dof.py @@ -481,6 +481,38 @@ def reset( self.state.angular_velocity = np.zeros(3, dtype=np.float64) self.actual_motor_thrusts = np.full(4, self.hover_thrust_per_motor, dtype=np.float64) + self.last_substep_positions = [self.state.position.copy()] + + def max_translational_acceleration(self, max_speed_bound: float = 25.0) -> float: + """Derive an analytical upper bound on translational acceleration ||p_ddot|| in m/s^2. + + From the translational equations of motion: + p_ddot = (1/m)*R(q)*[0, 0, T] + [0, 0, -g] + F_drag / m + By triangle inequality: + ||p_ddot|| <= (T_max / m) + g + (||F_drag||_max / m) + + T_max incorporates Cheeseman-Bennett ground effect amplification (up to 1.30) + when enabled. F_drag considers linear damping and quadratic drag over relative + airspeed up to max_speed_bound + ||wind_vector||. + """ + # Maximum collective thrust across all 4 motors + t_max_nominal = 4.0 * self.max_thrust_per_motor + if self.enable_ground_effect: + t_max = 1.30 * t_max_nominal + else: + t_max = t_max_nominal + + # Airspeed bound + wind_speed = float(np.linalg.norm(self.wind_vector)) + v_rel_max = max(0.0, float(max_speed_bound)) + wind_speed + + # Drag force upper bound + drag_force_max = self.linear_damping * v_rel_max + if self.quadratic_drag_coeff > 0.0: + drag_force_max += self.quadratic_drag_coeff * (v_rel_max**2) + + acc_max = (t_max / self.mass) + self.gravity + (drag_force_max / self.mass) + return float(acc_max) def compute_forces_and_torques( self, motor_thrusts: np.ndarray, altitude: Optional[float] = None @@ -1015,10 +1047,15 @@ def _get_obs(self, custom_state: Optional[DroneState6DOF] = None) -> np.ndarray: return np.asarray(np.clip(raw, -1.0, 1.0), dtype=np.float32) def _check_collision( - self, pos: np.ndarray, prev_pos: Optional[np.ndarray] = None + self, + pos: np.ndarray, + prev_pos: Optional[np.ndarray] = None, + intermediate_positions: Optional[Sequence[np.ndarray]] = None, + curvature_error_bound: float = 0.0, ) -> Tuple[bool, str]: - r = self.collision_radius + r = float(self.collision_radius) bx, by, bz = self.bounds + eps_curv = max(0.0, float(curvature_error_bound)) if prev_pos is None: if pos[0] - r <= 0.0 or pos[0] + r >= bx: @@ -1033,36 +1070,76 @@ def _check_collision( 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 min_p[1] - r <= 0.0 or max_p[1] + r >= by: - return True, "boundary_y" - if min_p[2] - r <= 0.0 or max_p[2] + r >= bz: - return True, "boundary_z" + if p0.shape != (3,) or not np.all(np.isfinite(p0)): + raise ValueError(f"prev_pos must be a finite 3-element vector, got {prev_pos}") + if p1.shape != (3,) or not np.all(np.isfinite(p1)): + raise ValueError(f"pos must be a finite 3-element vector, got {pos}") + + # If intermediate positions are provided, decompose path into subsegments + if intermediate_positions is not None: + if not isinstance(intermediate_positions, (list, tuple)): + raise ValueError("intermediate_positions must be a sequence of 3D coordinates") + if len(intermediate_positions) < 2: + raise ValueError("intermediate_positions must contain at least 2 positions") + + validated_pts: List[np.ndarray] = [] + for idx, pt in enumerate(intermediate_positions): + arr = np.asarray(pt, dtype=np.float64) + if arr.shape != (3,): + raise ValueError( + f"intermediate_positions[{idx}] must have shape (3,), got {arr.shape}" + ) + if not np.all(np.isfinite(arr)): + raise ValueError( + f"intermediate_positions[{idx}] contains non-finite values: {arr}" + ) + validated_pts.append(arr) + + if np.linalg.norm(validated_pts[0] - p0) > 1e-3: + raise ValueError( + f"intermediate_positions[0] {validated_pts[0]} does not match prev_pos {p0}" + ) + if np.linalg.norm(validated_pts[-1] - p1) > 1e-3: + raise ValueError( + f"intermediate_positions[-1] {validated_pts[-1]} does not match pos {p1}" + ) + + segments = [ + (validated_pts[k], validated_pts[k + 1]) for k in range(len(validated_pts) - 1) + ] + else: + segments = [(p0, p1)] - # 2. Swept obstacle spheres check - seg = p1 - p0 - seg_len_sq = float(np.dot(seg, seg)) + for p_start, p_end in segments: + # 1. Swept boundary check with curvature error bound + min_p = np.minimum(p_start, p_end) + max_p = np.maximum(p_start, p_end) + if min_p[0] - r - eps_curv <= 0.0 or max_p[0] + r + eps_curv >= bx: + return True, "boundary_x" + if min_p[1] - r - eps_curv <= 0.0 or max_p[1] + r + eps_curv >= by: + return True, "boundary_y" + if min_p[2] - r - eps_curv <= 0.0 or max_p[2] + r + eps_curv >= bz: + return True, "boundary_z" - for obs in self._obstacles: - 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" + # 2. Swept obstacle spheres check with curvature error bound + seg = p_end - p_start + seg_len_sq = float(np.dot(seg, seg)) + + for obs in self._obstacles: + c = obs.center + threshold = obs.radius + r + eps_curv + if seg_len_sq < 1e-12: + dist = float(np.linalg.norm(p_start - c)) + else: + t = float(np.dot(c - p_start, seg) / seg_len_sq) + t_clamped = max(0.0, min(1.0, t)) + closest_point = p_start + t_clamped * seg + dist = float(np.linalg.norm(closest_point - c)) + if dist <= threshold: + return True, "obstacle" return False, "none" @@ -1279,7 +1356,17 @@ def step( 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, prev_pos=old_pos) + a_max = self.dynamics.max_translational_acceleration(self.max_velocity) + substep_h = self.dynamics.substep_dt + eps_curv = 0.125 * a_max * (substep_h**2) + inter_pos = self.dynamics.last_substep_positions + + is_collision, collision_type = self._check_collision( + self._position, + prev_pos=old_pos, + intermediate_positions=inter_pos, + curvature_error_bound=eps_curv, + ) is_success = curr_distance <= self.target_radius terminated = False diff --git a/src/adaptive_rl/flight_testing/recorder.py b/src/adaptive_rl/flight_testing/recorder.py index 7d4ffb8..d863241 100644 --- a/src/adaptive_rl/flight_testing/recorder.py +++ b/src/adaptive_rl/flight_testing/recorder.py @@ -381,17 +381,25 @@ def create_record( scenario_provenance.sort(key=lambda s: str(s.get("name", ""))) # Verify whether all scenario_names have corresponding provenance - expected_scenario_names = set(m.scenario_name for m in metrics_list) - found_scenario_names = set(s.get("name") for s in scenario_provenance if "name" in s) - if expected_scenario_names and not expected_scenario_names.issubset(found_scenario_names): + expected_scenario_names = sorted(str(m.scenario_name) for m in metrics_list) + found_scenario_names = sorted( + str(s.get("name", "")) + for s in scenario_provenance + if isinstance(s, dict) and "name" in s + ) + if expected_scenario_names != found_scenario_names: provenance_complete = False if not expected_scenario_names and not scenario_provenance: provenance_complete = True - # Map executed scenario definitions by name - exec_sc_by_name = { - s.get("name"): s for s in scenario_provenance if isinstance(s, dict) and "name" in s - } + def _canonical_sc_repr(sc: Any) -> str: + if not isinstance(sc, dict): + return str(sc) + # Filter out provenance_complete flag, keeping critical scenario definitions + # (name, initial_position, initial_velocity, initial_quaternion, waypoints, obstacles, + # dynamic_obstacles, bounds, collision_radius, target_radius, controller settings) + norm = {k: v for k, v in sc.items() if k != "provenance_complete"} + return canonical_json_dumps(norm) # Build comprehensive ExperimentConfigurationFingerprint if configuration is not None: @@ -404,37 +412,31 @@ def create_record( # Validate explicitly supplied configuration against executed scenarios: cfg_valid = True - if expected_scenario_names: - cfg_names = set(exp_config.scenario_names) - if not expected_scenario_names.issubset(cfg_names): - cfg_valid = False - - cfg_sc_by_name = { - s.get("name"): s - for s in exp_config.scenarios - if isinstance(s, dict) and "name" in s - } - if not expected_scenario_names.issubset(cfg_sc_by_name.keys()): - cfg_valid = False - - # Compare canonical serialized scenario definitions (not only names) - for sc_name in expected_scenario_names: - exec_def = exec_sc_by_name.get(sc_name) - cfg_def = cfg_sc_by_name.get(sc_name) - if exec_def is None or cfg_def is None: + cfg_scenario_names = sorted(exp_config.scenario_names) + if expected_scenario_names != cfg_scenario_names: + cfg_valid = False + + if len(scenario_provenance) != len(exp_config.scenarios): + cfg_valid = False + else: + # 1-to-1 canonical definition matching of sorted lists (handles duplicates without collision) + sorted_exec = sorted( + scenario_provenance, + key=lambda s: (str(s.get("name", "")), _canonical_sc_repr(s)), + ) + sorted_cfg = sorted( + exp_config.scenarios, + key=lambda s: (str(s.get("name", "")), _canonical_sc_repr(s)), + ) + + for exec_sc, cfg_sc in zip(sorted_exec, sorted_cfg): + if not isinstance(cfg_sc, dict) or cfg_sc.get("provenance_complete") is False: cfg_valid = False break - # Normalize comparison: compare behavior-affecting content - exec_content = {k: v for k, v in exec_def.items() if k != "provenance_complete"} - cfg_content = {k: v for k, v in cfg_def.items() if k != "provenance_complete"} - if canonical_json_dumps(exec_content) != canonical_json_dumps(cfg_content): + if _canonical_sc_repr(exec_sc) != _canonical_sc_repr(cfg_sc): cfg_valid = False break - for sc in exp_config.scenarios: - if isinstance(sc, dict) and sc.get("provenance_complete") is False: - cfg_valid = False - # Define ONE authoritative provenance completeness result authoritative_complete = ( provenance_complete and cfg_valid and bool(exp_config.provenance_complete) diff --git a/src/adaptive_rl/flight_testing/scenarios.py b/src/adaptive_rl/flight_testing/scenarios.py index ded6c63..f5777e4 100644 --- a/src/adaptive_rl/flight_testing/scenarios.py +++ b/src/adaptive_rl/flight_testing/scenarios.py @@ -347,16 +347,31 @@ def _safe_provenance_value(val: Any, _visited: Optional[set[int]] = None) -> Tup qual = getattr(val, "__qualname__", getattr(val, "__name__", str(val))) if mod == "builtins" or mod.startswith("numpy") or mod.startswith("scipy"): return f"class:{mod}.{qual}", True - # User/custom class: fingerprint class attributes + + # Custom class: inspect MRO for base classes and inherited methods + mro_bases = [ + f"{b.__module__}.{b.__qualname__}" + for b in inspect.getmro(val) + if b not in (object,) and getattr(b, "__module__", "") != "builtins" + ] cls_attrs: Dict[str, Any] = {} cls_complete = True - for k, v in sorted(val.__dict__.items(), key=lambda item: str(item[0])): - if not k.startswith("__"): - ser_v, ok_v = _safe_provenance_value(v, _visited=_visited) - cls_attrs[k] = ser_v - if not ok_v: - cls_complete = False - return {"class": f"{mod}.{qual}", "attributes": cls_attrs}, cls_complete + + for base in reversed(inspect.getmro(val)): + if base is object or getattr(base, "__module__", "") == "builtins": + continue + for k, v in sorted(base.__dict__.items(), key=lambda item: str(item[0])): + if not k.startswith("__") or k in ("__call__", "__init__"): + ser_v, ok_v = _safe_provenance_value(v, _visited=_visited) + cls_attrs[k] = ser_v + if not ok_v: + cls_complete = False + + return { + "class": f"{mod}.{qual}", + "mro": mro_bases, + "attributes": cls_attrs, + }, cls_complete # Callable class instance if callable(val) and hasattr(val, "__call__"): @@ -384,6 +399,20 @@ def _safe_provenance_value(val: Any, _visited: Optional[set[int]] = None) -> Tup return s, False +def _hash_code_recursive(code_obj: Any, hasher: Any) -> None: + """Recursively hash code object bytecode, referenced names, and nested code constants.""" + hasher.update(getattr(code_obj, "co_code", b"")) + for name in getattr(code_obj, "co_names", ()): + hasher.update(name.encode("utf-8", errors="replace")) + for vname in getattr(code_obj, "co_varnames", ()): + hasher.update(vname.encode("utf-8", errors="replace")) + for const in getattr(code_obj, "co_consts", ()): + if inspect.iscode(const) or hasattr(const, "co_code"): + _hash_code_recursive(const, hasher) + else: + hasher.update(str(const).encode("utf-8", errors="replace")) + + def extract_callable_provenance(fn: Any, _visited: Optional[set[int]] = None) -> Dict[str, Any]: """Extract a stable, deterministic provenance record for a callable. @@ -445,7 +474,27 @@ def extract_callable_provenance(fn: Any, _visited: Optional[set[int]] = None) -> info["module"] = str(fn.__module__) if is_callable_instance: - info["callable_class"] = f"{type(fn).__module__}.{type(fn).__qualname__}" + cls = type(fn) + info["callable_class"] = f"{cls.__module__}.{cls.__qualname__}" + info["class_mro"] = [ + f"{b.__module__}.{b.__qualname__}" + for b in inspect.getmro(cls) + if b not in (object,) and getattr(b, "__module__", "") != "builtins" + ] + # Inspect inherited methods across MRO + inherited_methods: Dict[str, Any] = {} + for base in reversed(inspect.getmro(cls)): + if base is object or getattr(base, "__module__", "") == "builtins": + continue + for k, v in base.__dict__.items(): + if not k.startswith("__") or k == "__call__": + if inspect.isfunction(v) or inspect.ismethod(v): + m_prov, m_ok = _safe_provenance_value(v, _visited=set(_visited)) + inherited_methods[k] = m_prov + if not m_ok: + provenance_complete = False + if inherited_methods: + info["class_methods"] = inherited_methods if partial_args: info["partial_args"] = partial_args @@ -517,10 +566,33 @@ def extract_callable_provenance(fn: Any, _visited: Optional[set[int]] = None) -> for name in sorted(set(co_names)): if name in fn_globals and name not in builtin_names: g_val = fn_globals[name] - ser_g, ok_g = _safe_provenance_value(g_val, _visited=set(_visited)) - ref_globals[name] = ser_g - if not ok_g: - provenance_complete = False + if inspect.ismodule(g_val): + # Resolve module-qualified dependencies referenced in co_names + mod_attrs: Dict[str, Any] = {} + mod_all_ok = True + for attr_name in sorted(set(co_names)): + if hasattr(g_val, attr_name) and attr_name not in builtin_names: + try: + attr_obj = getattr(g_val, attr_name) + ser_attr, ok_attr = _safe_provenance_value( + attr_obj, _visited=set(_visited) + ) + mod_attrs[attr_name] = ser_attr + if not ok_attr: + mod_all_ok = False + except Exception: + mod_attrs[attr_name] = f"unresolvable:{attr_name}" + mod_all_ok = False + if mod_attrs: + ref_globals[f"{name}.attributes"] = mod_attrs + ref_globals[name] = f"module:{g_val.__name__}" + if not mod_all_ok: + provenance_complete = False + else: + ser_g, ok_g = _safe_provenance_value(g_val, _visited=set(_visited)) + ref_globals[name] = ser_g + if not ok_g: + provenance_complete = False if ref_globals: info["referenced_globals"] = ref_globals @@ -535,17 +607,7 @@ def extract_callable_provenance(fn: Any, _visited: Optional[set[int]] = None) -> } hasher = hashlib.sha256() - hasher.update(getattr(code, "co_code", b"")) - co_consts = getattr(code, "co_consts", ()) - for const in co_consts: - if hasattr(const, "co_code"): - hasher.update(const.co_code) - else: - hasher.update(str(const).encode("utf-8", errors="replace")) - for name in getattr(code, "co_names", ()): - hasher.update(name.encode("utf-8", errors="replace")) - for vname in getattr(code, "co_varnames", ()): - hasher.update(vname.encode("utf-8", errors="replace")) + _hash_code_recursive(code, hasher) info["code_hash"] = hasher.hexdigest() info["provenance_complete"] = provenance_complete @@ -1309,11 +1371,24 @@ def run_scenario( min_obs_d = d min_clearances.append(min_obs_d) - # Collision check - coll, coll_type = self.env._check_collision(new_state.position, prev_pos=old_pos) + # Collision check with analytical curvature error bound + a_max = ( + self.env.dynamics.max_translational_acceleration(25.0) + if hasattr(self.env.dynamics, "max_translational_acceleration") + else 35.0 + ) + substep_h = getattr(self.env.dynamics, "substep_dt", dt) + eps_curv = 0.125 * a_max * (substep_h**2) + intermediate_positions = getattr(self.env.dynamics, "last_substep_positions", None) + + coll, coll_type = self.env._check_collision( + new_state.position, + prev_pos=old_pos, + intermediate_positions=intermediate_positions, + curvature_error_bound=eps_curv, + ) # Continuous swept dynamic obstacle collision check if not coll: - intermediate_positions = getattr(self.env.dynamics, "last_substep_positions", None) for dyn_obs in scenario.dynamic_obstacles: if dyn_obs.check_continuous_collision( prev_drone_pos=old_pos, @@ -1322,6 +1397,7 @@ def run_scenario( curr_time=time_sec, vehicle_radius=scenario.collision_radius, intermediate_drone_positions=intermediate_positions, + veh_acc_max=a_max, ): coll = True coll_type = "obstacle" diff --git a/src/adaptive_rl/navigation/dynamic_obstacles.py b/src/adaptive_rl/navigation/dynamic_obstacles.py index d763080..7c6eedc 100644 --- a/src/adaptive_rl/navigation/dynamic_obstacles.py +++ b/src/adaptive_rl/navigation/dynamic_obstacles.py @@ -127,7 +127,7 @@ def _check_subsegment_collision( t0: float, t1: float, threshold: float, - veh_acc_max: float = 30.0, + veh_acc_max: float = 35.0, ) -> bool: """Evaluate continuous swept-sphere collision across a single linear vehicle segment [t0, t1].""" p0_arr = np.asarray(p0, dtype=np.float64) @@ -217,40 +217,83 @@ def check_continuous_collision( curr_time: float, vehicle_radius: float, intermediate_drone_positions: Optional[List[np.ndarray]] = None, + veh_acc_max: float = 35.0, ) -> bool: """Check continuous swept-sphere collision between vehicle and dynamic obstacle over [prev_time, curr_time]. Calculates relative clearance between vehicle trajectory and moving obstacle trajectory. Supports actual 6-DOF intermediate RK4 substep positions when available, and bounds trajectory curvature using physical acceleration limits (eps_veh <= 1/8 * a_max * dt^2). + Fails closed on malformed or inconsistent intermediate trajectory data. """ threshold = float(self.radius + vehicle_radius) t0 = float(prev_time) t1 = float(curr_time) dt = t1 - t0 + p_start_check = np.asarray(prev_drone_pos, dtype=np.float64) + p_end_check = np.asarray(curr_drone_pos, dtype=np.float64) + + if p_start_check.shape != (3,) or not np.all(np.isfinite(p_start_check)): + raise ValueError(f"prev_drone_pos must be a finite 3D vector, got {prev_drone_pos}") + if p_end_check.shape != (3,) or not np.all(np.isfinite(p_end_check)): + raise ValueError(f"curr_drone_pos must be a finite 3D vector, got {curr_drone_pos}") + if dt <= 1e-9: c = self.center_at(t0) - return bool(np.linalg.norm(np.asarray(curr_drone_pos) - c) <= threshold) - - if intermediate_drone_positions and len(intermediate_drone_positions) >= 2: - num_segs = len(intermediate_drone_positions) - 1 + return bool(np.linalg.norm(p_end_check - c) <= threshold) + + if intermediate_drone_positions is not None: + if not isinstance(intermediate_drone_positions, (list, tuple)): + raise ValueError( + "intermediate_drone_positions must be a sequence of 3D coordinates" + ) + if len(intermediate_drone_positions) < 2: + raise ValueError("intermediate_drone_positions must contain at least 2 positions") + + validated_pts: List[np.ndarray] = [] + for idx, pt in enumerate(intermediate_drone_positions): + arr = np.asarray(pt, dtype=np.float64) + if arr.shape != (3,): + raise ValueError( + f"intermediate_drone_positions[{idx}] must have shape (3,), got {arr.shape}" + ) + if not np.all(np.isfinite(arr)): + raise ValueError( + f"intermediate_drone_positions[{idx}] contains non-finite values: {arr}" + ) + validated_pts.append(arr) + + # Agreement with endpoints + if np.linalg.norm(validated_pts[0] - p_start_check) > 1e-3: + raise ValueError( + f"intermediate_drone_positions[0] {validated_pts[0]} does not match prev_drone_pos {p_start_check}" + ) + if np.linalg.norm(validated_pts[-1] - p_end_check) > 1e-3: + raise ValueError( + f"intermediate_drone_positions[-1] {validated_pts[-1]} does not match curr_drone_pos {p_end_check}" + ) + + num_segs = len(validated_pts) - 1 for j in range(num_segs): - p_a = intermediate_drone_positions[j] - p_b = intermediate_drone_positions[j + 1] + p_a = validated_pts[j] + p_b = validated_pts[j + 1] t_a = t0 + (j / num_segs) * dt t_b = t0 + ((j + 1) / num_segs) * dt - if self._check_subsegment_collision(p_a, p_b, t_a, t_b, threshold): + if self._check_subsegment_collision( + p_a, p_b, t_a, t_b, threshold, veh_acc_max=veh_acc_max + ): return True return False # If intermediate positions are not provided, evaluate the single chord with full-interval vehicle curvature bound return self._check_subsegment_collision( - np.asarray(prev_drone_pos), - np.asarray(curr_drone_pos), + p_start_check, + p_end_check, t0, t1, threshold, + veh_acc_max=veh_acc_max, ) def distance_to(self, point: np.ndarray, time_sec: float) -> float: @@ -330,14 +373,14 @@ def predict_occupied_spheres( if horizon_sec < 0.0: raise ValueError(f"horizon_sec must be non-negative, got {horizon_sec!r}") - if dt is not None: - if ( - isinstance(dt, bool) - or not isinstance(dt, (int, float)) - or not math.isfinite(dt) - or dt <= 0.0 - ): - raise ValueError(f"dt must be a strictly positive finite float, got {dt!r}") + if ( + dt is None + or isinstance(dt, bool) + or not isinstance(dt, (int, float)) + or not math.isfinite(dt) + or dt <= 0.0 + ): + raise ValueError(f"dt must be a strictly positive finite float, got {dt!r}") t_start_f = float(t_start) horizon_f = float(horizon_sec) @@ -358,21 +401,17 @@ def predict_occupied_spheres( # Required number of temporal steps to guarantee center spacing <= 1.2 * radius everywhere needed_steps = max(1, int(math.ceil(total_dist_bound / max_spacing))) - needed_spheres = needed_steps + 1 - - if needed_spheres > max_spheres: - if max_spheres == 1: - t_mid = t_start_f + 0.5 * horizon_f - eff_radius = max(self.radius, self.radius + 0.5 * total_dist_bound) - return [ObstacleSphere3D(center=self.center_at(t_mid), radius=eff_radius)] - - actual_steps = max_spheres - 1 - step_dt = horizon_f / actual_steps - step_disp_bound = v_max * step_dt - eff_radius = max(self.radius, step_disp_bound / 1.2) - else: - actual_steps = needed_steps - eff_radius = self.radius + + if max_spheres == 1: + t_mid = t_start_f + 0.5 * horizon_f + eff_radius = self.radius + 0.5 * total_dist_bound + return [ObstacleSphere3D(center=self.center_at(t_mid), radius=eff_radius)] + + actual_steps = min(needed_steps, max_spheres - 1) + actual_steps = max(1, actual_steps) + step_dt = horizon_f / actual_steps + step_disp_bound = v_max * step_dt + eff_radius = max(self.radius + 0.5 * step_disp_bound, step_disp_bound / 1.2) spheres: List[ObstacleSphere3D] = [] for i in range(actual_steps + 1): diff --git a/tests/test_navigation_dynamic_obstacles.py b/tests/test_navigation_dynamic_obstacles.py index 9ec1943..9fdb76c 100644 --- a/tests/test_navigation_dynamic_obstacles.py +++ b/tests/test_navigation_dynamic_obstacles.py @@ -859,3 +859,245 @@ def test_real_closed_loop_flight_scenario_repeated_replanning() -> None: # 3. Final position error within target radius: assert result.metrics.final_position_error <= sc.target_radius + + # 4. Telemetry progression: t_{reached, wp1} < t_{reached, wp2} < t_{reached, wp3} + t_wp1 = None + t_wp2 = None + t_wp3 = None + for telem in result.telemetry: + pos = telem.position + if t_wp1 is None and np.linalg.norm(pos - wp1) <= sc.target_radius: + t_wp1 = telem.time_sec + if t_wp2 is None and np.linalg.norm(pos - wp2) <= sc.target_radius: + t_wp2 = telem.time_sec + if t_wp3 is None and np.linalg.norm(pos - wp3) <= sc.target_radius: + t_wp3 = telem.time_sec + + assert t_wp1 is not None, "Waypoint 1 was never reached" + assert t_wp2 is not None, "Waypoint 2 was never reached" + assert t_wp3 is not None, "Waypoint 3 was never reached" + assert t_wp1 < t_wp2 < t_wp3, ( + f"Waypoints reached out of order: t1={t_wp1}, t2={t_wp2}, t3={t_wp3}" + ) + + +def test_predict_occupied_spheres_mathematical_swept_coverage() -> None: + """Continuous swept volume coverage: points outside uninflated spheres are covered by R_eff.""" + # Linear motion from [0, 0, 0] to [4, 0, 0] over 2 seconds (v=2 m/s), R=1.0 + obs = DynamicObstacleSphere3D( + center_initial=np.array([0.0, 0.0, 0.0]), + radius=1.0, + velocity=np.array([2.0, 0.0, 0.0]), + bounds=(100.0, 100.0, 50.0), + ) + # 2 spheres (N=2, t=0 and t=2) + spheres = obs.predict_occupied_spheres(t_start=0.0, horizon_sec=2.0, max_spheres=2) + assert len(spheres) == 2 + c0 = spheres[0].center + c1 = spheres[1].center + r_eff = spheres[0].radius + + # At midpoint t=1.0, obstacle center is at [2.0, 0.0, 0.0]. + # Point p = [2.0, 0.9, 0.0] is inside the true obstacle at t=1.0 (dist=0.9 <= 1.0). + test_pt = np.array([2.0, 0.9, 0.0]) + dist_to_c0 = float(np.linalg.norm(test_pt - c0)) + dist_to_c1 = float(np.linalg.norm(test_pt - c1)) + # Outside uninflated spheres (radius 1.0) + assert dist_to_c0 > 1.0 + assert dist_to_c1 > 1.0 + # Inside inflated R_eff sphere + assert dist_to_c0 <= r_eff or dist_to_c1 <= r_eff + + +def test_predict_occupied_spheres_one_sphere_and_clamped_caps() -> None: + """Hard count caps and 1-sphere conservative envelope.""" + obs = DynamicObstacleSphere3D( + center_initial=np.array([10.0, 10.0, 10.0]), + radius=1.5, + velocity=np.array([3.0, 0.0, 0.0]), + bounds=(50.0, 50.0, 50.0), + ) + # max_spheres = 1 + s1 = obs.predict_occupied_spheres(t_start=0.0, horizon_sec=3.0, max_spheres=1) + assert len(s1) == 1 + # Center should be at midpoint t=1.5 + np.testing.assert_allclose(s1[0].center, obs.center_at(1.5), atol=1e-5) + # Trajectory endpoints must be inside s1 + assert ( + float(np.linalg.norm(obs.center_at(0.0) - s1[0].center)) + obs.radius <= s1[0].radius + 1e-6 + ) + assert ( + float(np.linalg.norm(obs.center_at(3.0) - s1[0].center)) + obs.radius <= s1[0].radius + 1e-6 + ) + + # Clamped hard caps: max_spheres in [2, 5, 10] (needed is 18) + for cap in [2, 5, 10]: + sc = obs.predict_occupied_spheres(t_start=0.0, horizon_sec=10.0, max_spheres=cap) + assert len(sc) == cap + + # Unclamped when max_spheres exceeds needed count + sc_unclamped = obs.predict_occupied_spheres(t_start=0.0, horizon_sec=10.0, max_spheres=50) + assert len(sc_unclamped) <= 50 + assert len(sc_unclamped) == 18 + + +def test_predict_occupied_spheres_randomized_oracle_coverage() -> None: + """Randomized oracle: every point in the continuous swept obstacle volume is covered.""" + # Test across linear bounce and oscillation + obs_bounce = DynamicObstacleSphere3D( + center_initial=np.array([5.0, 5.0, 5.0]), + radius=0.8, + velocity=np.array([4.0, -3.0, 2.0]), + bounds=(12.0, 12.0, 12.0), + motion_type="bounce", + ) + obs_osc = DynamicObstacleSphere3D( + center_initial=np.array([15.0, 15.0, 8.0]), + radius=1.0, + velocity=np.zeros(3), + motion_type="oscillate", + oscillation_period=2.0, + oscillation_amplitude=np.array([5.0, 0.0, 3.0]), + bounds=(30.0, 30.0, 15.0), + ) + + rng = np.random.default_rng(12345) + for obs in [obs_bounce, obs_osc]: + for max_sph in [5, 25, 100]: + spheres = obs.predict_occupied_spheres( + t_start=0.5, horizon_sec=2.5, max_spheres=max_sph + ) + # Sample 50 random timestamps and 5 points per timestamp + for _ in range(50): + t_rand = float(rng.uniform(0.5, 3.0)) + c_rand = obs.center_at(t_rand) + # Random points within ball B(c_rand, obs.radius) + dir_vec = rng.normal(size=(5, 3)) + dir_vec /= np.linalg.norm(dir_vec, axis=1, keepdims=True) + r_sample = rng.uniform(0.0, obs.radius, size=(5, 1)) + pts = c_rand + dir_vec * r_sample + + for pt in pts: + # Point must be inside at least one predicted sphere + covered = any( + float(np.linalg.norm(pt - s.center)) <= s.radius + 1e-5 for s in spheres + ) + assert covered, ( + f"Point {pt} at t={t_rand} was NOT covered by any of {len(spheres)} spheres!" + ) + + +def test_predict_occupied_spheres_dt_strict_validation() -> None: + """predict_occupied_spheres strictly rejects invalid dt types (None, bool, non-finite, <=0).""" + import pytest + + obs = DynamicObstacleSphere3D( + center_initial=np.array([5.0, 5.0, 5.0]), + radius=1.0, + velocity=np.array([1.0, 0.0, 0.0]), + ) + for bad_dt in [None, True, False, 0.0, -0.5, float("inf"), float("nan"), "0.5"]: + with pytest.raises(ValueError, match="dt must be a strictly positive finite float"): + obs.predict_occupied_spheres(t_start=0.0, horizon_sec=1.0, dt=bad_dt) # type: ignore[arg-type] + + +def test_drone_dynamics_max_translational_acceleration() -> None: + """DroneDynamics6DOF calculates analytical, defensible translational acceleration bound.""" + from adaptive_rl.environments.drone_6dof import DroneDynamics6DOF + + # 1. Base dynamics + dyn_base = DroneDynamics6DOF( + mass=1.0, + gravity=9.81, + max_thrust_per_motor=5.0, + linear_damping=0.1, + enable_ground_effect=False, + ) + a_base = dyn_base.max_translational_acceleration(max_speed_bound=25.0) + # T_max/m + g + D*v/m = 20.0 + 9.81 + 0.1*25.0 = 32.31 + assert abs(a_base - (20.0 + 9.81 + 2.5)) < 1e-4 + + # 2. Ground effect increases acceleration bound + dyn_ige = DroneDynamics6DOF( + mass=1.0, + gravity=9.81, + max_thrust_per_motor=5.0, + linear_damping=0.1, + enable_ground_effect=True, + ) + a_ige = dyn_ige.max_translational_acceleration(max_speed_bound=25.0) + assert a_ige > a_base + + # 3. Wind vector increases airspeed and drag contribution + dyn_wind = DroneDynamics6DOF( + mass=1.0, + gravity=9.81, + max_thrust_per_motor=5.0, + linear_damping=0.1, + wind_vector=np.array([10.0, 0.0, 0.0]), + ) + a_wind = dyn_wind.max_translational_acceleration(max_speed_bound=25.0) + assert a_wind > a_base + + +def test_drone_curved_path_static_obstacle_and_boundary_collision() -> None: + """Curved 6-DOF trajectory detects static obstacle and boundary breaches when chords are clear.""" + import pytest + + from adaptive_rl.environments.drone_6dof import Drone6DOFEnv, ObstacleSphere3D + + # Environment with obstacle at [5.0, 6.0, 5.0], radius 1.0, drone radius 0.3 + obs = ObstacleSphere3D(center=np.array([5.0, 6.0, 5.0]), radius=1.0) + env = Drone6DOFEnv( + bounds=(10.0, 10.0, 10.0), + collision_radius=0.3, + num_obstacles=0, + ) + env._obstacles = [obs] + + # Chord from [2.0, 4.0, 5.0] to [8.0, 4.0, 5.0]: y=4.0 everywhere. + # Closest point on chord to [5.0, 6.0, 5.0] is [5.0, 4.0, 5.0], distance = 2.0. + # Obstacle threshold = 1.0 + 0.3 = 1.3 < 2.0. Chord is clear! + p_start = np.array([2.0, 4.0, 5.0]) + p_end = np.array([8.0, 4.0, 5.0]) + + coll_chord, _ = env._check_collision(p_end, prev_pos=p_start) + assert coll_chord is False, "Straight chord must be clear of obstacle!" + + # Curved intermediate substep trajectory bulging to y=5.0 (dist=1.0 <= 1.3) + curved_pts = [ + p_start, + np.array([4.0, 4.5, 5.0]), + np.array([5.0, 5.0, 5.0]), # Intrusive intermediate point + np.array([6.0, 4.5, 5.0]), + p_end, + ] + coll_curved, coll_type = env._check_collision( + p_end, prev_pos=p_start, intermediate_positions=curved_pts + ) + assert coll_curved is True + assert coll_type == "obstacle" + + # Curved intermediate boundary breach: y dips below collision_radius + boundary_curved_pts = [ + np.array([5.0, 2.0, 5.0]), + np.array([5.0, 0.1, 5.0]), # Breaches y=0 bound (0.1 - 0.3 = -0.2 <= 0) + np.array([5.0, 2.0, 5.0]), + ] + coll_bnd, bnd_type = env._check_collision( + np.array([5.0, 2.0, 5.0]), + prev_pos=np.array([5.0, 2.0, 5.0]), + intermediate_positions=boundary_curved_pts, + ) + assert coll_bnd is True + assert bnd_type == "boundary_y" + + # Malformed intermediate positions fail closed + with pytest.raises( + ValueError, match="intermediate_positions must contain at least 2 positions" + ): + env._check_collision(p_end, prev_pos=p_start, intermediate_positions=[p_start]) + + with pytest.raises(ValueError, match="does not match prev_pos"): + bad_start = [np.array([0.0, 0.0, 0.0]), p_end] + env._check_collision(p_end, prev_pos=p_start, intermediate_positions=bad_start) diff --git a/tests/test_recorder_fingerprint_phase3.py b/tests/test_recorder_fingerprint_phase3.py index 3fd76b8..d2c99fe 100644 --- a/tests/test_recorder_fingerprint_phase3.py +++ b/tests/test_recorder_fingerprint_phase3.py @@ -819,3 +819,168 @@ def test_explicit_configuration_fully_equivalent_preserved(tmp_path: Path) -> No assert record.configuration.provenance_complete is True assert record.verify_config_fingerprint() is True assert record.verify_record_digest() is True + + +def test_provenance_captures_module_helper_changes() -> None: + """extract_callable_provenance fingerprints module-qualified helper functions referenced in co_names.""" + import types + + from adaptive_rl.flight_testing.scenarios import extract_callable_provenance + + mod1 = types.ModuleType("dummy_helper_mod") + mod1.transform = lambda x: x * 2 # type: ignore[attr-defined] + + mod2 = types.ModuleType("dummy_helper_mod") + mod2.transform = lambda x: x * 999 # type: ignore[attr-defined] + + def caller_fn(val: float) -> float: + return dummy_mod.transform(val) # type: ignore[name-defined] # noqa: F821 + + # Caller with mod1 + caller_fn.__globals__["dummy_mod"] = mod1 + prov1 = extract_callable_provenance(caller_fn) + + # Caller with mod2 + caller_fn.__globals__["dummy_mod"] = mod2 + prov2 = extract_callable_provenance(caller_fn) + + # Different module helper implementation MUST yield different referenced_globals provenance! + assert prov1["referenced_globals"] != prov2["referenced_globals"] + + +def test_provenance_captures_inherited_class_methods() -> None: + """Custom callable class instances capture MRO hierarchy and inherited methods.""" + from adaptive_rl.flight_testing.scenarios import extract_callable_provenance + + class BaseA: + def compute(self, x: float) -> float: + return x + 1.0 + + class DerivedA(BaseA): + def __call__(self, x: float) -> float: + return self.compute(x) + + class BaseB: + def compute(self, x: float) -> float: + return x + 100.0 + + class DerivedB(BaseB): + def __call__(self, x: float) -> float: + return self.compute(x) + + inst_a = DerivedA() + inst_b = DerivedB() + + prov_a = extract_callable_provenance(inst_a) + prov_b = extract_callable_provenance(inst_b) + + assert prov_a["callable_class"] != prov_b["callable_class"] + assert "class_methods" in prov_a + assert "class_methods" in prov_b + assert prov_a["class_methods"] != prov_b["class_methods"] + + +def test_explicit_configuration_duplicate_scenario_names_1to1_matching(tmp_path: Path) -> None: + """1-to-1 canonical matching correctly handles duplicate scenario names without collision.""" + from adaptive_rl.flight_testing.scenarios import ( + FlightScenarioDefinition, + FlightScenarioRunner, + ) + + runner = FlightScenarioRunner() + sc1 = FlightScenarioDefinition( + name="repeat_scenario", + description="trial 1", + initial_position=np.array([1.0, 1.0, 1.0]), + max_steps=5, + ) + sc2 = FlightScenarioDefinition( + name="repeat_scenario", + description="trial 2 with different initial position", + initial_position=np.array([4.0, 4.0, 2.0]), + max_steps=5, + ) + res1 = runner.run_scenario(sc1) + res2 = runner.run_scenario(sc2) + + physics = PhysicsSimulationFingerprint( + mass=1.0, + gravity=9.81, + arm_length=0.2, + arm_span=0.4, + rotor_offset_d=0.1414, + rotor_radius=0.09, + max_thrust_per_motor=5.0, + torque_to_thrust_ratio=0.01, + linear_damping=0.1, + angular_damping=0.02, + inertia=[0.01, 0.01, 0.02], + dt=0.05, + substeps=5, + substep_dt=0.01, + body_frame_lidar=False, + num_lidar_rays=16, + lidar_range=20.0, + ) + + # 1. Matching 1-to-1 duplicate configuration + cfg_matching = ExperimentConfigurationFingerprint( + physics=physics, + environment_bounds=[30.0, 30.0, 15.0], + algorithm_name="test", + checkpoint_hash=None, + random_seed=42, + scenario_names=["repeat_scenario", "repeat_scenario"], + scenarios=[sc1.to_dict(), sc2.to_dict()], + provenance_complete=True, + ) + + recorder = FlightRecorder(output_dir=tmp_path) + record_ok = recorder.create_record( + experiment_id="dup_sc_match", + physics_params={"mass": 1.0, "gravity": 9.81}, + scenario_results=[res1, res2], + configuration=cfg_matching, + ) + assert record_ok.provenance_complete is True + assert record_ok.configuration.provenance_complete is True + + # 2. Missing one of the duplicate runs (length mismatch) + cfg_single = ExperimentConfigurationFingerprint( + physics=physics, + environment_bounds=[30.0, 30.0, 15.0], + algorithm_name="test", + checkpoint_hash=None, + random_seed=42, + scenario_names=["repeat_scenario"], + scenarios=[sc1.to_dict()], + provenance_complete=True, + ) + record_missing = recorder.create_record( + experiment_id="dup_sc_missing", + physics_params={"mass": 1.0, "gravity": 9.81}, + scenario_results=[res1, res2], + configuration=cfg_single, + ) + assert record_missing.provenance_complete is False + assert record_missing.configuration.provenance_complete is False + + # 3. Differing duplicate scenario configuration (e.g. sc1 and sc1 instead of sc1 and sc2) + cfg_mismatch = ExperimentConfigurationFingerprint( + physics=physics, + environment_bounds=[30.0, 30.0, 15.0], + algorithm_name="test", + checkpoint_hash=None, + random_seed=42, + scenario_names=["repeat_scenario", "repeat_scenario"], + scenarios=[sc1.to_dict(), sc1.to_dict()], # Second sc2 replaced by duplicate of sc1! + provenance_complete=True, + ) + record_mismatch = recorder.create_record( + experiment_id="dup_sc_mismatch", + physics_params={"mass": 1.0, "gravity": 9.81}, + scenario_results=[res1, res2], + configuration=cfg_mismatch, + ) + assert record_mismatch.provenance_complete is False + assert record_mismatch.configuration.provenance_complete is False From f703f66cf32eaddcd9774e066cc87b4ad3605a33 Mon Sep 17 00:00:00 2001 From: Aryan Saxena Date: Sun, 15 Nov 2026 23:39:36 +0530 Subject: [PATCH 7/7] fix(phase3): adversarial dynamic collision validation, bounce splitting, defensible acceleration, and recursive descriptor provenance --- docs/AUTONOMOUS_NAVIGATION.md | 33 ++- src/adaptive_rl/environments/drone_6dof.py | 104 +++++++- src/adaptive_rl/flight_testing/scenarios.py | 121 +++++++--- .../navigation/dynamic_obstacles.py | 198 ++++++++++++++-- tests/test_navigation_dynamic_obstacles.py | 222 ++++++++++++++++++ tests/test_recorder_fingerprint_phase3.py | 200 ++++++++++++++++ 6 files changed, 809 insertions(+), 69 deletions(-) diff --git a/docs/AUTONOMOUS_NAVIGATION.md b/docs/AUTONOMOUS_NAVIGATION.md index c62ecbf..517bb53 100644 --- a/docs/AUTONOMOUS_NAVIGATION.md +++ b/docs/AUTONOMOUS_NAVIGATION.md @@ -69,7 +69,7 @@ The navigation system lives in `adaptive_rl.navigation` and cleanly separates ge +-----------------------+ ``` -### 2.1 Provably Collision-Free 3D A* Path Planning (`AStarPlanner3D`) +### 2.1 3D A* Path Planning (`AStarPlanner3D`) - **Spatial Grid Representation:** Discretizes the 3D bounding box $[-10, 10] \times [-10, 10] \times [0, 15]\text{ m}$ into uniform voxels (default grid resolution $\Delta = 0.5\text{ m}$). - **Connectivity & Corner-Cutting Prevention:** 26-connected neighbor graph (orthogonal, planar diagonal, and spatial cubic diagonal transitions) with true Euclidean edge costs: $$c(u, v) = \|p_u - p_v\|_2$$ @@ -79,11 +79,22 @@ The navigation system lives in `adaptive_rl.navigation` and cleanly separates ge - **Obstacle Inflation:** Every spherical obstacle with radius $r_{\text{obs}}$ is inflated by: $$r_{\text{effective}} = r_{\text{obs}} + r_{\text{vehicle}} + \delta_{\text{margin}}$$ where nominal $r_{\text{vehicle}} = 0.25\text{ m}$ and configurable safety margin $\delta_{\text{margin}} = 0.20\text{ m}$. Any voxel or segment within $r_{\text{effective}}$ of an obstacle center is impassable. -- **Provably Collision-Free Line-of-Sight (LoS) Simplification:** The raw grid path undergoes deterministic ray-cast shortcutting (`simplify_path`). Every prospective segment connecting non-adjacent waypoints is verified with continuous collision checks. If an unvalidated shortcut occurs, the simplification fallback guarantees that no unvalidated or intersecting segment is ever appended to the path. If no safe continuation exists, the planner provably reports `NO_PATH` rather than returning an unsafe path. - -### 2.2 Dynamic Obstacles & Receding-Horizon Avoidance (`DynamicObstacleSphere3D`) -- **Kinematic Horizon Prediction:** Dynamic obstacles propagate according to linear or sinusoidal velocity models. `predict_trajectory(t_start, horizon_sec, dt)` and `predict_occupied_spheres(...)` forecast the swept volume of moving obstacles over a lookahead horizon $[t, t + H]$ (e.g., $H = 2.0\text{ s}$). -- **Occupied Volume Inflation:** Spheres occupied across $[t, t + H]$ are merged with the static obstacle field during predictive replanning. +- **Line-of-Sight (LoS) Simplification:** The raw grid path undergoes deterministic ray-cast shortcutting (`simplify_path`). Every prospective segment connecting non-adjacent waypoints is verified with continuous collision checks. If an unvalidated shortcut occurs, the simplification fallback guarantees that no unvalidated or intersecting segment is ever appended to the path. If no safe continuation exists, the planner safely reports `NO_PATH` rather than returning an unsafe path. + *Note on Guarantees:* Path collision safety is guaranteed conditional on grid resolution $\Delta$, the safety inflation margin $\delta_{\text{margin}}$, valid bounded vehicle tracking error, and static obstacle environments. + +### 2.2 Dynamic Obstacles & Continuous Swept Collision Detection (`DynamicObstacleSphere3D`) +- **Kinematic Horizon Prediction:** Dynamic obstacles propagate according to linear, oscillating, or bouncing velocity models. `predict_occupied_spheres(...)` forecasts the swept volume of moving obstacles over a lookahead horizon $[t, t + H]$ (e.g., $H = 2.0\text{ s}$) using analytical upper bounds on displacement ($v_{\max} H$) and spacing $\le 1.2 R$ to guarantee zero corridor gaps. +- **Continuous Swept Collision with Boundary Reflection Decomposition:** + Dynamic obstacles reflecting off arena boundaries follow piecewise linear trajectories with sharp directional changes. Standard linear chord interpolation between $[t_0, t_1]$ cuts across the arena interior, missing boundary reflections and leading to false negatives. + - `get_bounce_times(t0, t1)` analytically computes all exact reflection timestamps $\tau_{\text{bounce}} \in (t_0, t_1)$ where the obstacle reflects from arena boundaries. + - The interval is partitioned into exact linear pieces $[t_a, t_b]$ at reflection events, checking swept relative clearance on each piece with interpolated vehicle endpoints. +- **Rigorous Trajectory Curvature Envelopes:** + - **Vehicle Curvature Deviation:** When 6-DOF intermediate RK4 substeps are provided, the local curvature deviation bound is $\varepsilon_{\text{veh}} = \frac{1}{8} a_{\max} (\Delta \tau)^2$ across each substep. When only step endpoints are provided, the bound defensively evaluates against the full step duration: $\varepsilon_{\text{veh}} = \frac{1}{8} a_{\max} (\Delta t)^2$. + - **Oscillating Obstacle Curvature:** For harmonic oscillating obstacles with period $T$ and amplitude $A$, maximum acceleration is $\omega^2 \|A\|_2$ (where $\omega = 2\pi / T$), yielding curvature deviation $\varepsilon_{\text{obs}} = \frac{1}{8} \tau_{\text{step}}^2 \omega^2 \|A\|_2$. +- **Defensible Acceleration Envelope (`max_translational_acceleration`):** + Analytical maximum translational acceleration is derived directly from the physical parameters of `DroneDynamics6DOF`: + $$a_{\max}(v) = \frac{4 f_{\max}}{m} + g + \frac{D_v v + D_q v^2}{m}$$ + where $v$ is bounded by the operational envelope: $\max(v_{\text{nominal}}, v_{\text{state}}, v_{\text{substeps}})$, or derived from terminal velocity $v_{\text{term}}$ under full motor thrust and gravity when no bound is specified. Arbitrary empirical fallbacks are strictly prohibited; calculations fail closed if physical parameters are invalid. ### 2.3 Mission Manager (`MissionManager`) - Coordinates multi-waypoint missions, predictive dynamic obstacle avoidance, and fail-safe recovery. @@ -247,6 +258,16 @@ In any strapdown inertial navigation system without external aiding: Parameters can be sampled reproducibly from explicit random seeds and serialized/reloaded via canonical JSON configuration files. +### 6.1 Cryptographic Experiment Configuration Fingerprinting & Deep Provenance +To guarantee exact reproducibility and provenance across arbitrary user callbacks, controllers, and fault injectors, `adaptive_rl.flight_testing` implements multi-layer cryptographic digest tracking: +- **Physics Simulation Fingerprint (`PhysicsSimulationFingerprint`):** Full parameter coverage of mass, gravity, arm geometry, rotor dynamics, damping, inertia, and simulation timesteps. +- **Deep Callable Provenance (`extract_callable_provenance`):** + - **Descriptor Inspection:** Unwraps and fingerprints `staticmethod` (`.__func__`), `classmethod` (`.__func__`), and `property` (`.fget`, `.fset`, `.fdel`) descriptors across the entire class MRO hierarchy. + - **Recursive Module Dependency Resolution:** Recursively resolves submodules and nested attribute chains (`pkg.submod.helper`) referenced in bytecode `co_names`. Changes to underlying helper functions in imported submodules alter the provenance digest deterministically. + - **Safe Recursion and Cycle Handling:** Detects circular call graphs and self-recursive functions, breaking cycles via deterministic lexical cycle references (`recursion_ref`) without stack overflow. + - **Fails-Closed Incomplete Tracking:** Dynamic or non-serializable objects (e.g. opaque native handles, threading locks) are safely flagged as `provenance_complete = False` with structured error taxonomy, preventing non-deterministic memory address leakage into cryptographic digests. +- **Dual Integrity Verification:** `FlightRecord` computes two separate digests: `config_fingerprint` (covering experiment preconditions) and `record_digest` (covering post-execution metrics, trajectories, and outcomes). Tampering with either pre-flight configuration or post-flight telemetry is strictly detected. + --- ## 7. Autonomous Scenario Suite (18 Scenarios) diff --git a/src/adaptive_rl/environments/drone_6dof.py b/src/adaptive_rl/environments/drone_6dof.py index e9f3658..818334a 100644 --- a/src/adaptive_rl/environments/drone_6dof.py +++ b/src/adaptive_rl/environments/drone_6dof.py @@ -393,6 +393,7 @@ def __init__( angular_velocity=np.zeros(3, dtype=np.float64), ) self.last_substep_positions: List[np.ndarray] = [self.state.position.copy()] + self.last_substep_velocities: List[np.ndarray] = [self.state.velocity.copy()] @property def arm_span(self) -> float: @@ -482,19 +483,43 @@ def reset( self.actual_motor_thrusts = np.full(4, self.hover_thrust_per_motor, dtype=np.float64) self.last_substep_positions = [self.state.position.copy()] + self.last_substep_velocities = [self.state.velocity.copy()] - def max_translational_acceleration(self, max_speed_bound: float = 25.0) -> float: - """Derive an analytical upper bound on translational acceleration ||p_ddot|| in m/s^2. + def max_translational_acceleration(self, max_speed_bound: Optional[float] = None) -> float: + """Derive an analytical, physically defensible upper bound on translational acceleration ||p_ddot|| in m/s^2. From the translational equations of motion: p_ddot = (1/m)*R(q)*[0, 0, T] + [0, 0, -g] + F_drag / m By triangle inequality: ||p_ddot|| <= (T_max / m) + g + (||F_drag||_max / m) - T_max incorporates Cheeseman-Bennett ground effect amplification (up to 1.30) - when enabled. F_drag considers linear damping and quadratic drag over relative - airspeed up to max_speed_bound + ||wind_vector||. + Validates all physical parameters, rejecting NaN, Inf, and invalid bounds. + Accounts for the actual vehicle operating speed envelope: if max_speed_bound is supplied, + it verifies whether the vehicle's state or recent substeps exceed the nominal bound, + always taking the conservative envelope max(max_speed_bound, actual_speed). + If max_speed_bound is omitted (None), derives the analytical terminal velocity under + continuous maximum diving thrust and gravity. """ + # Validate internal physical parameters + if not math.isfinite(self.mass) or self.mass <= 0.0: + raise ValueError(f"Dynamics mass must be positive finite, got {self.mass}") + if not math.isfinite(self.gravity) or self.gravity < 0.0: + raise ValueError(f"Dynamics gravity must be non-negative finite, got {self.gravity}") + if not math.isfinite(self.max_thrust_per_motor) or self.max_thrust_per_motor <= 0.0: + raise ValueError( + f"Dynamics max_thrust_per_motor must be positive finite, got {self.max_thrust_per_motor}" + ) + if not math.isfinite(self.linear_damping) or self.linear_damping < 0.0: + raise ValueError( + f"Dynamics linear_damping must be non-negative finite, got {self.linear_damping}" + ) + if not math.isfinite(self.quadratic_drag_coeff) or self.quadratic_drag_coeff < 0.0: + raise ValueError( + f"Dynamics quadratic_drag_coeff must be non-negative finite, got {self.quadratic_drag_coeff}" + ) + if not np.all(np.isfinite(self.wind_vector)): + raise ValueError(f"Dynamics wind_vector contains non-finite values: {self.wind_vector}") + # Maximum collective thrust across all 4 motors t_max_nominal = 4.0 * self.max_thrust_per_motor if self.enable_ground_effect: @@ -502,9 +527,42 @@ def max_translational_acceleration(self, max_speed_bound: float = 25.0) -> float else: t_max = t_max_nominal - # Airspeed bound + # Actual speeds observed in current physical state and recent substeps + v_state = float(np.linalg.norm(self.state.velocity)) + v_substep = max( + (float(np.linalg.norm(v)) for v in getattr(self, "last_substep_velocities", [])), + default=0.0, + ) + v_actual_max = max(v_state, v_substep) + + if max_speed_bound is not None: + if ( + isinstance(max_speed_bound, bool) + or not isinstance(max_speed_bound, (int, float)) + or not math.isfinite(max_speed_bound) + or max_speed_bound < 0.0 + ): + raise ValueError( + f"max_speed_bound must be a non-negative finite float, got {max_speed_bound!r}" + ) + v_envelope = max(float(max_speed_bound), v_actual_max) + else: + # Analytical terminal velocity under continuous full thrust + gravity in flight direction + f_drive = t_max + self.mass * self.gravity + if self.quadratic_drag_coeff > 0.0: + disc = (self.linear_damping**2) + 4.0 * self.quadratic_drag_coeff * f_drive + v_term = (-self.linear_damping + math.sqrt(disc)) / ( + 2.0 * self.quadratic_drag_coeff + ) + elif self.linear_damping > 0.0: + v_term = f_drive / self.linear_damping + else: + v_term = 0.0 + v_envelope = max(v_term, v_actual_max) + + # Airspeed bound incorporating atmospheric wind vector wind_speed = float(np.linalg.norm(self.wind_vector)) - v_rel_max = max(0.0, float(max_speed_bound)) + wind_speed + v_rel_max = v_envelope + wind_speed # Drag force upper bound drag_force_max = self.linear_damping * v_rel_max @@ -601,6 +659,7 @@ def step_rk4(self, motor_thrusts: np.ndarray) -> DroneState6DOF: omega = self.state.angular_velocity.copy() substep_positions = [pos.copy()] + substep_velocities = [vel.copy()] for _ in range(self.substeps): f_0 = self.actual_motor_thrusts.copy() @@ -672,8 +731,10 @@ def step_rk4(self, motor_thrusts: np.ndarray) -> DroneState6DOF: omega += (h / 6.0) * (k1_w + 2.0 * k2_w + 2.0 * k3_w + k4_w) substep_positions.append(pos.copy()) + substep_velocities.append(vel.copy()) self.last_substep_positions = substep_positions + self.last_substep_velocities = substep_velocities self.state.position = pos self.state.velocity = vel self.state.quaternion = quat @@ -1051,11 +1112,32 @@ def _check_collision( pos: np.ndarray, prev_pos: Optional[np.ndarray] = None, intermediate_positions: Optional[Sequence[np.ndarray]] = None, - curvature_error_bound: float = 0.0, + curvature_error_bound: Optional[float] = None, ) -> Tuple[bool, str]: r = float(self.collision_radius) bx, by, bz = self.bounds - eps_curv = max(0.0, float(curvature_error_bound)) + + if curvature_error_bound is not None: + if ( + isinstance(curvature_error_bound, bool) + or not isinstance(curvature_error_bound, (int, float)) + or not math.isfinite(curvature_error_bound) + or curvature_error_bound < 0.0 + ): + raise ValueError( + f"curvature_error_bound must be a non-negative finite float, got {curvature_error_bound!r}" + ) + eps_curv = float(curvature_error_bound) + elif prev_pos is not None: + # Derive physically defensible curvature bound from dynamics + a_max = self.dynamics.max_translational_acceleration(self.max_velocity) + if intermediate_positions is not None: + dt_step = self.dynamics.substep_dt + else: + dt_step = self.dynamics.dt + eps_curv = 0.125 * a_max * (dt_step**2) + else: + eps_curv = 0.0 if prev_pos is None: if pos[0] - r <= 0.0 or pos[0] + r >= bx: @@ -1356,16 +1438,12 @@ def step( dist_delta = self._prev_distance_to_goal - curr_distance self._prev_distance_to_goal = curr_distance - a_max = self.dynamics.max_translational_acceleration(self.max_velocity) - substep_h = self.dynamics.substep_dt - eps_curv = 0.125 * a_max * (substep_h**2) inter_pos = self.dynamics.last_substep_positions is_collision, collision_type = self._check_collision( self._position, prev_pos=old_pos, intermediate_positions=inter_pos, - curvature_error_bound=eps_curv, ) is_success = curr_distance <= self.target_radius diff --git a/src/adaptive_rl/flight_testing/scenarios.py b/src/adaptive_rl/flight_testing/scenarios.py index f5777e4..54c5a20 100644 --- a/src/adaptive_rl/flight_testing/scenarios.py +++ b/src/adaptive_rl/flight_testing/scenarios.py @@ -47,7 +47,7 @@ import time from dataclasses import asdict, dataclass, field from pathlib import Path -from typing import Any, Callable, Dict, List, Optional, Tuple +from typing import Any, Callable, Dict, List, Optional, Sequence, Tuple import numpy as np @@ -326,6 +326,22 @@ def _safe_provenance_value(val: Any, _visited: Optional[set[int]] = None) -> Tup meta = getattr(val, "metadata") if meta is not None: return _safe_provenance_value(meta, _visited=_visited) + if isinstance(val, (staticmethod, classmethod)): + fn_val = getattr(val, "__func__", None) + fn_prov, ok = _safe_provenance_value(fn_val, _visited=_visited) + return {"descriptor": type(val).__name__, "func": fn_prov}, ok + + if isinstance(val, property): + prop_info: Dict[str, Any] = {"descriptor": "property"} + all_ok = True + for part_name, fn_part in [("fget", val.fget), ("fset", val.fset), ("fdel", val.fdel)]: + if fn_part is not None: + p_prov, p_ok = _safe_provenance_value(fn_part, _visited=_visited) + prop_info[part_name] = p_prov + if not p_ok: + all_ok = False + return prop_info, all_ok + if inspect.ismodule(val): return f"module:{val.__name__}", True @@ -394,9 +410,18 @@ def _safe_provenance_value(val: Any, _visited: Optional[set[int]] = None) -> Tup s = str(val) if re.search(r"\bat 0x[0-9a-fA-F]+\b", s) or re.search(r"\b0x[0-9a-fA-F]{6,16}\b", s): - cleaned = re.sub(r"\bat 0x[0-9a-fA-F]+\b", "", s).strip() - return cleaned, False - return s, False + cleaned = re.sub(r"\bat 0x[0-9a-fA-F]+\b", "", s) + cleaned = re.sub(r"\b0x[0-9a-fA-F]{6,16}\b", "", cleaned).strip() + return { + "unsupported_dependency": type(val).__name__, + "reason": "non_deterministic_address_or_unsupported_object", + "repr": cleaned, + }, False + return { + "unsupported_dependency": type(val).__name__, + "reason": "unserializable_opaque_value", + "repr": s, + }, False def _hash_code_recursive(code_obj: Any, hasher: Any) -> None: @@ -413,6 +438,50 @@ def _hash_code_recursive(code_obj: Any, hasher: Any) -> None: hasher.update(str(const).encode("utf-8", errors="replace")) +def _resolve_module_dependencies( + mod: Any, + co_names: Sequence[str], + visited_modules: set[int], + _visited: set[int], +) -> Tuple[Dict[str, Any], bool]: + """Recursively resolve module and submodule attributes referenced in co_names.""" + if id(mod) in visited_modules: + return {"recursion_ref": getattr(mod, "__name__", str(mod))}, True + visited_modules.add(id(mod)) + + builtin_names = set(dir(builtins)) + attrs: Dict[str, Any] = {} + all_ok = True + + for attr_name in sorted(set(co_names)): + if hasattr(mod, attr_name) and attr_name not in builtin_names: + try: + attr_obj = getattr(mod, attr_name) + if inspect.ismodule(attr_obj): + sub_attrs, sub_ok = _resolve_module_dependencies( + attr_obj, co_names, set(visited_modules), _visited + ) + attrs[attr_name] = { + "module": getattr(attr_obj, "__name__", attr_name), + "attributes": sub_attrs, + } + if not sub_ok: + all_ok = False + else: + ser_attr, ok_attr = _safe_provenance_value(attr_obj, _visited=_visited) + attrs[attr_name] = ser_attr + if not ok_attr: + all_ok = False + except Exception as exc: + attrs[attr_name] = { + "unresolved_attribute": attr_name, + "error": f"{type(exc).__name__}: {exc}", + } + all_ok = False + + return attrs, all_ok + + def extract_callable_provenance(fn: Any, _visited: Optional[set[int]] = None) -> Dict[str, Any]: """Extract a stable, deterministic provenance record for a callable. @@ -487,8 +556,12 @@ def extract_callable_provenance(fn: Any, _visited: Optional[set[int]] = None) -> if base is object or getattr(base, "__module__", "") == "builtins": continue for k, v in base.__dict__.items(): - if not k.startswith("__") or k == "__call__": - if inspect.isfunction(v) or inspect.ismethod(v): + if not k.startswith("__") or k in ("__call__", "__init__"): + if ( + inspect.isfunction(v) + or inspect.ismethod(v) + or isinstance(v, (staticmethod, classmethod, property)) + ): m_prov, m_ok = _safe_provenance_value(v, _visited=set(_visited)) inherited_methods[k] = m_prov if not m_ok: @@ -567,26 +640,14 @@ def extract_callable_provenance(fn: Any, _visited: Optional[set[int]] = None) -> if name in fn_globals and name not in builtin_names: g_val = fn_globals[name] if inspect.ismodule(g_val): - # Resolve module-qualified dependencies referenced in co_names - mod_attrs: Dict[str, Any] = {} - mod_all_ok = True - for attr_name in sorted(set(co_names)): - if hasattr(g_val, attr_name) and attr_name not in builtin_names: - try: - attr_obj = getattr(g_val, attr_name) - ser_attr, ok_attr = _safe_provenance_value( - attr_obj, _visited=set(_visited) - ) - mod_attrs[attr_name] = ser_attr - if not ok_attr: - mod_all_ok = False - except Exception: - mod_attrs[attr_name] = f"unresolvable:{attr_name}" - mod_all_ok = False + # Recursively resolve module-qualified dependencies referenced in co_names + mod_attrs, mod_ok = _resolve_module_dependencies( + g_val, co_names, set(), set(_visited) + ) if mod_attrs: ref_globals[f"{name}.attributes"] = mod_attrs ref_globals[name] = f"module:{g_val.__name__}" - if not mod_all_ok: + if not mod_ok: provenance_complete = False else: ser_g, ok_g = _safe_provenance_value(g_val, _visited=set(_visited)) @@ -1372,11 +1433,15 @@ def run_scenario( min_clearances.append(min_obs_d) # Collision check with analytical curvature error bound - a_max = ( - self.env.dynamics.max_translational_acceleration(25.0) - if hasattr(self.env.dynamics, "max_translational_acceleration") - else 35.0 - ) + if hasattr(self.env.dynamics, "max_translational_acceleration"): + a_max = self.env.dynamics.max_translational_acceleration( + max_speed_bound=getattr(scenario, "max_speed", 25.0) + ) + else: + raise ValueError( + "Safety certification failure: environment dynamics does not provide " + "a physically defensible max_translational_acceleration() calculation." + ) substep_h = getattr(self.env.dynamics, "substep_dt", dt) eps_curv = 0.125 * a_max * (substep_h**2) intermediate_positions = getattr(self.env.dynamics, "last_substep_positions", None) diff --git a/src/adaptive_rl/navigation/dynamic_obstacles.py b/src/adaptive_rl/navigation/dynamic_obstacles.py index 7c6eedc..b9efa5e 100644 --- a/src/adaptive_rl/navigation/dynamic_obstacles.py +++ b/src/adaptive_rl/navigation/dynamic_obstacles.py @@ -32,11 +32,16 @@ class DynamicObstacleSphere3D: ) def __post_init__(self) -> None: - self.radius = float(self.radius) - if not math.isfinite(self.radius) or self.radius <= 0.0: + if ( + isinstance(self.radius, bool) + or not isinstance(self.radius, (int, float)) + or not math.isfinite(self.radius) + or self.radius <= 0.0 + ): raise ValueError( - f"Obstacle radius must be strictly positive and finite, got {self.radius}" + f"Obstacle radius must be strictly positive and finite, got {self.radius!r}" ) + self.radius = float(self.radius) if self.motion_type not in ("bounce", "oscillate"): raise ValueError( @@ -97,6 +102,13 @@ def max_speed(self) -> float: def center_at(self, time_sec: float) -> np.ndarray: """Compute the deterministic 3D position of the obstacle at a given simulation time.""" + if ( + isinstance(time_sec, bool) + or not isinstance(time_sec, (int, float)) + or not math.isfinite(time_sec) + ): + raise ValueError(f"time_sec must be a finite float, got {time_sec!r}") + t = float(time_sec) if self.motion_type == "oscillate": omega = 2.0 * math.pi / self.oscillation_period @@ -120,23 +132,68 @@ def center_at(self, time_sec: float) -> np.ndarray: reflected[i] = b_max - (val - span) return reflected - def _check_subsegment_collision( + def get_bounce_times(self, t0: float, t1: float) -> List[float]: + """Identify all exact boundary reflection timestamps in (t0, t1) for bounce motion.""" + if self.motion_type != "bounce": + return [] + t0_f = float(t0) + t1_f = float(t1) + if t1_f <= t0_f: + return [] + + bounce_times: List[float] = [] + for i in range(3): + vi = float(self.velocity[i]) + if abs(vi) <= 1e-9: + continue + b_min = float(self.radius) + b_max = float(self.bounds[i] - self.radius) + span = b_max - b_min + if span <= 1e-9: + continue + + x0 = float(self.center_initial[i]) + vi * t0_f + x1 = float(self.center_initial[i]) + vi * t1_f + x_min = min(x0, x1) + x_max = max(x0, x1) + + k_start = int(math.ceil((x_min - b_min) / span)) + k_end = int(math.floor((x_max - b_min) / span)) + + for k in range(k_start, k_end + 1): + wall_pos = b_min + k * span + t_bounce = (wall_pos - float(self.center_initial[i])) / vi + if t0_f + 1e-7 < t_bounce < t1_f - 1e-7: + bounce_times.append(float(t_bounce)) + + if not bounce_times: + return [] + + bounce_times.sort() + unique_times: List[float] = [bounce_times[0]] + for t in bounce_times[1:]: + if t - unique_times[-1] > 1e-7: + unique_times.append(t) + + return unique_times + + def _check_subsegment_collision_linear( self, - p0: np.ndarray, - p1: np.ndarray, + p0_arr: np.ndarray, + p1_arr: np.ndarray, t0: float, t1: float, threshold: float, - veh_acc_max: float = 35.0, + veh_acc_max: float, + veh_interval_dt: Optional[float] = None, ) -> bool: - """Evaluate continuous swept-sphere collision across a single linear vehicle segment [t0, t1].""" - p0_arr = np.asarray(p0, dtype=np.float64) - p1_arr = np.asarray(p1, dtype=np.float64) + """Evaluate continuous collision across a single interval [t0, t1] where obstacle motion is non-reflecting.""" dt = t1 - t0 - if dt <= 1e-9: c = self.center_at(t0) - return bool(np.linalg.norm(p1_arr - c) <= threshold) + return bool( + np.linalg.norm(p0_arr - c) <= threshold or np.linalg.norm(p1_arr - c) <= threshold + ) # Immediate endpoint checks if np.linalg.norm(p0_arr - self.center_at(t0)) <= threshold: @@ -155,8 +212,8 @@ def _check_subsegment_collision( desired_substeps = max(1, int(math.ceil(dt / sub_dt))) num_substeps = min(max_substeps, desired_substeps) - # Theoretical curvature error bounds over subsegment tau_step: - # 1. Obstacle curvature bound: + # Theoretical curvature error bounds: + # 1. Obstacle curvature bound (over subsegment tau_step): tau_step = dt / num_substeps if self.motion_type == "oscillate": omega = 2.0 * math.pi / self.oscillation_period @@ -165,8 +222,11 @@ def _check_subsegment_collision( else: eps_obs = 0.0 - # 2. 6-DOF Vehicle curvature deviation bound from chord (||p''(t)|| <= veh_acc_max): - eps_veh = (1.0 / 8.0) * (tau_step**2) * veh_acc_max + # 2. Vehicle curvature bound: + # If genuine intermediate trajectory was provided, veh_interval_dt is the substep dt; + # if only endpoints were provided, veh_interval_dt is the full interval duration dt. + effective_veh_dt = dt if veh_interval_dt is None else float(veh_interval_dt) + eps_veh = (1.0 / 8.0) * (effective_veh_dt**2) * veh_acc_max effective_threshold = threshold + eps_obs + eps_veh @@ -197,8 +257,6 @@ def _check_subsegment_collision( if min_dist <= effective_threshold: return True - # When computational limits are reached (desired_substeps > max_substeps), - # fail conservatively if swept bounding volumes intersect. if desired_substeps > max_substeps: mid_drone = 0.5 * (p0_arr + p1_arr) mid_obs = 0.5 * (self.center_at(t0) + self.center_at(t1)) @@ -209,6 +267,55 @@ def _check_subsegment_collision( return False + def _check_subsegment_collision( + self, + p0: np.ndarray, + p1: np.ndarray, + t0: float, + t1: float, + threshold: float, + veh_acc_max: float = 35.0, + veh_interval_dt: Optional[float] = None, + ) -> bool: + """Evaluate continuous swept-sphere collision across [t0, t1], splitting at bounce events.""" + t0_f = float(t0) + t1_f = float(t1) + if t1_f < t0_f: + raise ValueError(f"t1 ({t1_f}) must be >= t0 ({t0_f})") + + p0_arr = np.asarray(p0, dtype=np.float64) + p1_arr = np.asarray(p1, dtype=np.float64) + dt = t1_f - t0_f + + if dt <= 1e-9: + c = self.center_at(t0_f) + return bool( + np.linalg.norm(p0_arr - c) <= threshold or np.linalg.norm(p1_arr - c) <= threshold + ) + + # Identify boundary reflections and split interval at reflection events + bounce_times = self.get_bounce_times(t0_f, t1_f) + if not bounce_times: + return self._check_subsegment_collision_linear( + p0_arr, p1_arr, t0_f, t1_f, threshold, veh_acc_max, veh_interval_dt + ) + + # Multiple subintervals split at exact bounce events + split_times = [t0_f] + bounce_times + [t1_f] + for m in range(len(split_times) - 1): + ta = split_times[m] + tb = split_times[m + 1] + fa = (ta - t0_f) / dt + fb = (tb - t0_f) / dt + pa = p0_arr + fa * (p1_arr - p0_arr) + pb = p0_arr + fb * (p1_arr - p0_arr) + if self._check_subsegment_collision_linear( + pa, pb, ta, tb, threshold, veh_acc_max, veh_interval_dt + ): + return True + + return False + def check_continuous_collision( self, prev_drone_pos: np.ndarray, @@ -226,9 +333,45 @@ def check_continuous_collision( trajectory curvature using physical acceleration limits (eps_veh <= 1/8 * a_max * dt^2). Fails closed on malformed or inconsistent intermediate trajectory data. """ - threshold = float(self.radius + vehicle_radius) + if ( + isinstance(prev_time, bool) + or not isinstance(prev_time, (int, float)) + or not math.isfinite(prev_time) + ): + raise ValueError(f"prev_time must be a finite float, got {prev_time!r}") + if ( + isinstance(curr_time, bool) + or not isinstance(curr_time, (int, float)) + or not math.isfinite(curr_time) + ): + raise ValueError(f"curr_time must be a finite float, got {curr_time!r}") + t0 = float(prev_time) t1 = float(curr_time) + if t1 < t0: + raise ValueError(f"curr_time ({t1}) must be >= prev_time ({t0})") + + if ( + isinstance(vehicle_radius, bool) + or not isinstance(vehicle_radius, (int, float)) + or not math.isfinite(vehicle_radius) + or vehicle_radius < 0.0 + ): + raise ValueError( + f"vehicle_radius must be a non-negative finite float, got {vehicle_radius!r}" + ) + + if ( + isinstance(veh_acc_max, bool) + or not isinstance(veh_acc_max, (int, float)) + or not math.isfinite(veh_acc_max) + or veh_acc_max < 0.0 + ): + raise ValueError( + f"veh_acc_max must be a non-negative finite float, got {veh_acc_max!r}" + ) + + threshold = float(self.radius + vehicle_radius) dt = t1 - t0 p_start_check = np.asarray(prev_drone_pos, dtype=np.float64) @@ -241,7 +384,10 @@ def check_continuous_collision( if dt <= 1e-9: c = self.center_at(t0) - return bool(np.linalg.norm(p_end_check - c) <= threshold) + return bool( + np.linalg.norm(p_start_check - c) <= threshold + or np.linalg.norm(p_end_check - c) <= threshold + ) if intermediate_drone_positions is not None: if not isinstance(intermediate_drone_positions, (list, tuple)): @@ -280,13 +426,20 @@ def check_continuous_collision( p_b = validated_pts[j + 1] t_a = t0 + (j / num_segs) * dt t_b = t0 + ((j + 1) / num_segs) * dt + sub_dt = t_b - t_a if self._check_subsegment_collision( - p_a, p_b, t_a, t_b, threshold, veh_acc_max=veh_acc_max + p_a, + p_b, + t_a, + t_b, + threshold, + veh_acc_max=veh_acc_max, + veh_interval_dt=sub_dt, ): return True return False - # If intermediate positions are not provided, evaluate the single chord with full-interval vehicle curvature bound + # If intermediate positions are not provided, evaluate chord with full-interval vehicle curvature bound return self._check_subsegment_collision( p_start_check, p_end_check, @@ -294,6 +447,7 @@ def check_continuous_collision( t1, threshold, veh_acc_max=veh_acc_max, + veh_interval_dt=dt, ) def distance_to(self, point: np.ndarray, time_sec: float) -> float: diff --git a/tests/test_navigation_dynamic_obstacles.py b/tests/test_navigation_dynamic_obstacles.py index 9fdb76c..322dc84 100644 --- a/tests/test_navigation_dynamic_obstacles.py +++ b/tests/test_navigation_dynamic_obstacles.py @@ -1101,3 +1101,225 @@ def test_drone_curved_path_static_obstacle_and_boundary_collision() -> None: with pytest.raises(ValueError, match="does not match prev_pos"): bad_start = [np.array([0.0, 0.0, 0.0]), p_end] env._check_collision(p_end, prev_pos=p_start, intermediate_positions=bad_start) + + +def test_dynamic_collision_parameter_validation() -> None: + """check_continuous_collision and center_at enforce strict safety validation and fail closed.""" + import pytest + + obs = DynamicObstacleSphere3D( + center_initial=np.array([10.0, 10.0, 5.0]), + radius=1.0, + velocity=np.array([2.0, 0.0, 0.0]), + bounds=(30.0, 30.0, 15.0), + ) + p0 = np.array([5.0, 5.0, 5.0]) + p1 = np.array([6.0, 5.0, 5.0]) + + # 1. Reversed time interval (curr_time < prev_time) + with pytest.raises(ValueError, match="must be >= prev_time"): + obs.check_continuous_collision( + prev_drone_pos=p0, curr_drone_pos=p1, prev_time=2.0, curr_time=1.0, vehicle_radius=0.5 + ) + + # 2. Non-finite timestamps + for bad_t in [float("nan"), float("inf"), -float("inf"), True, False]: + with pytest.raises(ValueError, match="prev_time must be a finite float"): + obs.check_continuous_collision( + prev_drone_pos=p0, + curr_drone_pos=p1, + prev_time=bad_t, + curr_time=1.0, + vehicle_radius=0.5, + ) + with pytest.raises(ValueError, match="curr_time must be a finite float"): + obs.check_continuous_collision( + prev_drone_pos=p0, + curr_drone_pos=p1, + prev_time=0.0, + curr_time=bad_t, + vehicle_radius=0.5, + ) + + # 3. Invalid vehicle radius + for bad_r in [-0.5, -10.0, float("nan"), float("inf"), True, False]: + with pytest.raises(ValueError, match="vehicle_radius must be a non-negative finite float"): + obs.check_continuous_collision( + prev_drone_pos=p0, + curr_drone_pos=p1, + prev_time=0.0, + curr_time=1.0, + vehicle_radius=bad_r, + ) + + # 4. Invalid veh_acc_max + for bad_a in [-1.0, float("nan"), float("inf"), True, False]: + with pytest.raises(ValueError, match="veh_acc_max must be a non-negative finite float"): + obs.check_continuous_collision( + prev_drone_pos=p0, + curr_drone_pos=p1, + prev_time=0.0, + curr_time=1.0, + vehicle_radius=0.5, + veh_acc_max=bad_a, + ) + + # 5. Zero-duration collision check (prev_time == curr_time) + coll_zero = obs.check_continuous_collision( + prev_drone_pos=np.array([10.5, 10.0, 5.0]), # Within radius (1.0 + 0.5 = 1.5) + curr_drone_pos=np.array([10.5, 10.0, 5.0]), + prev_time=0.0, + curr_time=0.0, + vehicle_radius=0.5, + ) + assert coll_zero is True + + coll_zero_safe = obs.check_continuous_collision( + prev_drone_pos=np.array([20.0, 10.0, 5.0]), # Far outside + curr_drone_pos=np.array([20.0, 10.0, 5.0]), + prev_time=0.0, + curr_time=0.0, + vehicle_radius=0.5, + ) + assert coll_zero_safe is False + + # 6. center_at input validation + for bad_t in [float("nan"), float("inf"), True, False]: + with pytest.raises(ValueError, match="time_sec must be a finite float"): + obs.center_at(bad_t) + + +def test_endpoint_only_full_interval_curvature_safety_vs_oracle() -> None: + """Endpoint-only continuous collision checking applies full-interval curvature envelope verified by oracle.""" + # Stationary obstacle at [5.0, 6.0, 5.0] with radius 1.0, vehicle radius 0.2, threshold = 1.2 + obs = DynamicObstacleSphere3D( + center_initial=np.array([5.0, 6.0, 5.0]), + radius=1.0, + velocity=np.zeros(3), + bounds=(20.0, 20.0, 20.0), + ) + + # Vehicle flies from [0.0, 4.0, 5.0] to [10.0, 4.0, 5.0] over T = 1.0 s. + # Linear chord is at y=4.0 everywhere. Clearance to obstacle is 6.0 - 4.0 = 2.0 > 1.2. + # Straight chord appears completely collision-free! + p0 = np.array([0.0, 4.0, 5.0]) + p1 = np.array([10.0, 4.0, 5.0]) + T = 1.0 + + # True trajectory has parabolic excursion: delta_y(t) = 4 * delta_max * t * (T - t) / T^2. + # With a_max = 8.0 m/s^2, maximum deflection is delta_max = (1/8) * a_max * T^2 = 1.0 m. + # At t=0.5, y(0.5) = 4.0 + 1.0 = 5.0. Distance to [5.0, 6.0, 5.0] is 6.0 - 5.0 = 1.0 <= 1.2! + a_veh = 8.0 + + # 1. High-resolution numerical oracle (1000 samples along true trajectory) + t_samples = np.linspace(0.0, T, 1000) + oracle_collision = False + for t in t_samples: + pos_t = p0 + (t / T) * (p1 - p0) + pos_t[1] += 4.0 * 1.0 * t * (T - t) / (T**2) + if np.linalg.norm(pos_t - obs.center_at(t)) <= 1.2: + oracle_collision = True + break + assert oracle_collision is True, "Oracle confirms true physical trajectory penetrates obstacle!" + + # 2. Continuous collision check WITHOUT intermediate positions + # Must use full-interval curvature inflation eps_veh = (1/8) * a_max * T^2 = 1.0 m + # Effective threshold = 1.2 + 1.0 = 2.2 m >= 2.0 m chord distance! + detected = obs.check_continuous_collision( + prev_drone_pos=p0, + curr_drone_pos=p1, + prev_time=0.0, + curr_time=T, + vehicle_radius=0.2, + intermediate_drone_positions=None, + veh_acc_max=a_veh, + ) + assert detected is True, ( + "Endpoint-only check with defensible curvature envelope must catch collision!" + ) + + +def test_bounce_trajectory_exact_reflection_detection() -> None: + """Dynamic obstacle reflection at arena boundary is split at exact bounce time, catching wall collisions.""" + # Arena bounds: y in [0.0, 10.0], radius 1.0. Lower wall for obstacle center is at y = 1.0. + # Initial position at [5.0, 3.0, 5.0], moving at vy = -4.0 m/s. + # Reaches y = 1.0 at t = (3.0 - 1.0) / 4.0 = 0.5 s, then bounces back towards positive y. + obs = DynamicObstacleSphere3D( + center_initial=np.array([5.0, 3.0, 5.0]), + radius=1.0, + velocity=np.array([0.0, -4.0, 0.0]), + bounds=(10.0, 10.0, 10.0), + motion_type="bounce", + ) + + # 1. Exact bounce times + bounces = obs.get_bounce_times(0.0, 1.0) + assert len(bounces) == 1 + assert abs(bounces[0] - 0.5) < 1e-6 + + # 2. Collision near reflection wall + # Stationary drone hovering at [5.0, 1.5, 5.0], radius 0.2. + # At t=0.5, obstacle center is at [5.0, 1.0, 5.0]. Distance is 0.5 <= 1.0 + 0.2 = 1.2. + # Endpoints of obstacle over [0.0, 1.0]: + # At t=0.0, c = [5.0, 3.0, 5.0], dist = 1.5 > 1.2. + # At t=1.0, c = [5.0, 3.0, 5.0], dist = 1.5 > 1.2. + # Without bounce splitting, chord between c(0) and c(1) is at y=3.0, completely missing the drone at y=1.5! + coll_bounce = obs.check_continuous_collision( + prev_drone_pos=np.array([5.0, 1.5, 5.0]), + curr_drone_pos=np.array([5.0, 1.5, 5.0]), + prev_time=0.0, + curr_time=1.0, + vehicle_radius=0.2, + veh_acc_max=0.0, + ) + assert coll_bounce is True, "Bounce splitting must detect collision at boundary reflection!" + + # 3. Near-miss trajectory just outside obstacle threshold + coll_near_miss = obs.check_continuous_collision( + prev_drone_pos=np.array( + [6.3, 1.0, 5.0] + ), # Distance at bounce is sqrt((6.3-5)^2) = 1.3 > 1.2 + curr_drone_pos=np.array([6.3, 1.0, 5.0]), + prev_time=0.0, + curr_time=1.0, + vehicle_radius=0.2, + veh_acc_max=0.0, + ) + assert coll_near_miss is False + + # 4. Multiple bounces across large interval + bounces_multi = obs.get_bounce_times(0.0, 5.0) + assert len(bounces_multi) >= 2 + + +def test_drone_dynamics_max_translational_acceleration_safety_envelope() -> None: + """DroneDynamics6DOF validates physical parameters and enforces operating speed envelope.""" + import math + + import pytest + + from adaptive_rl.environments.drone_6dof import DroneDynamics6DOF + + dyn = DroneDynamics6DOF( + mass=1.0, + gravity=9.81, + max_thrust_per_motor=5.0, + linear_damping=0.1, + ) + + # 1. Invalid max_speed_bound + for bad_v in [-1.0, float("nan"), float("inf"), True, False]: + with pytest.raises(ValueError, match="max_speed_bound must be a non-negative finite float"): + dyn.max_translational_acceleration(max_speed_bound=bad_v) # type: ignore[arg-type] + + # 2. None derives analytical terminal velocity + a_term = dyn.max_translational_acceleration(max_speed_bound=None) + assert a_term > 0.0 + assert math.isfinite(a_term) + + # 3. High actual velocity expands acceleration bound conservatively + dyn.state.velocity = np.array([50.0, 0.0, 0.0]) # Actual vehicle speed 50 m/s + a_high_speed = dyn.max_translational_acceleration(max_speed_bound=10.0) + # Must use at least 50 m/s, so a_high_speed > nominal calculation at 10 m/s + a_nominal_10 = 20.0 + 9.81 + 0.1 * 10.0 + assert a_high_speed > a_nominal_10 diff --git a/tests/test_recorder_fingerprint_phase3.py b/tests/test_recorder_fingerprint_phase3.py index d2c99fe..4cd2b64 100644 --- a/tests/test_recorder_fingerprint_phase3.py +++ b/tests/test_recorder_fingerprint_phase3.py @@ -404,6 +404,9 @@ def fault(step: int, thrusts: np.ndarray) -> np.ndarray: GLOBAL_SCALING_FACTOR = 1.0 +_test_dummy_pkg = None +_test_opaque_val = None +_test_factorial_global = None def test_callable_provenance_defaults_and_kwdefaults() -> None: @@ -984,3 +987,200 @@ def test_explicit_configuration_duplicate_scenario_names_1to1_matching(tmp_path: ) assert record_mismatch.provenance_complete is False assert record_mismatch.configuration.provenance_complete is False + + +def test_provenance_captures_descriptors_staticmethod_classmethod_property() -> None: + """Class methods, static methods, and properties are unwrapped and their changes alter fingerprints.""" + from adaptive_rl.flight_testing.scenarios import extract_callable_provenance + + class ControllerA: + @staticmethod + def static_gain() -> float: + return 1.5 + + @classmethod + def class_limit(cls) -> float: + return 10.0 + + @property + def prop_val(self) -> float: + return 42.0 + + def __call__(self, x: float) -> float: + return x + self.static_gain() + self.class_limit() + self.prop_val + + class ControllerB: + @staticmethod + def static_gain() -> float: + return 2.5 # Changed static method + + @classmethod + def class_limit(cls) -> float: + return 10.0 + + @property + def prop_val(self) -> float: + return 42.0 + + def __call__(self, x: float) -> float: + return x + self.static_gain() + self.class_limit() + self.prop_val + + class ControllerC: + @staticmethod + def static_gain() -> float: + return 1.5 + + @classmethod + def class_limit(cls) -> float: + return 20.0 # Changed class method + + @property + def prop_val(self) -> float: + return 42.0 + + def __call__(self, x: float) -> float: + return x + self.static_gain() + self.class_limit() + self.prop_val + + class ControllerD: + @staticmethod + def static_gain() -> float: + return 1.5 + + @classmethod + def class_limit(cls) -> float: + return 10.0 + + @property + def prop_val(self) -> float: + return 99.0 # Changed property + + def __call__(self, x: float) -> float: + return x + self.static_gain() + self.class_limit() + self.prop_val + + prov_a = extract_callable_provenance(ControllerA()) + prov_b = extract_callable_provenance(ControllerB()) + prov_c = extract_callable_provenance(ControllerC()) + prov_d = extract_callable_provenance(ControllerD()) + + fp_a = compute_config_fingerprint(prov_a) + fp_b = compute_config_fingerprint(prov_b) + fp_c = compute_config_fingerprint(prov_c) + fp_d = compute_config_fingerprint(prov_d) + + # All fingerprints must differ and provenance must be complete + assert fp_a != fp_b + assert fp_a != fp_c + assert fp_a != fp_d + assert fp_b != fp_c + assert fp_c != fp_d + + assert prov_a.get("provenance_complete") is True + assert "class_methods" in prov_a + assert "static_gain" in prov_a["class_methods"] + assert "class_limit" in prov_a["class_methods"] + assert "prop_val" in prov_a["class_methods"] + assert prov_a["class_methods"]["static_gain"]["descriptor"] == "staticmethod" + assert prov_a["class_methods"]["class_limit"]["descriptor"] == "classmethod" + assert prov_a["class_methods"]["prop_val"]["descriptor"] == "property" + + +def test_provenance_nested_module_dependency_resolution() -> None: + """Nested module attributes accessed via co_names are recursively resolved and fingerprinted.""" + import types + + from adaptive_rl.flight_testing.scenarios import extract_callable_provenance + + dummy_pkg = types.ModuleType("dummy_pkg") + dummy_sub = types.ModuleType("dummy_sub") + dummy_pkg.sub = dummy_sub + + def helper_v1(x: float) -> float: + return x + 1.0 + + def helper_v2(x: float) -> float: + return x + 2.0 + + dummy_sub.helper = helper_v1 + + globals()["_test_dummy_pkg"] = dummy_pkg + try: + + def compute(val: float) -> float: + return _test_dummy_pkg.sub.helper(val) + + prov1 = extract_callable_provenance(compute) + assert prov1.get("provenance_complete") is True + assert "referenced_globals" in prov1 + assert "_test_dummy_pkg" in prov1["referenced_globals"] + assert "_test_dummy_pkg.attributes" in prov1["referenced_globals"] + assert "sub" in prov1["referenced_globals"]["_test_dummy_pkg.attributes"] + sub_info = prov1["referenced_globals"]["_test_dummy_pkg.attributes"]["sub"] + assert "helper" in sub_info.get("attributes", {}) + + dummy_sub.helper = helper_v2 + prov2 = extract_callable_provenance(compute) + fp1 = compute_config_fingerprint(prov1) + fp2 = compute_config_fingerprint(prov2) + assert fp1 != fp2 + finally: + globals().pop("_test_dummy_pkg", None) + + +def test_provenance_unsupported_dynamic_dependency_records_incomplete() -> None: + """Unsupported dynamic objects mark provenance as incomplete with explanatory reason.""" + from adaptive_rl.flight_testing.scenarios import extract_callable_provenance + + globals()["_test_opaque_val"] = object() + try: + + def dynamic_func(x: float) -> float: + return x + float(id(_test_opaque_val)) + + prov = extract_callable_provenance(dynamic_func) + assert prov.get("provenance_complete") is False + assert "referenced_globals" in prov + assert "_test_opaque_val" in prov["referenced_globals"] + assert prov["referenced_globals"]["_test_opaque_val"]["unsupported_dependency"] == "object" + assert "reason" in prov["referenced_globals"]["_test_opaque_val"] + finally: + globals().pop("_test_opaque_val", None) + + +def test_provenance_self_recursive_function_cycle_handling() -> None: + """Self-recursive functions terminate cycle tracking cleanly without infinite recursion.""" + from adaptive_rl.flight_testing.scenarios import extract_callable_provenance + + # 1. Closure-based self-recursion + def factorial_closure(n: int) -> int: + if n <= 1: + return 1 + return n * factorial_closure(n - 1) + + prov_c = extract_callable_provenance(factorial_closure) + assert prov_c.get("provenance_complete") is True + fp_c = compute_config_fingerprint(prov_c) + assert len(fp_c) == 64 + assert "closure" in prov_c + assert "factorial_closure" in prov_c["closure"] + assert "recursion_ref" in prov_c["closure"]["factorial_closure"] + + # 2. Global-based self-recursion + globals()["_test_factorial_global"] = lambda n: 1 + try: + + def factorial_global(n: int) -> int: + global _test_factorial_global + if n <= 1: + return 1 + return n * _test_factorial_global(n - 1) + + globals()["_test_factorial_global"] = factorial_global + prov_g = extract_callable_provenance(factorial_global) + assert prov_g.get("provenance_complete") is True + fp_g = compute_config_fingerprint(prov_g) + assert len(fp_g) == 64 + assert "referenced_globals" in prov_g + assert "_test_factorial_global" in prov_g["referenced_globals"] + assert "recursion_ref" in prov_g["referenced_globals"]["_test_factorial_global"] + finally: + globals().pop("_test_factorial_global", None)