diff --git a/benchmark_accuracy.py b/benchmark_accuracy.py index 8811e94..8be0ef0 100644 --- a/benchmark_accuracy.py +++ b/benchmark_accuracy.py @@ -11,7 +11,8 @@ import numpy as np import sys -sys.path.insert(0, '/home/user/Eigen-Geometric-Control') + +sys.path.insert(0, "/home/user/Eigen-Geometric-Control") from src import ( run_arm_simulation, @@ -20,7 +21,7 @@ forward_kinematics, compute_ds2, compute_gradient, - compute_change_stability + compute_change_stability, ) import pandas as pd @@ -37,7 +38,7 @@ def run_arm_with_lightlike_damping( eps_change=1e-3, L1=0.9, L2=0.9, - window=3 + window=3, ): """Run arm simulation WITH lightlike observer damping""" theta1, theta2 = theta_init @@ -80,28 +81,30 @@ def run_arm_with_lightlike_damping( C, S, ds2_CS = compute_change_stability(delta, eps_change) # Record state - rows.append({ - 'tick': t, - 'theta1_rad': theta1, - 'theta2_rad': theta2, - 'x': x, - 'y': y, - 'ds2_total': ds2_total, - 'target_term': components['target_term'], - 'obs_term': components['obs_term'], - 'reg_term': components['reg_term'], - 'grad_norm': grad_norm, - 'C': C, - 'S': S, - 'ds2_CS': ds2_CS, - 'd_obs': components['d_obs'], - 'delta_theta1': delta[0], - 'delta_theta2': delta[1], - 'delta_theta_sq': float(np.sum(delta**2)), - 'oscillating': oscillating, - 'osc_strength': osc_strength, - 'damping': damping, - }) + rows.append( + { + "tick": t, + "theta1_rad": theta1, + "theta2_rad": theta2, + "x": x, + "y": y, + "ds2_total": ds2_total, + "target_term": components["target_term"], + "obs_term": components["obs_term"], + "reg_term": components["reg_term"], + "grad_norm": grad_norm, + "C": C, + "S": S, + "ds2_CS": ds2_CS, + "d_obs": components["d_obs"], + "delta_theta1": delta[0], + "delta_theta2": delta[1], + "delta_theta_sq": float(np.sum(delta**2)), + "oscillating": oscillating, + "osc_strength": osc_strength, + "damping": damping, + } + ) # Update for next iteration theta1, theta2 = theta1_new, theta2_new @@ -111,34 +114,34 @@ def run_arm_with_lightlike_damping( def compute_metrics(df, target, name=""): """Compute accuracy metrics from simulation results""" - final_x = df['x'].iloc[-1] - final_y = df['y'].iloc[-1] - final_dist = np.sqrt((final_x - target[0])**2 + (final_y - target[1])**2) + final_x = df["x"].iloc[-1] + final_y = df["y"].iloc[-1] + final_dist = np.sqrt((final_x - target[0]) ** 2 + (final_y - target[1]) ** 2) # Convergence: first tick where distance < 0.1m - target_dist = np.sqrt((df['x'] - target[0])**2 + (df['y'] - target[1])**2) + target_dist = np.sqrt((df["x"] - target[0]) ** 2 + (df["y"] - target[1]) ** 2) converged_ticks = target_dist[target_dist < 0.1] ticks_to_converge = converged_ticks.index[0] if len(converged_ticks) > 0 else len(df) # Oscillation metrics - grad_norms = df['grad_norm'].values + grad_norms = df["grad_norm"].values osc_count = 0 for i in range(10, len(grad_norms) - 1): # Check if gradient reverses (sign of oscillation) - if grad_norms[i] > grad_norms[i-1] * 1.5: # Gradient increased significantly + if grad_norms[i] > grad_norms[i - 1] * 1.5: # Gradient increased significantly osc_count += 1 metrics = { - 'name': name, - 'final_distance_to_target_m': final_dist, - 'ticks_to_converge': ticks_to_converge, - 'final_ds2': df['ds2_total'].iloc[-1], - 'initial_ds2': df['ds2_total'].iloc[0], - 'ds2_reduction': df['ds2_total'].iloc[0] - df['ds2_total'].iloc[-1], - 'min_obstacle_dist_m': df['d_obs'].min(), - 'final_grad_norm': df['grad_norm'].iloc[-1], - 'oscillation_events': osc_count, - 'mean_step_size': df['delta_theta_sq'].mean(), + "name": name, + "final_distance_to_target_m": final_dist, + "ticks_to_converge": ticks_to_converge, + "final_ds2": df["ds2_total"].iloc[-1], + "initial_ds2": df["ds2_total"].iloc[0], + "ds2_reduction": df["ds2_total"].iloc[0] - df["ds2_total"].iloc[-1], + "min_obstacle_dist_m": df["d_obs"].min(), + "final_grad_norm": df["grad_norm"].iloc[-1], + "oscillation_events": osc_count, + "mean_step_size": df["delta_theta_sq"].mean(), } return metrics @@ -159,21 +162,14 @@ def main(): # Run baseline (v1.0.0 algorithm - no lightlike observer) print("1. Running baseline (v1.0.0 algorithm)...") df_baseline = run_arm_simulation( - theta_init=(-1.4, 1.2), - target=tuple(target), - n_ticks=140, - eta=0.12 + theta_init=(-1.4, 1.2), target=tuple(target), n_ticks=140, eta=0.12 ) metrics_baseline = compute_metrics(df_baseline, target, "v1.0.0 Baseline") # Run with lightlike observer print("2. Running with lightlike observer damping...") df_lightlike = run_arm_with_lightlike_damping( - theta_init=(-1.4, 1.2), - target=tuple(target), - n_ticks=140, - eta=0.12, - window=3 + theta_init=(-1.4, 1.2), target=tuple(target), n_ticks=140, eta=0.12, window=3 ) metrics_lightlike = compute_metrics(df_lightlike, target, "With Lightlike Observer") @@ -189,40 +185,56 @@ def main(): print("-" * 90) # Final accuracy - baseline_dist = metrics_baseline['final_distance_to_target_m'] - lightlike_dist = metrics_lightlike['final_distance_to_target_m'] - change_dist = ((lightlike_dist - baseline_dist) / baseline_dist * 100) if baseline_dist > 0 else 0 - print(f"{'Final distance to target (m)':<40} {baseline_dist:<20.4f} {lightlike_dist:<20.4f} {change_dist:+.1f}%") + baseline_dist = metrics_baseline["final_distance_to_target_m"] + lightlike_dist = metrics_lightlike["final_distance_to_target_m"] + change_dist = ( + ((lightlike_dist - baseline_dist) / baseline_dist * 100) if baseline_dist > 0 else 0 + ) + print( + f"{'Final distance to target (m)':<40} {baseline_dist:<20.4f} {lightlike_dist:<20.4f} {change_dist:+.1f}%" + ) # Convergence speed - baseline_conv = metrics_baseline['ticks_to_converge'] - lightlike_conv = metrics_lightlike['ticks_to_converge'] - change_conv = ((lightlike_conv - baseline_conv) / baseline_conv * 100) if baseline_conv > 0 else 0 - print(f"{'Ticks to converge (<0.1m)':<40} {baseline_conv:<20} {lightlike_conv:<20} {change_conv:+.1f}%") + baseline_conv = metrics_baseline["ticks_to_converge"] + lightlike_conv = metrics_lightlike["ticks_to_converge"] + change_conv = ( + ((lightlike_conv - baseline_conv) / baseline_conv * 100) if baseline_conv > 0 else 0 + ) + print( + f"{'Ticks to converge (<0.1m)':<40} {baseline_conv:<20} {lightlike_conv:<20} {change_conv:+.1f}%" + ) # ds2 reduction - baseline_ds2 = metrics_baseline['final_ds2'] - lightlike_ds2 = metrics_lightlike['final_ds2'] - change_ds2 = ((lightlike_ds2 - baseline_ds2) / abs(baseline_ds2) * 100) if baseline_ds2 != 0 else 0 + baseline_ds2 = metrics_baseline["final_ds2"] + lightlike_ds2 = metrics_lightlike["final_ds2"] + change_ds2 = ( + ((lightlike_ds2 - baseline_ds2) / abs(baseline_ds2) * 100) if baseline_ds2 != 0 else 0 + ) print(f"{'Final ds²':<40} {baseline_ds2:<20.4f} {lightlike_ds2:<20.4f} {change_ds2:+.1f}%") # Obstacle clearance - baseline_obs = metrics_baseline['min_obstacle_dist_m'] - lightlike_obs = metrics_lightlike['min_obstacle_dist_m'] + baseline_obs = metrics_baseline["min_obstacle_dist_m"] + lightlike_obs = metrics_lightlike["min_obstacle_dist_m"] change_obs = ((lightlike_obs - baseline_obs) / baseline_obs * 100) if baseline_obs > 0 else 0 - print(f"{'Min obstacle distance (m)':<40} {baseline_obs:<20.4f} {lightlike_obs:<20.4f} {change_obs:+.1f}%") + print( + f"{'Min obstacle distance (m)':<40} {baseline_obs:<20.4f} {lightlike_obs:<20.4f} {change_obs:+.1f}%" + ) # Oscillation - baseline_osc = metrics_baseline['oscillation_events'] - lightlike_osc = metrics_lightlike['oscillation_events'] + baseline_osc = metrics_baseline["oscillation_events"] + lightlike_osc = metrics_lightlike["oscillation_events"] change_osc = lightlike_osc - baseline_osc print(f"{'Oscillation events':<40} {baseline_osc:<20} {lightlike_osc:<20} {change_osc:+d}") # Final gradient - baseline_grad = metrics_baseline['final_grad_norm'] - lightlike_grad = metrics_lightlike['final_grad_norm'] - change_grad = ((lightlike_grad - baseline_grad) / baseline_grad * 100) if baseline_grad > 0 else 0 - print(f"{'Final gradient norm':<40} {baseline_grad:<20.6f} {lightlike_grad:<20.6f} {change_grad:+.1f}%") + baseline_grad = metrics_baseline["final_grad_norm"] + lightlike_grad = metrics_lightlike["final_grad_norm"] + change_grad = ( + ((lightlike_grad - baseline_grad) / baseline_grad * 100) if baseline_grad > 0 else 0 + ) + print( + f"{'Final gradient norm':<40} {baseline_grad:<20.6f} {lightlike_grad:<20.6f} {change_grad:+.1f}%" + ) print() print("=" * 80) @@ -257,9 +269,9 @@ def main(): print("=" * 80) # Check for lightlike observer activations - if 'damping' in df_lightlike.columns: - damping_used = (df_lightlike['damping'] > 0).sum() - max_damping = df_lightlike['damping'].max() + if "damping" in df_lightlike.columns: + damping_used = (df_lightlike["damping"] > 0).sum() + max_damping = df_lightlike["damping"].max() print(f"\nLightlike observer activations: {damping_used} / {len(df_lightlike)} ticks") print(f"Maximum damping applied: {max_damping:.4f}") @@ -267,5 +279,5 @@ def main(): print("\n→ Lightlike observer was NEVER activated (no oscillation detected)") -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/benchmark_adaptive_twophase.py b/benchmark_adaptive_twophase.py index 08ac6de..ad2b8d5 100644 --- a/benchmark_adaptive_twophase.py +++ b/benchmark_adaptive_twophase.py @@ -16,7 +16,8 @@ import numpy as np import sys -sys.path.insert(0, '/home/user/Eigen-Geometric-Control') + +sys.path.insert(0, "/home/user/Eigen-Geometric-Control") from src import ( adaptive_control_parameters, @@ -68,9 +69,7 @@ def run_fixed_eta_control( damping = 0.0 if use_lightlike and len(state_history) >= 4: - oscillating, osc_strength = detect_oscillation( - state_history, window=4, threshold=0.92 - ) + oscillating, osc_strength = detect_oscillation(state_history, window=4, threshold=0.92) if oscillating: damping = lightlike_damping_factor(osc_strength) * 0.85 @@ -79,21 +78,23 @@ def run_fixed_eta_control( theta1_new = theta1 + delta[0] theta2_new = theta2 + delta[1] - distance_to_target = np.sqrt((x - target[0])**2 + (y - target[1])**2) - - rows.append({ - 'tick': t, - 'x': x, - 'y': y, - 'distance_to_target': distance_to_target, - 'ds2_total': ds2_total, - 'grad_norm': grad_norm, - 'd_obs': components['d_obs'], - 'eta': eta, - 'phase': 'FIXED', - 'oscillating': oscillating, - 'damping': damping, - }) + distance_to_target = np.sqrt((x - target[0]) ** 2 + (y - target[1]) ** 2) + + rows.append( + { + "tick": t, + "x": x, + "y": y, + "distance_to_target": distance_to_target, + "ds2_total": ds2_total, + "grad_norm": grad_norm, + "d_obs": components["d_obs"], + "eta": eta, + "phase": "FIXED", + "oscillating": oscillating, + "damping": damping, + } + ) theta1, theta2 = theta1_new, theta2_new @@ -125,7 +126,7 @@ def run_adaptive_control( state = np.array([theta1, theta2]) state_history.append(state) - distance_to_target = np.sqrt((x - target[0])**2 + (y - target[1])**2) + distance_to_target = np.sqrt((x - target[0]) ** 2 + (y - target[1]) ** 2) # ADAPTIVE: Determine eta and lightlike based on distance eta, use_lightlike, phase = adaptive_control_parameters( @@ -146,9 +147,7 @@ def run_adaptive_control( damping = 0.0 if use_lightlike and len(state_history) >= 4: - oscillating, osc_strength = detect_oscillation( - state_history, window=4, threshold=0.92 - ) + oscillating, osc_strength = detect_oscillation(state_history, window=4, threshold=0.92) if oscillating: damping = lightlike_damping_factor(osc_strength) * 0.85 @@ -157,19 +156,21 @@ def run_adaptive_control( theta1_new = theta1 + delta[0] theta2_new = theta2 + delta[1] - rows.append({ - 'tick': t, - 'x': x, - 'y': y, - 'distance_to_target': distance_to_target, - 'ds2_total': ds2_total, - 'grad_norm': grad_norm, - 'd_obs': components['d_obs'], - 'eta': eta, - 'phase': phase, - 'oscillating': oscillating, - 'damping': damping, - }) + rows.append( + { + "tick": t, + "x": x, + "y": y, + "distance_to_target": distance_to_target, + "ds2_total": ds2_total, + "grad_norm": grad_norm, + "d_obs": components["d_obs"], + "eta": eta, + "phase": phase, + "oscillating": oscillating, + "damping": damping, + } + ) theta1, theta2 = theta1_new, theta2_new @@ -192,8 +193,8 @@ def main(): print("\n[1/3] Running fixed FAST approach...") df_fast = run_fixed_eta_control(eta=0.12, use_lightlike=False, n_ticks=500) - fast_final_error = df_fast['distance_to_target'].iloc[-1] * 1000 # mm - fast_ticks_to_10cm = len(df_fast[df_fast['distance_to_target'] > 0.1]) + fast_final_error = df_fast["distance_to_target"].iloc[-1] * 1000 # mm + fast_ticks_to_10cm = len(df_fast[df_fast["distance_to_target"] > 0.1]) print(f" Final error: {fast_final_error:.1f}mm") print(f" Ticks to reach 10cm: {fast_ticks_to_10cm}") @@ -202,8 +203,8 @@ def main(): print("\n[2/3] Running fixed ULTRA-SLOW approach...") df_ultraslow = run_fixed_eta_control(eta=0.015, use_lightlike=True, n_ticks=500) - ultraslow_final_error = df_ultraslow['distance_to_target'].iloc[-1] * 1000 # mm - ultraslow_ticks_to_10cm = len(df_ultraslow[df_ultraslow['distance_to_target'] > 0.1]) + ultraslow_final_error = df_ultraslow["distance_to_target"].iloc[-1] * 1000 # mm + ultraslow_ticks_to_10cm = len(df_ultraslow[df_ultraslow["distance_to_target"] > 0.1]) print(f" Final error: {ultraslow_final_error:.1f}mm") print(f" Ticks to reach 10cm: {ultraslow_ticks_to_10cm}") @@ -212,11 +213,11 @@ def main(): print("\n[3/3] Running ADAPTIVE two-phase control...") df_adaptive = run_adaptive_control(n_ticks=500, terminal_threshold=0.15) - adaptive_final_error = df_adaptive['distance_to_target'].iloc[-1] * 1000 # mm - adaptive_ticks_to_10cm = len(df_adaptive[df_adaptive['distance_to_target'] > 0.1]) + adaptive_final_error = df_adaptive["distance_to_target"].iloc[-1] * 1000 # mm + adaptive_ticks_to_10cm = len(df_adaptive[df_adaptive["distance_to_target"] > 0.1]) # Find when it switched to terminal phase - terminal_starts = df_adaptive[df_adaptive['phase'] == 'TERMINAL_DESCENT'] + terminal_starts = df_adaptive[df_adaptive["phase"] == "TERMINAL_DESCENT"] if len(terminal_starts) > 0: terminal_start_tick = terminal_starts.index[0] else: @@ -235,44 +236,54 @@ def main(): results = [ { - 'Strategy': 'Fixed Fast', - 'Final Error (mm)': f"{fast_final_error:.1f}", - 'Ticks to 10cm': fast_ticks_to_10cm, - 'Speed': '⚡ Fast', - 'Precision': '❌ Poor', + "Strategy": "Fixed Fast", + "Final Error (mm)": f"{fast_final_error:.1f}", + "Ticks to 10cm": fast_ticks_to_10cm, + "Speed": "⚡ Fast", + "Precision": "❌ Poor", }, { - 'Strategy': 'Fixed Ultra-Slow', - 'Final Error (mm)': f"{ultraslow_final_error:.1f}", - 'Ticks to 10cm': ultraslow_ticks_to_10cm, - 'Speed': '🐌 Very Slow', - 'Precision': '✓ Excellent', + "Strategy": "Fixed Ultra-Slow", + "Final Error (mm)": f"{ultraslow_final_error:.1f}", + "Ticks to 10cm": ultraslow_ticks_to_10cm, + "Speed": "🐌 Very Slow", + "Precision": "✓ Excellent", }, { - 'Strategy': 'Adaptive Two-Phase', - 'Final Error (mm)': f"{adaptive_final_error:.1f}", - 'Ticks to 10cm': adaptive_ticks_to_10cm, - 'Speed': '✓ Fast', - 'Precision': '✓ Excellent', + "Strategy": "Adaptive Two-Phase", + "Final Error (mm)": f"{adaptive_final_error:.1f}", + "Ticks to 10cm": adaptive_ticks_to_10cm, + "Speed": "✓ Fast", + "Precision": "✓ Excellent", }, ] # Print table - print(f"{'Strategy':<25} {'Final Error':<15} {'Ticks to 10cm':<15} {'Speed':<15} {'Precision':<15}") + print( + f"{'Strategy':<25} {'Final Error':<15} {'Ticks to 10cm':<15} {'Speed':<15} {'Precision':<15}" + ) print("-" * 85) for r in results: - print(f"{r['Strategy']:<25} {r['Final Error (mm)']:<15} {r['Ticks to 10cm']:<15} {r['Speed']:<15} {r['Precision']:<15}") + print( + f"{r['Strategy']:<25} {r['Final Error (mm)']:<15} {r['Ticks to 10cm']:<15} {r['Speed']:<15} {r['Precision']:<15}" + ) print() print("KEY INSIGHT:") print(" Adaptive two-phase gets BOTH:") - print(f" • Speed of fast approach ({adaptive_ticks_to_10cm} vs {fast_ticks_to_10cm} ticks to 10cm)") - print(f" • Precision of ultra-slow ({adaptive_final_error:.1f}mm vs {ultraslow_final_error:.1f}mm final error)") + print( + f" • Speed of fast approach ({adaptive_ticks_to_10cm} vs {fast_ticks_to_10cm} ticks to 10cm)" + ) + print( + f" • Precision of ultra-slow ({adaptive_final_error:.1f}mm vs {ultraslow_final_error:.1f}mm final error)" + ) print() # Comparison vs fixed approaches vs_fast_precision = ((fast_final_error - adaptive_final_error) / fast_final_error) * 100 - vs_ultraslow_speed = ((ultraslow_ticks_to_10cm - adaptive_ticks_to_10cm) / ultraslow_ticks_to_10cm) * 100 + vs_ultraslow_speed = ( + (ultraslow_ticks_to_10cm - adaptive_ticks_to_10cm) / ultraslow_ticks_to_10cm + ) * 100 print("IMPROVEMENTS:") print(f" vs Fixed Fast:") @@ -281,7 +292,9 @@ def main(): print() print(f" vs Fixed Ultra-Slow:") print(f" • {vs_ultraslow_speed:+.1f}% faster to 10cm") - print(f" • Similar precision ({adaptive_final_error:.1f}mm vs {ultraslow_final_error:.1f}mm)") + print( + f" • Similar precision ({adaptive_final_error:.1f}mm vs {ultraslow_final_error:.1f}mm)" + ) print() # Phase transition analysis @@ -290,10 +303,16 @@ def main(): terminal_phase_ticks = len(df_adaptive) - terminal_start_tick print("PHASE BREAKDOWN:") - print(f" Fast Approach Phase: {fast_phase_ticks:3d} ticks ({fast_phase_ticks/len(df_adaptive)*100:.1f}%)") - print(f" Terminal Descent Phase: {terminal_phase_ticks:3d} ticks ({terminal_phase_ticks/len(df_adaptive)*100:.1f}%)") + print( + f" Fast Approach Phase: {fast_phase_ticks:3d} ticks ({fast_phase_ticks/len(df_adaptive)*100:.1f}%)" + ) + print( + f" Terminal Descent Phase: {terminal_phase_ticks:3d} ticks ({terminal_phase_ticks/len(df_adaptive)*100:.1f}%)" + ) print() - print(f" Adaptive control spent only {terminal_phase_ticks/len(df_adaptive)*100:.1f}% of time in slow mode,") + print( + f" Adaptive control spent only {terminal_phase_ticks/len(df_adaptive)*100:.1f}% of time in slow mode," + ) print(f" but achieved precision comparable to 100% ultra-slow!") print() diff --git a/benchmark_coherent_decomposition.py b/benchmark_coherent_decomposition.py index ef03c4e..05f3510 100644 --- a/benchmark_coherent_decomposition.py +++ b/benchmark_coherent_decomposition.py @@ -109,12 +109,8 @@ def compute_trajectory_metrics( # Path smoothness (lower is smoother) if len(trajectory) > 2: - velocities = [ - trajectory[i] - trajectory[i - 1] for i in range(1, len(trajectory)) - ] - accelerations = [ - velocities[i] - velocities[i - 1] for i in range(1, len(velocities)) - ] + velocities = [trajectory[i] - trajectory[i - 1] for i in range(1, len(trajectory))] + accelerations = [velocities[i] - velocities[i - 1] for i in range(1, len(velocities))] jitter = np.mean([np.linalg.norm(a) for a in accelerations]) else: jitter = 0.0 @@ -219,9 +215,7 @@ def avg_metric(metrics, key): * 100 ) jitter_reduction = ( - (results["naive_jitter"] - results["filtered_jitter"]) - / results["naive_jitter"] - * 100 + (results["naive_jitter"] - results["filtered_jitter"]) / results["naive_jitter"] * 100 ) results["error_improvement"] = error_improvement @@ -244,12 +238,8 @@ def avg_metric(metrics, key): print("=" * 70) print() - avg_error_improvement = np.mean( - [r["error_improvement"] for r in all_results.values()] - ) - avg_jitter_reduction = np.mean( - [r["jitter_reduction"] for r in all_results.values()] - ) + avg_error_improvement = np.mean([r["error_improvement"] for r in all_results.values()]) + avg_jitter_reduction = np.mean([r["jitter_reduction"] for r in all_results.values()]) print(f"Average Error Improvement: {avg_error_improvement:+.1f}%") print(f"Average Jitter Reduction: {avg_jitter_reduction:+.1f}%") @@ -342,9 +332,7 @@ def plot_results(results: dict, target: np.ndarray): # Bottom row: Performance comparison ax = axes[1, idx] - ax.set_title( - f"Improvement: {data['error_improvement']:+.1f}%", fontsize=11 - ) + ax.set_title(f"Improvement: {data['error_improvement']:+.1f}%", fontsize=11) metrics = ["Mean Error", "Jitter"] naive_vals = [data["naive_mean_error"], data["naive_jitter"]] @@ -354,9 +342,7 @@ def plot_results(results: dict, target: np.ndarray): width = 0.35 ax.bar(x - width / 2, naive_vals, width, label="Naive", color="red", alpha=0.7) - ax.bar( - x + width / 2, filt_vals, width, label="Filtered", color="blue", alpha=0.7 - ) + ax.bar(x + width / 2, filt_vals, width, label="Filtered", color="blue", alpha=0.7) ax.set_ylabel("Value") ax.set_xticks(x) diff --git a/benchmark_dynamic.py b/benchmark_dynamic.py index e8fd935..4bb765b 100644 --- a/benchmark_dynamic.py +++ b/benchmark_dynamic.py @@ -15,7 +15,8 @@ import numpy as np import sys -sys.path.insert(0, '/home/user/Eigen-Geometric-Control') + +sys.path.insert(0, "/home/user/Eigen-Geometric-Control") from src import ( detect_oscillation, @@ -23,7 +24,7 @@ forward_kinematics, compute_ds2, compute_gradient, - compute_change_stability + compute_change_stability, ) import pandas as pd @@ -70,20 +71,22 @@ def run_dynamic_baseline( C, S, ds2_CS = compute_change_stability(delta, 1e-3) # Record state - rows.append({ - 'tick': t, - 'theta1_rad': theta1, - 'theta2_rad': theta2, - 'x': x, - 'y': y, - 'target_x': target[0], - 'target_y': target[1], - 'obs_x': obstacle_center[0], - 'obs_y': obstacle_center[1], - 'ds2_total': ds2_total, - 'grad_norm': grad_norm, - 'd_obs': components['d_obs'], - }) + rows.append( + { + "tick": t, + "theta1_rad": theta1, + "theta2_rad": theta2, + "x": x, + "y": y, + "target_x": target[0], + "target_y": target[1], + "obs_x": obstacle_center[0], + "obs_y": obstacle_center[1], + "ds2_total": ds2_total, + "grad_norm": grad_norm, + "d_obs": components["d_obs"], + } + ) # Update for next iteration theta1, theta2 = theta1_new, theta2_new @@ -151,23 +154,25 @@ def run_dynamic_lightlike( C, S, ds2_CS = compute_change_stability(delta, 1e-3) # Record state - rows.append({ - 'tick': t, - 'theta1_rad': theta1, - 'theta2_rad': theta2, - 'x': x, - 'y': y, - 'target_x': target[0], - 'target_y': target[1], - 'obs_x': obstacle_center[0], - 'obs_y': obstacle_center[1], - 'ds2_total': ds2_total, - 'grad_norm': grad_norm, - 'd_obs': components['d_obs'], - 'oscillating': oscillating, - 'osc_strength': osc_strength, - 'damping': damping, - }) + rows.append( + { + "tick": t, + "theta1_rad": theta1, + "theta2_rad": theta2, + "x": x, + "y": y, + "target_x": target[0], + "target_y": target[1], + "obs_x": obstacle_center[0], + "obs_y": obstacle_center[1], + "ds2_total": ds2_total, + "grad_norm": grad_norm, + "d_obs": components["d_obs"], + "oscillating": oscillating, + "osc_strength": osc_strength, + "damping": damping, + } + ) # Update for next iteration theta1, theta2 = theta1_new, theta2_new @@ -178,35 +183,33 @@ def run_dynamic_lightlike( def analyze_tracking(df, scenario_name): """Analyze tracking performance""" # Tracking error - tracking_error = np.sqrt( - (df['x'] - df['target_x'])**2 + (df['y'] - df['target_y'])**2 - ) + tracking_error = np.sqrt((df["x"] - df["target_x"]) ** 2 + (df["y"] - df["target_y"]) ** 2) # Oscillation detection (gradient reversals) - grad_norms = df['grad_norm'].values + grad_norms = df["grad_norm"].values oscillations = 0 for i in range(10, len(grad_norms) - 1): - if grad_norms[i] > grad_norms[i-1] * 1.3: # Gradient spike + if grad_norms[i] > grad_norms[i - 1] * 1.3: # Gradient spike oscillations += 1 # Collision risk (how close to obstacle) - min_clearance = df['d_obs'].min() - near_collisions = (df['d_obs'] < 0.30).sum() # Within 5cm of collision + min_clearance = df["d_obs"].min() + near_collisions = (df["d_obs"] < 0.30).sum() # Within 5cm of collision metrics = { - 'scenario': scenario_name, - 'mean_tracking_error_mm': tracking_error.mean() * 1000, - 'max_tracking_error_mm': tracking_error.max() * 1000, - 'final_tracking_error_mm': tracking_error.iloc[-1] * 1000, - 'tracking_variance': tracking_error.std() * 1000, - 'oscillations': oscillations, - 'min_clearance_m': min_clearance, - 'near_collisions': near_collisions, + "scenario": scenario_name, + "mean_tracking_error_mm": tracking_error.mean() * 1000, + "max_tracking_error_mm": tracking_error.max() * 1000, + "final_tracking_error_mm": tracking_error.iloc[-1] * 1000, + "tracking_variance": tracking_error.std() * 1000, + "oscillations": oscillations, + "min_clearance_m": min_clearance, + "near_collisions": near_collisions, } - if 'damping' in df.columns: - metrics['lightlike_activations'] = (df['damping'] > 0).sum() - metrics['max_damping'] = df['damping'].max() + if "damping" in df.columns: + metrics["lightlike_activations"] = (df["damping"] > 0).sum() + metrics["max_damping"] = df["damping"].max() return metrics @@ -224,112 +227,109 @@ def main(): # Scenario 1: Moving target (linear motion) print("Scenario 1: Linear moving target (tracking task)...") + def target_linear(t): - return [1.0 + 0.002*t, 0.3 + 0.001*t] + return [1.0 + 0.002 * t, 0.3 + 0.001 * t] + def obstacle_static(t): return [0.6, 0.1] df_base_1 = run_dynamic_baseline( - target_trajectory=target_linear, - obstacle_trajectory=obstacle_static, - n_ticks=150 + target_trajectory=target_linear, obstacle_trajectory=obstacle_static, n_ticks=150 ) df_light_1 = run_dynamic_lightlike( - target_trajectory=target_linear, - obstacle_trajectory=obstacle_static, - n_ticks=150 + target_trajectory=target_linear, obstacle_trajectory=obstacle_static, n_ticks=150 ) - results.append({ - 'baseline': analyze_tracking(df_base_1, "Moving Target - Baseline"), - 'lightlike': analyze_tracking(df_light_1, "Moving Target - Lightlike"), - }) + results.append( + { + "baseline": analyze_tracking(df_base_1, "Moving Target - Baseline"), + "lightlike": analyze_tracking(df_light_1, "Moving Target - Lightlike"), + } + ) # Scenario 2: Oscillating target (periodic motion) print("Scenario 2: Oscillating target (tracking periodic motion)...") + def target_oscillate(t): - return [1.2 + 0.1*np.sin(t*0.3), 0.3 + 0.08*np.cos(t*0.3)] + return [1.2 + 0.1 * np.sin(t * 0.3), 0.3 + 0.08 * np.cos(t * 0.3)] df_base_2 = run_dynamic_baseline( - target_trajectory=target_oscillate, - obstacle_trajectory=obstacle_static, - n_ticks=150 + target_trajectory=target_oscillate, obstacle_trajectory=obstacle_static, n_ticks=150 ) df_light_2 = run_dynamic_lightlike( - target_trajectory=target_oscillate, - obstacle_trajectory=obstacle_static, - n_ticks=150 + target_trajectory=target_oscillate, obstacle_trajectory=obstacle_static, n_ticks=150 ) - results.append({ - 'baseline': analyze_tracking(df_base_2, "Oscillating Target - Baseline"), - 'lightlike': analyze_tracking(df_light_2, "Oscillating Target - Lightlike"), - }) + results.append( + { + "baseline": analyze_tracking(df_base_2, "Oscillating Target - Baseline"), + "lightlike": analyze_tracking(df_light_2, "Oscillating Target - Lightlike"), + } + ) # Scenario 3: Moving obstacle (avoidance) print("Scenario 3: Moving obstacle (dynamic avoidance)...") + def target_fixed(t): return [1.2, 0.3] + def obstacle_moving(t): - return [0.5 + 0.003*t, 0.15 + 0.001*t] + return [0.5 + 0.003 * t, 0.15 + 0.001 * t] df_base_3 = run_dynamic_baseline( - target_trajectory=target_fixed, - obstacle_trajectory=obstacle_moving, - n_ticks=150 + target_trajectory=target_fixed, obstacle_trajectory=obstacle_moving, n_ticks=150 ) df_light_3 = run_dynamic_lightlike( - target_trajectory=target_fixed, - obstacle_trajectory=obstacle_moving, - n_ticks=150 + target_trajectory=target_fixed, obstacle_trajectory=obstacle_moving, n_ticks=150 ) - results.append({ - 'baseline': analyze_tracking(df_base_3, "Moving Obstacle - Baseline"), - 'lightlike': analyze_tracking(df_light_3, "Moving Obstacle - Lightlike"), - }) + results.append( + { + "baseline": analyze_tracking(df_base_3, "Moving Obstacle - Baseline"), + "lightlike": analyze_tracking(df_light_3, "Moving Obstacle - Lightlike"), + } + ) # Scenario 4: Both moving (complex dynamics) print("Scenario 4: Both target and obstacle moving (complex dynamics)...") df_base_4 = run_dynamic_baseline( - target_trajectory=target_linear, - obstacle_trajectory=obstacle_moving, - n_ticks=150 + target_trajectory=target_linear, obstacle_trajectory=obstacle_moving, n_ticks=150 ) df_light_4 = run_dynamic_lightlike( - target_trajectory=target_linear, - obstacle_trajectory=obstacle_moving, - n_ticks=150 + target_trajectory=target_linear, obstacle_trajectory=obstacle_moving, n_ticks=150 ) - results.append({ - 'baseline': analyze_tracking(df_base_4, "Both Moving - Baseline"), - 'lightlike': analyze_tracking(df_light_4, "Both Moving - Lightlike"), - }) + results.append( + { + "baseline": analyze_tracking(df_base_4, "Both Moving - Baseline"), + "lightlike": analyze_tracking(df_light_4, "Both Moving - Lightlike"), + } + ) # Scenario 5: Evasive obstacle (approaches end-effector) print("Scenario 5: Evasive obstacle (adversarial scenario)...") + def obstacle_evasive(t): # Obstacle tries to block path to target - return [0.8 + 0.002*t, 0.2 + 0.002*t] + return [0.8 + 0.002 * t, 0.2 + 0.002 * t] df_base_5 = run_dynamic_baseline( target_trajectory=target_fixed, obstacle_trajectory=obstacle_evasive, n_ticks=150, - Go=6.0 # Stronger repulsion + Go=6.0, # Stronger repulsion ) df_light_5 = run_dynamic_lightlike( - target_trajectory=target_fixed, - obstacle_trajectory=obstacle_evasive, - n_ticks=150, - Go=6.0 + target_trajectory=target_fixed, obstacle_trajectory=obstacle_evasive, n_ticks=150, Go=6.0 ) - results.append({ - 'baseline': analyze_tracking(df_base_5, "Evasive Obstacle - Baseline"), - 'lightlike': analyze_tracking(df_light_5, "Evasive Obstacle - Lightlike"), - }) + results.append( + { + "baseline": analyze_tracking(df_base_5, "Evasive Obstacle - Baseline"), + "lightlike": analyze_tracking(df_light_5, "Evasive Obstacle - Lightlike"), + } + ) # Display results print() @@ -341,51 +341,67 @@ def obstacle_evasive(t): print("Mean Tracking Error (mm):") print("-" * 80) for r in results: - base = r['baseline'] - light = r['lightlike'] - improvement = (base['mean_tracking_error_mm'] - light['mean_tracking_error_mm']) / base['mean_tracking_error_mm'] * 100 + base = r["baseline"] + light = r["lightlike"] + improvement = ( + (base["mean_tracking_error_mm"] - light["mean_tracking_error_mm"]) + / base["mean_tracking_error_mm"] + * 100 + ) arrow = "↓" if improvement > 0 else "↑" - scenario = base['scenario'].replace(' - Baseline', '') - print(f"{scenario:<30} {base['mean_tracking_error_mm']:>7.1f}mm → {light['mean_tracking_error_mm']:>7.1f}mm " - f"{arrow} {abs(improvement):>5.1f}%") + scenario = base["scenario"].replace(" - Baseline", "") + print( + f"{scenario:<30} {base['mean_tracking_error_mm']:>7.1f}mm → {light['mean_tracking_error_mm']:>7.1f}mm " + f"{arrow} {abs(improvement):>5.1f}%" + ) print() print("Tracking Stability (std deviation in mm):") print("-" * 80) for r in results: - base = r['baseline'] - light = r['lightlike'] - improvement = (base['tracking_variance'] - light['tracking_variance']) / base['tracking_variance'] * 100 + base = r["baseline"] + light = r["lightlike"] + improvement = ( + (base["tracking_variance"] - light["tracking_variance"]) + / base["tracking_variance"] + * 100 + ) arrow = "↓" if improvement > 0 else "↑" - scenario = base['scenario'].replace(' - Baseline', '') - print(f"{scenario:<30} {base['tracking_variance']:>7.1f}mm → {light['tracking_variance']:>7.1f}mm " - f"{arrow} {abs(improvement):>5.1f}%") + scenario = base["scenario"].replace(" - Baseline", "") + print( + f"{scenario:<30} {base['tracking_variance']:>7.1f}mm → {light['tracking_variance']:>7.1f}mm " + f"{arrow} {abs(improvement):>5.1f}%" + ) print() print("Oscillation Events (gradient reversals):") print("-" * 80) for r in results: - base = r['baseline'] - light = r['lightlike'] - reduction = base['oscillations'] - light['oscillations'] - - scenario = base['scenario'].replace(' - Baseline', '') - print(f"{scenario:<30} {base['oscillations']:>4} → {light['oscillations']:>4} events " - f"({reduction:+d})") + base = r["baseline"] + light = r["lightlike"] + reduction = base["oscillations"] - light["oscillations"] + + scenario = base["scenario"].replace(" - Baseline", "") + print( + f"{scenario:<30} {base['oscillations']:>4} → {light['oscillations']:>4} events " + f"({reduction:+d})" + ) print() print("Collision Safety (near-collision events):") print("-" * 80) for r in results: - base = r['baseline'] - light = r['lightlike'] - reduction = base['near_collisions'] - light['near_collisions'] - - scenario = base['scenario'].replace(' - Baseline', '') - print(f"{scenario:<30} {base['near_collisions']:>4} → {light['near_collisions']:>4} events " - f"({reduction:+d})") + base = r["baseline"] + light = r["lightlike"] + reduction = base["near_collisions"] - light["near_collisions"] + + scenario = base["scenario"].replace(" - Baseline", "") + print( + f"{scenario:<30} {base['near_collisions']:>4} → {light['near_collisions']:>4} events " + f"({reduction:+d})" + ) print() print("=" * 80) @@ -395,20 +411,21 @@ def obstacle_evasive(t): # Calculate overall improvements total_tracking_improvement = sum( - (r['baseline']['mean_tracking_error_mm'] - r['lightlike']['mean_tracking_error_mm']) / - r['baseline']['mean_tracking_error_mm'] * 100 + (r["baseline"]["mean_tracking_error_mm"] - r["lightlike"]["mean_tracking_error_mm"]) + / r["baseline"]["mean_tracking_error_mm"] + * 100 for r in results ) / len(results) total_stability_improvement = sum( - (r['baseline']['tracking_variance'] - r['lightlike']['tracking_variance']) / - r['baseline']['tracking_variance'] * 100 + (r["baseline"]["tracking_variance"] - r["lightlike"]["tracking_variance"]) + / r["baseline"]["tracking_variance"] + * 100 for r in results ) / len(results) total_oscillation_reduction = sum( - r['baseline']['oscillations'] - r['lightlike']['oscillations'] - for r in results + r["baseline"]["oscillations"] - r["lightlike"]["oscillations"] for r in results ) print(f"Average tracking accuracy improvement: {total_tracking_improvement:+.1f}%") @@ -442,23 +459,31 @@ def obstacle_evasive(t): print("=" * 80) # Best scenario analysis - best_idx = max(range(len(results)), - key=lambda i: (results[i]['baseline']['mean_tracking_error_mm'] - - results[i]['lightlike']['mean_tracking_error_mm'])) + best_idx = max( + range(len(results)), + key=lambda i: ( + results[i]["baseline"]["mean_tracking_error_mm"] + - results[i]["lightlike"]["mean_tracking_error_mm"] + ), + ) best = results[best_idx] print() print(f"BEST PERFORMANCE: {best['baseline']['scenario'].replace(' - Baseline', '')}") - improvement = (best['baseline']['mean_tracking_error_mm'] - - best['lightlike']['mean_tracking_error_mm']) / \ - best['baseline']['mean_tracking_error_mm'] * 100 + improvement = ( + (best["baseline"]["mean_tracking_error_mm"] - best["lightlike"]["mean_tracking_error_mm"]) + / best["baseline"]["mean_tracking_error_mm"] + * 100 + ) print(f" Tracking improvement: {improvement:.1f}%") - print(f" Oscillations reduced: {best['baseline']['oscillations'] - best['lightlike']['oscillations']} events") + print( + f" Oscillations reduced: {best['baseline']['oscillations'] - best['lightlike']['oscillations']} events" + ) - if 'lightlike_activations' in best['lightlike']: + if "lightlike_activations" in best["lightlike"]: print(f" Observer activations: {best['lightlike']['lightlike_activations']}") print(f" Max damping: {best['lightlike']['max_damping']:.3f}") -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/benchmark_dynamic_adaptation.py b/benchmark_dynamic_adaptation.py index 7a068a4..2fb93e0 100644 --- a/benchmark_dynamic_adaptation.py +++ b/benchmark_dynamic_adaptation.py @@ -135,9 +135,7 @@ def simulate_robot_control( if stable_params: avg_params = { "c": np.mean([p["c"] for p in stable_params]), - "eta_recommended": np.mean( - [p["eta_recommended"] for p in stable_params] - ), + "eta_recommended": np.mean([p["eta_recommended"] for p in stable_params]), "confidence": np.mean([p["confidence"] for p in stable_params]), } @@ -212,9 +210,7 @@ def run_benchmark(): avg_err_fixed = np.mean(final_errors_fixed) # Compute improvements - conv_improvement = ( - (avg_conv_fixed - avg_conv_adaptive) / avg_conv_fixed * 100 - ) + conv_improvement = (avg_conv_fixed - avg_conv_adaptive) / avg_conv_fixed * 100 error_improvement = (avg_err_fixed - avg_err_adaptive) / avg_err_fixed * 100 # Average detected parameters @@ -266,12 +262,8 @@ def run_benchmark(): print("=" * 70) print() - avg_conv_improvement = np.mean( - [r["conv_improvement"] for r in all_results.values()] - ) - avg_error_improvement = np.mean( - [r["error_improvement"] for r in all_results.values()] - ) + avg_conv_improvement = np.mean([r["conv_improvement"] for r in all_results.values()]) + avg_error_improvement = np.mean([r["error_improvement"] for r in all_results.values()]) print(f"Average Convergence Improvement: {avg_conv_improvement:+.1f}%") print(f"Average Error Improvement: {avg_error_improvement:+.1f}%") @@ -390,9 +382,7 @@ def plot_results(results: dict, target: np.ndarray): x = np.arange(len(metrics)) width = 0.35 - ax.bar( - x - width / 2, fixed_vals, width, label="Fixed", color="red", alpha=0.7 - ) + ax.bar(x - width / 2, fixed_vals, width, label="Fixed", color="red", alpha=0.7) ax.bar( x + width / 2, adaptive_vals, @@ -410,12 +400,8 @@ def plot_results(results: dict, target: np.ndarray): # Add value labels for i, (fv, av) in enumerate(zip(fixed_vals, adaptive_vals)): - ax.text( - i - width / 2, fv, f"{fv:.2f}", ha="center", va="bottom", fontsize=8 - ) - ax.text( - i + width / 2, av, f"{av:.2f}", ha="center", va="bottom", fontsize=8 - ) + ax.text(i - width / 2, fv, f"{fv:.2f}", ha="center", va="bottom", fontsize=8) + ax.text(i + width / 2, av, f"{av:.2f}", ha="center", va="bottom", fontsize=8) plt.tight_layout() plt.savefig("benchmark_dynamic_adaptation.png", dpi=150, bbox_inches="tight") diff --git a/benchmark_geodesic_planning.py b/benchmark_geodesic_planning.py index 6ec5eb8..05687f9 100644 --- a/benchmark_geodesic_planning.py +++ b/benchmark_geodesic_planning.py @@ -248,12 +248,8 @@ def run_benchmark(): print("=" * 70) print() - avg_success_improvement = np.mean( - [r["success_improvement"] for r in all_results.values()] - ) - avg_collision_reduction = np.mean( - [r["collision_reduction"] for r in all_results.values()] - ) + avg_success_improvement = np.mean([r["success_improvement"] for r in all_results.values()]) + avg_collision_reduction = np.mean([r["collision_reduction"] for r in all_results.values()]) print(f"Average Success Improvement: {avg_success_improvement:+.1f}%") print(f"Average Collision Reduction: {avg_collision_reduction:+.1f}%") @@ -307,15 +303,11 @@ def plot_results(results: dict): # Top row: Trajectories ax = axes[0, idx] - ax.set_title( - f"{scenario_name.replace('_', ' ').title()}", fontsize=12, fontweight="bold" - ) + ax.set_title(f"{scenario_name.replace('_', ' ').title()}", fontsize=12, fontweight="bold") # Plot obstacles for obs in obstacles: - circle = plt.Circle( - obs.position, obs.radius, color="red", alpha=0.3, label="Obstacle" - ) + circle = plt.Circle(obs.position, obs.radius, color="red", alpha=0.3, label="Obstacle") ax.add_patch(circle) # Plot trajectories @@ -362,9 +354,7 @@ def plot_results(results: dict): width = 0.35 ax.bar(x - width / 2, naive_vals, width, label="Naive", color="red", alpha=0.7) - ax.bar( - x + width / 2, geo_vals, width, label="Geodesic", color="blue", alpha=0.7 - ) + ax.bar(x + width / 2, geo_vals, width, label="Geodesic", color="blue", alpha=0.7) ax.set_ylabel("Value") ax.set_xticks(x) @@ -374,9 +364,7 @@ def plot_results(results: dict): # Add value labels for i, (nv, gv) in enumerate(zip(naive_vals, geo_vals)): - ax.text( - i - width / 2, nv, f"{nv:.1f}", ha="center", va="bottom", fontsize=8 - ) + ax.text(i - width / 2, nv, f"{nv:.1f}", ha="center", va="bottom", fontsize=8) ax.text(i + width / 2, gv, f"{gv:.1f}", ha="center", va="bottom", fontsize=8) plt.tight_layout() diff --git a/benchmark_landing.py b/benchmark_landing.py index f66b011..fcbdfd5 100644 --- a/benchmark_landing.py +++ b/benchmark_landing.py @@ -23,7 +23,8 @@ import numpy as np import sys -sys.path.insert(0, '/home/user/Eigen-Geometric-Control') + +sys.path.insert(0, "/home/user/Eigen-Geometric-Control") from src import ( detect_oscillation, @@ -31,7 +32,7 @@ forward_kinematics, compute_ds2, compute_gradient, - compute_change_stability + compute_change_stability, ) import pandas as pd @@ -77,19 +78,21 @@ def run_landing_baseline( C, S, ds2_CS = compute_change_stability(delta, 1e-3) # Record state - rows.append({ - 'tick': t, - 'theta1_rad': theta1, - 'theta2_rad': theta2, - 'x': x, - 'y': y, - 'target_x': target[0], - 'target_y': target[1], - 'distance_to_target': np.sqrt((x - target[0])**2 + (y - target[1])**2), - 'ds2_total': ds2_total, - 'grad_norm': grad_norm, - 'd_obs': components['d_obs'], - }) + rows.append( + { + "tick": t, + "theta1_rad": theta1, + "theta2_rad": theta2, + "x": x, + "y": y, + "target_x": target[0], + "target_y": target[1], + "distance_to_target": np.sqrt((x - target[0]) ** 2 + (y - target[1]) ** 2), + "ds2_total": ds2_total, + "grad_norm": grad_norm, + "d_obs": components["d_obs"], + } + ) theta1, theta2 = theta1_new, theta2_new @@ -155,22 +158,24 @@ def run_landing_lightlike( C, S, ds2_CS = compute_change_stability(delta, 1e-3) # Record state - rows.append({ - 'tick': t, - 'theta1_rad': theta1, - 'theta2_rad': theta2, - 'x': x, - 'y': y, - 'target_x': target[0], - 'target_y': target[1], - 'distance_to_target': np.sqrt((x - target[0])**2 + (y - target[1])**2), - 'ds2_total': ds2_total, - 'grad_norm': grad_norm, - 'd_obs': components['d_obs'], - 'oscillating': oscillating, - 'osc_strength': osc_strength, - 'damping': damping, - }) + rows.append( + { + "tick": t, + "theta1_rad": theta1, + "theta2_rad": theta2, + "x": x, + "y": y, + "target_x": target[0], + "target_y": target[1], + "distance_to_target": np.sqrt((x - target[0]) ** 2 + (y - target[1]) ** 2), + "ds2_total": ds2_total, + "grad_norm": grad_norm, + "d_obs": components["d_obs"], + "oscillating": oscillating, + "osc_strength": osc_strength, + "damping": damping, + } + ) theta1, theta2 = theta1_new, theta2_new @@ -180,7 +185,7 @@ def run_landing_lightlike( def analyze_landing(df, scenario_name): """Analyze landing performance with focus on final approach""" # Overall tracking - distances = df['distance_to_target'].values + distances = df["distance_to_target"].values # Final approach (last 20% of trajectory) final_approach_start = int(len(df) * 0.8) @@ -193,10 +198,10 @@ def analyze_landing(df, scenario_name): approach_variance = np.std(final_distances) * 1000 # mm # Count oscillations in final approach (critical phase) - grad_norms = df['grad_norm'].values[final_approach_start:] + grad_norms = df["grad_norm"].values[final_approach_start:] final_oscillations = 0 for i in range(1, len(grad_norms)): - if grad_norms[i] > grad_norms[i-1] * 1.4: + if grad_norms[i] > grad_norms[i - 1] * 1.4: final_oscillations += 1 # Time to reach landing zone (within 2cm) @@ -204,27 +209,27 @@ def analyze_landing(df, scenario_name): landing_zone_reached = np.argmax(within_zone) if within_zone.any() else len(df) # Overall oscillations - all_grad_norms = df['grad_norm'].values + all_grad_norms = df["grad_norm"].values total_oscillations = 0 for i in range(10, len(all_grad_norms)): - if all_grad_norms[i] > all_grad_norms[i-1] * 1.4: + if all_grad_norms[i] > all_grad_norms[i - 1] * 1.4: total_oscillations += 1 metrics = { - 'scenario': scenario_name, - 'final_landing_error_mm': final_error, - 'approach_stability_mm': approach_variance, - 'final_approach_oscillations': final_oscillations, - 'total_oscillations': total_oscillations, - 'time_to_landing_zone': landing_zone_reached, - 'mean_tracking_error_mm': distances.mean() * 1000, + "scenario": scenario_name, + "final_landing_error_mm": final_error, + "approach_stability_mm": approach_variance, + "final_approach_oscillations": final_oscillations, + "total_oscillations": total_oscillations, + "time_to_landing_zone": landing_zone_reached, + "mean_tracking_error_mm": distances.mean() * 1000, } - if 'damping' in df.columns: + if "damping" in df.columns: # Analyze damping during final approach - final_damping = df['damping'].iloc[final_approach_start:] - metrics['final_approach_damping_activations'] = (final_damping > 0).sum() - metrics['max_damping_final_approach'] = final_damping.max() + final_damping = df["damping"].iloc[final_approach_start:] + metrics["final_approach_damping_activations"] = (final_damping > 0).sum() + metrics["max_damping_final_approach"] = final_damping.max() return metrics @@ -242,56 +247,55 @@ def main(): # Scenario 1: Ship deck landing (oscillating platform - wave motion) print("Scenario 1: Ship deck landing (wave motion)...") + def ship_deck(t): # Oscillating landing pad (ship on waves) - return [1.3 + 0.03*np.sin(t*0.15), 0.4 + 0.02*np.cos(t*0.2)] + return [1.3 + 0.03 * np.sin(t * 0.15), 0.4 + 0.02 * np.cos(t * 0.2)] def obstacle_port_side(t): return [0.8, 0.3] # Ship superstructure df_base_1 = run_landing_baseline( - landing_trajectory=ship_deck, - obstacle_trajectory=obstacle_port_side, - n_ticks=180 + landing_trajectory=ship_deck, obstacle_trajectory=obstacle_port_side, n_ticks=180 ) df_light_1 = run_landing_lightlike( - landing_trajectory=ship_deck, - obstacle_trajectory=obstacle_port_side, - n_ticks=180 + landing_trajectory=ship_deck, obstacle_trajectory=obstacle_port_side, n_ticks=180 ) - results.append({ - 'baseline': analyze_landing(df_base_1, "Ship Deck - Baseline"), - 'lightlike': analyze_landing(df_light_1, "Ship Deck - Lightlike"), - }) + results.append( + { + "baseline": analyze_landing(df_base_1, "Ship Deck - Baseline"), + "lightlike": analyze_landing(df_light_1, "Ship Deck - Lightlike"), + } + ) # Scenario 2: Moving vehicle landing (truck bed, AGV) print("Scenario 2: Moving vehicle landing (truck/AGV)...") + def moving_vehicle(t): # Linear motion with small vibrations - return [1.2 + 0.003*t, 0.35 + 0.005*np.sin(t*0.5)] + return [1.2 + 0.003 * t, 0.35 + 0.005 * np.sin(t * 0.5)] def obstacle_fixed(t): return [0.7, 0.2] df_base_2 = run_landing_baseline( - landing_trajectory=moving_vehicle, - obstacle_trajectory=obstacle_fixed, - n_ticks=180 + landing_trajectory=moving_vehicle, obstacle_trajectory=obstacle_fixed, n_ticks=180 ) df_light_2 = run_landing_lightlike( - landing_trajectory=moving_vehicle, - obstacle_trajectory=obstacle_fixed, - n_ticks=180 + landing_trajectory=moving_vehicle, obstacle_trajectory=obstacle_fixed, n_ticks=180 ) - results.append({ - 'baseline': analyze_landing(df_base_2, "Moving Vehicle - Baseline"), - 'lightlike': analyze_landing(df_light_2, "Moving Vehicle - Lightlike"), - }) + results.append( + { + "baseline": analyze_landing(df_base_2, "Moving Vehicle - Baseline"), + "lightlike": analyze_landing(df_light_2, "Moving Vehicle - Lightlike"), + } + ) # Scenario 3: Precision landing (stationary but ultra-tight tolerance) print("Scenario 3: Precision landing (tight tolerance)...") + def precision_pad(t): return [1.35, 0.38] # Fixed position @@ -299,43 +303,45 @@ def precision_pad(t): landing_trajectory=precision_pad, obstacle_trajectory=obstacle_fixed, n_ticks=180, - eta=0.10 # Slower for precision + eta=0.10, # Slower for precision ) df_light_3 = run_landing_lightlike( - landing_trajectory=precision_pad, - obstacle_trajectory=obstacle_fixed, - n_ticks=180, - eta=0.10 + landing_trajectory=precision_pad, obstacle_trajectory=obstacle_fixed, n_ticks=180, eta=0.10 ) - results.append({ - 'baseline': analyze_landing(df_base_3, "Precision Landing - Baseline"), - 'lightlike': analyze_landing(df_light_3, "Precision Landing - Lightlike"), - }) + results.append( + { + "baseline": analyze_landing(df_base_3, "Precision Landing - Baseline"), + "lightlike": analyze_landing(df_light_3, "Precision Landing - Lightlike"), + } + ) # Scenario 4: Emergency descent (rapidly descending platform) print("Scenario 4: Emergency descent (fast approach)...") + def descending_platform(t): # Platform descending (emergency evacuation scenario) - return [1.3, 0.45 - 0.002*t] + return [1.3, 0.45 - 0.002 * t] df_base_4 = run_landing_baseline( landing_trajectory=descending_platform, obstacle_trajectory=obstacle_fixed, n_ticks=180, - eta=0.18 # Faster approach + eta=0.18, # Faster approach ) df_light_4 = run_landing_lightlike( landing_trajectory=descending_platform, obstacle_trajectory=obstacle_fixed, n_ticks=180, - eta=0.18 + eta=0.18, ) - results.append({ - 'baseline': analyze_landing(df_base_4, "Emergency Descent - Baseline"), - 'lightlike': analyze_landing(df_light_4, "Emergency Descent - Lightlike"), - }) + results.append( + { + "baseline": analyze_landing(df_base_4, "Emergency Descent - Baseline"), + "lightlike": analyze_landing(df_light_4, "Emergency Descent - Lightlike"), + } + ) # Scenario 5: Turbulent conditions (noisy target + disturbances) print("Scenario 5: Turbulent conditions (wind/noise)...") @@ -349,20 +355,18 @@ def turbulent_landing(t): return [base_x + noise_x[min(t, 179)], base_y + noise_y[min(t, 179)]] df_base_5 = run_landing_baseline( - landing_trajectory=turbulent_landing, - obstacle_trajectory=obstacle_fixed, - n_ticks=180 + landing_trajectory=turbulent_landing, obstacle_trajectory=obstacle_fixed, n_ticks=180 ) df_light_5 = run_landing_lightlike( - landing_trajectory=turbulent_landing, - obstacle_trajectory=obstacle_fixed, - n_ticks=180 + landing_trajectory=turbulent_landing, obstacle_trajectory=obstacle_fixed, n_ticks=180 ) - results.append({ - 'baseline': analyze_landing(df_base_5, "Turbulent - Baseline"), - 'lightlike': analyze_landing(df_light_5, "Turbulent - Lightlike"), - }) + results.append( + { + "baseline": analyze_landing(df_base_5, "Turbulent - Baseline"), + "lightlike": analyze_landing(df_light_5, "Turbulent - Lightlike"), + } + ) # Display results print() @@ -374,53 +378,69 @@ def turbulent_landing(t): print("Final Landing Error (mm) - MOST CRITICAL METRIC:") print("-" * 80) for r in results: - base = r['baseline'] - light = r['lightlike'] - improvement = (base['final_landing_error_mm'] - light['final_landing_error_mm']) / base['final_landing_error_mm'] * 100 + base = r["baseline"] + light = r["lightlike"] + improvement = ( + (base["final_landing_error_mm"] - light["final_landing_error_mm"]) + / base["final_landing_error_mm"] + * 100 + ) arrow = "✓" if improvement > 0 else "✗" - scenario = base['scenario'].replace(' - Baseline', '') - print(f"{scenario:<30} {base['final_landing_error_mm']:>7.1f}mm → {light['final_landing_error_mm']:>7.1f}mm " - f"{arrow} {abs(improvement):>6.1f}%") + scenario = base["scenario"].replace(" - Baseline", "") + print( + f"{scenario:<30} {base['final_landing_error_mm']:>7.1f}mm → {light['final_landing_error_mm']:>7.1f}mm " + f"{arrow} {abs(improvement):>6.1f}%" + ) print() print("Final Approach Stability (std dev in mm) - SAFETY CRITICAL:") print("-" * 80) for r in results: - base = r['baseline'] - light = r['lightlike'] - improvement = (base['approach_stability_mm'] - light['approach_stability_mm']) / base['approach_stability_mm'] * 100 + base = r["baseline"] + light = r["lightlike"] + improvement = ( + (base["approach_stability_mm"] - light["approach_stability_mm"]) + / base["approach_stability_mm"] + * 100 + ) arrow = "✓" if improvement > 0 else "✗" - scenario = base['scenario'].replace(' - Baseline', '') - print(f"{scenario:<30} {base['approach_stability_mm']:>7.1f}mm → {light['approach_stability_mm']:>7.1f}mm " - f"{arrow} {abs(improvement):>6.1f}%") + scenario = base["scenario"].replace(" - Baseline", "") + print( + f"{scenario:<30} {base['approach_stability_mm']:>7.1f}mm → {light['approach_stability_mm']:>7.1f}mm " + f"{arrow} {abs(improvement):>6.1f}%" + ) print() print("Final Approach Oscillations - CRASH RISK INDICATOR:") print("-" * 80) for r in results: - base = r['baseline'] - light = r['lightlike'] - reduction = base['final_approach_oscillations'] - light['final_approach_oscillations'] + base = r["baseline"] + light = r["lightlike"] + reduction = base["final_approach_oscillations"] - light["final_approach_oscillations"] status = "SAFER" if reduction > 0 else "SAME" if reduction == 0 else "WORSE" - scenario = base['scenario'].replace(' - Baseline', '') - print(f"{scenario:<30} {base['final_approach_oscillations']:>4} → {light['final_approach_oscillations']:>4} events " - f"{status:>6} ({reduction:+d})") + scenario = base["scenario"].replace(" - Baseline", "") + print( + f"{scenario:<30} {base['final_approach_oscillations']:>4} → {light['final_approach_oscillations']:>4} events " + f"{status:>6} ({reduction:+d})" + ) print() print("Time to Landing Zone (<2cm):") print("-" * 80) for r in results: - base = r['baseline'] - light = r['lightlike'] - change = light['time_to_landing_zone'] - base['time_to_landing_zone'] + base = r["baseline"] + light = r["lightlike"] + change = light["time_to_landing_zone"] - base["time_to_landing_zone"] faster = "faster" if change < 0 else "slower" - scenario = base['scenario'].replace(' - Baseline', '') - print(f"{scenario:<30} {base['time_to_landing_zone']:>4} → {light['time_to_landing_zone']:>4} ticks " - f"({abs(change):>3d} {faster})") + scenario = base["scenario"].replace(" - Baseline", "") + print( + f"{scenario:<30} {base['time_to_landing_zone']:>4} → {light['time_to_landing_zone']:>4} ticks " + f"({abs(change):>3d} {faster})" + ) print() print("=" * 80) @@ -430,19 +450,21 @@ def turbulent_landing(t): # Calculate critical metrics avg_landing_improvement = sum( - (r['baseline']['final_landing_error_mm'] - r['lightlike']['final_landing_error_mm']) / - r['baseline']['final_landing_error_mm'] * 100 + (r["baseline"]["final_landing_error_mm"] - r["lightlike"]["final_landing_error_mm"]) + / r["baseline"]["final_landing_error_mm"] + * 100 for r in results ) / len(results) avg_stability_improvement = sum( - (r['baseline']['approach_stability_mm'] - r['lightlike']['approach_stability_mm']) / - r['baseline']['approach_stability_mm'] * 100 + (r["baseline"]["approach_stability_mm"] - r["lightlike"]["approach_stability_mm"]) + / r["baseline"]["approach_stability_mm"] + * 100 for r in results ) / len(results) total_oscillation_reduction = sum( - r['baseline']['final_approach_oscillations'] - r['lightlike']['final_approach_oscillations'] + r["baseline"]["final_approach_oscillations"] - r["lightlike"]["final_approach_oscillations"] for r in results ) @@ -452,23 +474,33 @@ def turbulent_landing(t): print() # Best scenario - best_idx = max(range(len(results)), - key=lambda i: (results[i]['baseline']['final_landing_error_mm'] - - results[i]['lightlike']['final_landing_error_mm'])) + best_idx = max( + range(len(results)), + key=lambda i: ( + results[i]["baseline"]["final_landing_error_mm"] + - results[i]["lightlike"]["final_landing_error_mm"] + ), + ) best = results[best_idx] - best_improvement = (best['baseline']['final_landing_error_mm'] - - best['lightlike']['final_landing_error_mm']) / \ - best['baseline']['final_landing_error_mm'] * 100 + best_improvement = ( + (best["baseline"]["final_landing_error_mm"] - best["lightlike"]["final_landing_error_mm"]) + / best["baseline"]["final_landing_error_mm"] + * 100 + ) print(f"★ BEST PERFORMANCE: {best['baseline']['scenario'].replace(' - Baseline', '')}") print(f" Landing error improvement: {best_improvement:.1f}%") - print(f" Final approach oscillations reduced: {best['baseline']['final_approach_oscillations'] - best['lightlike']['final_approach_oscillations']} events") + print( + f" Final approach oscillations reduced: {best['baseline']['final_approach_oscillations'] - best['lightlike']['final_approach_oscillations']} events" + ) print() if avg_landing_improvement > 5.0 or total_oscillation_reduction > 5: print("★★★ CRITICAL BENEFIT FOR AUTONOMOUS LANDING ★★★") print() - print(f"The lightlike observer provides {avg_landing_improvement:.1f}% better landing precision") + print( + f"The lightlike observer provides {avg_landing_improvement:.1f}% better landing precision" + ) print(f"and {avg_stability_improvement:.1f}% more stable final approach.") print() print("For autonomous landing applications:") @@ -492,5 +524,5 @@ def turbulent_landing(t): print("=" * 80) -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/benchmark_mars_landing.py b/benchmark_mars_landing.py index 6c31de4..a773de6 100644 --- a/benchmark_mars_landing.py +++ b/benchmark_mars_landing.py @@ -25,7 +25,8 @@ import numpy as np import sys -sys.path.insert(0, '/home/user/Eigen-Geometric-Control') + +sys.path.insert(0, "/home/user/Eigen-Geometric-Control") from src import ( detect_oscillation, @@ -33,7 +34,7 @@ forward_kinematics, compute_ds2, compute_gradient, - compute_change_stability + compute_change_stability, ) import pandas as pd @@ -76,17 +77,19 @@ def run_mars_baseline( C, S, ds2_CS = compute_change_stability(delta, 1e-3) - rows.append({ - 'tick': t, - 'x': x, - 'y': y, - 'target_x': target[0], - 'target_y': target[1], - 'distance_to_target': np.sqrt((x - target[0])**2 + (y - target[1])**2), - 'ds2_total': ds2_total, - 'grad_norm': grad_norm, - 'd_obs': components['d_obs'], - }) + rows.append( + { + "tick": t, + "x": x, + "y": y, + "target_x": target[0], + "target_y": target[1], + "distance_to_target": np.sqrt((x - target[0]) ** 2 + (y - target[1]) ** 2), + "ds2_total": ds2_total, + "grad_norm": grad_norm, + "d_obs": components["d_obs"], + } + ) theta1, theta2 = theta1_new, theta2_new @@ -148,20 +151,22 @@ def run_mars_lightlike( C, S, ds2_CS = compute_change_stability(delta, 1e-3) - rows.append({ - 'tick': t, - 'x': x, - 'y': y, - 'target_x': target[0], - 'target_y': target[1], - 'distance_to_target': np.sqrt((x - target[0])**2 + (y - target[1])**2), - 'ds2_total': ds2_total, - 'grad_norm': grad_norm, - 'd_obs': components['d_obs'], - 'oscillating': oscillating, - 'osc_strength': osc_strength, - 'damping': damping, - }) + rows.append( + { + "tick": t, + "x": x, + "y": y, + "target_x": target[0], + "target_y": target[1], + "distance_to_target": np.sqrt((x - target[0]) ** 2 + (y - target[1]) ** 2), + "ds2_total": ds2_total, + "grad_norm": grad_norm, + "d_obs": components["d_obs"], + "oscillating": oscillating, + "osc_strength": osc_strength, + "damping": damping, + } + ) theta1, theta2 = theta1_new, theta2_new @@ -170,7 +175,7 @@ def run_mars_lightlike( def analyze_mars_landing(df, scenario_name, safe_zone_radius=0.015): """Analyze Mars landing with mission-critical metrics""" - distances = df['distance_to_target'].values + distances = df["distance_to_target"].values # Final landing precision (mission critical) final_error = distances[-1] * 1000 # mm @@ -184,15 +189,15 @@ def analyze_mars_landing(df, scenario_name, safe_zone_radius=0.015): descent_stability = np.std(final_descent_distances) * 1000 # Oscillation count during final descent (CRITICAL for Mars) - grad_norms = df['grad_norm'].values[final_descent_start:] + grad_norms = df["grad_norm"].values[final_descent_start:] descent_oscillations = 0 for i in range(1, len(grad_norms)): - if grad_norms[i] > grad_norms[i-1] * 1.5: # Significant gradient reversal + if grad_norms[i] > grad_norms[i - 1] * 1.5: # Significant gradient reversal descent_oscillations += 1 # Hazard clearance (minimum distance to obstacles) - min_clearance = df['d_obs'].min() - hazard_violations = (df['d_obs'] < 0.30).sum() + min_clearance = df["d_obs"].min() + hazard_violations = (df["d_obs"] < 0.30).sum() # Time to safe zone within_safe_zone = distances < safe_zone_radius @@ -202,22 +207,22 @@ def analyze_mars_landing(df, scenario_name, safe_zone_radius=0.015): mean_distance = distances.mean() * 1000 metrics = { - 'scenario': scenario_name, - 'final_landing_error_mm': final_error, - 'mission_success': mission_success, - 'safe_zone_radius_mm': safe_zone_radius * 1000, - 'descent_stability_mm': descent_stability, - 'descent_oscillations': descent_oscillations, - 'min_hazard_clearance_m': min_clearance, - 'hazard_violations': hazard_violations, - 'time_to_safe_zone': time_to_safe, - 'mean_tracking_error_mm': mean_distance, + "scenario": scenario_name, + "final_landing_error_mm": final_error, + "mission_success": mission_success, + "safe_zone_radius_mm": safe_zone_radius * 1000, + "descent_stability_mm": descent_stability, + "descent_oscillations": descent_oscillations, + "min_hazard_clearance_m": min_clearance, + "hazard_violations": hazard_violations, + "time_to_safe_zone": time_to_safe, + "mean_tracking_error_mm": mean_distance, } - if 'damping' in df.columns: - final_damping = df['damping'].iloc[final_descent_start:] - metrics['descent_damping_activations'] = (final_damping > 0).sum() - metrics['max_damping'] = df['damping'].max() + if "damping" in df.columns: + final_damping = df["damping"].iloc[final_descent_start:] + metrics["descent_damping_activations"] = (final_damping > 0).sum() + metrics["max_damping"] = df["damping"].max() return metrics @@ -239,28 +244,27 @@ def main(): # Scenario 1: Nominal Mars landing (like Perseverance) print("Scenario 1: Nominal Mars landing (clear weather)...") + def nominal_landing(t): # Slow descent to landing zone - return [1.35, 0.42 - 0.0005*t] + return [1.35, 0.42 - 0.0005 * t] def boulder_field(t): return [0.75, 0.25] # Large rock formation df_base_1 = run_mars_baseline( - landing_trajectory=nominal_landing, - obstacle_trajectory=boulder_field, - n_ticks=200 + landing_trajectory=nominal_landing, obstacle_trajectory=boulder_field, n_ticks=200 ) df_light_1 = run_mars_lightlike( - landing_trajectory=nominal_landing, - obstacle_trajectory=boulder_field, - n_ticks=200 + landing_trajectory=nominal_landing, obstacle_trajectory=boulder_field, n_ticks=200 ) - results.append({ - 'baseline': analyze_mars_landing(df_base_1, "Nominal - Baseline"), - 'lightlike': analyze_mars_landing(df_light_1, "Nominal - Lightlike"), - }) + results.append( + { + "baseline": analyze_mars_landing(df_base_1, "Nominal - Baseline"), + "lightlike": analyze_mars_landing(df_light_1, "Nominal - Lightlike"), + } + ) # Scenario 2: Atmospheric turbulence (dust storm effects) print("Scenario 2: Turbulent descent (dust storm effects)...") @@ -269,34 +273,33 @@ def boulder_field(t): turb_y = np.random.normal(0, 0.010, 200) def turbulent_descent(t): - base_x, base_y = 1.32, 0.38 - 0.0005*t + base_x, base_y = 1.32, 0.38 - 0.0005 * t return [base_x + turb_x[min(t, 199)], base_y + turb_y[min(t, 199)]] df_base_2 = run_mars_baseline( - landing_trajectory=turbulent_descent, - obstacle_trajectory=boulder_field, - n_ticks=200 + landing_trajectory=turbulent_descent, obstacle_trajectory=boulder_field, n_ticks=200 ) df_light_2 = run_mars_lightlike( - landing_trajectory=turbulent_descent, - obstacle_trajectory=boulder_field, - n_ticks=200 + landing_trajectory=turbulent_descent, obstacle_trajectory=boulder_field, n_ticks=200 ) - results.append({ - 'baseline': analyze_mars_landing(df_base_2, "Turbulent - Baseline"), - 'lightlike': analyze_mars_landing(df_light_2, "Turbulent - Lightlike"), - }) + results.append( + { + "baseline": analyze_mars_landing(df_base_2, "Turbulent - Baseline"), + "lightlike": analyze_mars_landing(df_light_2, "Turbulent - Lightlike"), + } + ) # Scenario 3: Mid-descent hazard avoidance (detected obstacle) print("Scenario 3: Hazard avoidance maneuver...") + def hazard_avoidance(t): # Detect hazard at t=80, shift landing site if t < 80: - return [1.35, 0.42 - 0.0005*t] + return [1.35, 0.42 - 0.0005 * t] else: # Shift 10cm to avoid detected boulder - return [1.45, 0.42 - 0.0005*t] + return [1.45, 0.42 - 0.0005 * t] def moving_hazard(t): # Hazard in original landing zone @@ -306,22 +309,22 @@ def moving_hazard(t): landing_trajectory=hazard_avoidance, obstacle_trajectory=moving_hazard, n_ticks=200, - Go=8.0 # Strong avoidance + Go=8.0, # Strong avoidance ) df_light_3 = run_mars_lightlike( - landing_trajectory=hazard_avoidance, - obstacle_trajectory=moving_hazard, - n_ticks=200, - Go=8.0 + landing_trajectory=hazard_avoidance, obstacle_trajectory=moving_hazard, n_ticks=200, Go=8.0 ) - results.append({ - 'baseline': analyze_mars_landing(df_base_3, "Hazard Avoidance - Baseline"), - 'lightlike': analyze_mars_landing(df_light_3, "Hazard Avoidance - Lightlike"), - }) + results.append( + { + "baseline": analyze_mars_landing(df_base_3, "Hazard Avoidance - Baseline"), + "lightlike": analyze_mars_landing(df_light_3, "Hazard Avoidance - Lightlike"), + } + ) # Scenario 4: Precision landing near hazard (tight constraints) print("Scenario 4: Precision landing near hazard...") + def precision_site(t): return [1.30, 0.38] # Fixed, small safe zone @@ -333,44 +336,57 @@ def nearby_hazard(t): obstacle_trajectory=nearby_hazard, n_ticks=200, eta=0.10, # Careful approach - Go=7.0 + Go=7.0, ) df_light_4 = run_mars_lightlike( landing_trajectory=precision_site, obstacle_trajectory=nearby_hazard, n_ticks=200, eta=0.10, - Go=7.0 + Go=7.0, ) - results.append({ - 'baseline': analyze_mars_landing(df_base_4, "Precision Near Hazard - Baseline", safe_zone_radius=0.010), - 'lightlike': analyze_mars_landing(df_light_4, "Precision Near Hazard - Lightlike", safe_zone_radius=0.010), - }) + results.append( + { + "baseline": analyze_mars_landing( + df_base_4, "Precision Near Hazard - Baseline", safe_zone_radius=0.010 + ), + "lightlike": analyze_mars_landing( + df_light_4, "Precision Near Hazard - Lightlike", safe_zone_radius=0.010 + ), + } + ) # Scenario 5: Sample return landing (must land on exact spot) print("Scenario 5: Sample return rendezvous (ultra-precision)...") + def sample_return_site(t): # Oscillating slightly (Mars Ascent Vehicle preparing for launch) - return [1.28 + 0.015*np.sin(t*0.1), 0.36 + 0.010*np.cos(t*0.15)] + return [1.28 + 0.015 * np.sin(t * 0.1), 0.36 + 0.010 * np.cos(t * 0.15)] df_base_5 = run_mars_baseline( landing_trajectory=sample_return_site, obstacle_trajectory=boulder_field, n_ticks=200, - eta=0.08 # Very careful + eta=0.08, # Very careful ) df_light_5 = run_mars_lightlike( landing_trajectory=sample_return_site, obstacle_trajectory=boulder_field, n_ticks=200, - eta=0.08 + eta=0.08, ) - results.append({ - 'baseline': analyze_mars_landing(df_base_5, "Sample Return - Baseline", safe_zone_radius=0.008), - 'lightlike': analyze_mars_landing(df_light_5, "Sample Return - Lightlike", safe_zone_radius=0.008), - }) + results.append( + { + "baseline": analyze_mars_landing( + df_base_5, "Sample Return - Baseline", safe_zone_radius=0.008 + ), + "lightlike": analyze_mars_landing( + df_light_5, "Sample Return - Lightlike", safe_zone_radius=0.008 + ), + } + ) # Display results print() @@ -382,45 +398,61 @@ def sample_return_site(t): print("Final Landing Precision (mm) - MISSION CRITICAL:") print("-" * 80) for r in results: - base = r['baseline'] - light = r['lightlike'] - improvement = (base['final_landing_error_mm'] - light['final_landing_error_mm']) / base['final_landing_error_mm'] * 100 + base = r["baseline"] + light = r["lightlike"] + improvement = ( + (base["final_landing_error_mm"] - light["final_landing_error_mm"]) + / base["final_landing_error_mm"] + * 100 + ) - base_success = "✓ SUCCESS" if base['mission_success'] else "✗ FAIL" - light_success = "✓ SUCCESS" if light['mission_success'] else "✗ FAIL" + base_success = "✓ SUCCESS" if base["mission_success"] else "✗ FAIL" + light_success = "✓ SUCCESS" if light["mission_success"] else "✗ FAIL" - scenario = base['scenario'].replace(' - Baseline', '') - safe_zone = base['safe_zone_radius_mm'] + scenario = base["scenario"].replace(" - Baseline", "") + safe_zone = base["safe_zone_radius_mm"] print(f"{scenario:<30}") - print(f" Baseline: {base['final_landing_error_mm']:>6.1f}mm {base_success} (safe zone: <{safe_zone:.0f}mm)") - print(f" Lightlike: {light['final_landing_error_mm']:>6.1f}mm {light_success} ({improvement:+.1f}%)") + print( + f" Baseline: {base['final_landing_error_mm']:>6.1f}mm {base_success} (safe zone: <{safe_zone:.0f}mm)" + ) + print( + f" Lightlike: {light['final_landing_error_mm']:>6.1f}mm {light_success} ({improvement:+.1f}%)" + ) print() print("Descent Stability (oscillations during final 30%):") print("-" * 80) for r in results: - base = r['baseline'] - light = r['lightlike'] - reduction = base['descent_oscillations'] - light['descent_oscillations'] + base = r["baseline"] + light = r["lightlike"] + reduction = base["descent_oscillations"] - light["descent_oscillations"] status = "SAFER" if reduction > 0 else "SAME" if reduction == 0 else "MORE RISK" - scenario = base['scenario'].replace(' - Baseline', '') - print(f"{scenario:<30} {base['descent_oscillations']:>3} → {light['descent_oscillations']:>3} oscillations " - f"{status:>10} ({reduction:+d})") + scenario = base["scenario"].replace(" - Baseline", "") + print( + f"{scenario:<30} {base['descent_oscillations']:>3} → {light['descent_oscillations']:>3} oscillations " + f"{status:>10} ({reduction:+d})" + ) print() print("Descent Smoothness (std dev in mm):") print("-" * 80) for r in results: - base = r['baseline'] - light = r['lightlike'] - improvement = (base['descent_stability_mm'] - light['descent_stability_mm']) / base['descent_stability_mm'] * 100 + base = r["baseline"] + light = r["lightlike"] + improvement = ( + (base["descent_stability_mm"] - light["descent_stability_mm"]) + / base["descent_stability_mm"] + * 100 + ) arrow = "↓" if improvement > 0 else "↑" - scenario = base['scenario'].replace(' - Baseline', '') - print(f"{scenario:<30} {base['descent_stability_mm']:>7.1f}mm → {light['descent_stability_mm']:>7.1f}mm " - f"{arrow} {abs(improvement):>5.1f}%") + scenario = base["scenario"].replace(" - Baseline", "") + print( + f"{scenario:<30} {base['descent_stability_mm']:>7.1f}mm → {light['descent_stability_mm']:>7.1f}mm " + f"{arrow} {abs(improvement):>5.1f}%" + ) print() print("=" * 80) @@ -429,29 +461,35 @@ def sample_return_site(t): print() # Mission success rates - baseline_successes = sum(1 for r in results if r['baseline']['mission_success']) - lightlike_successes = sum(1 for r in results if r['lightlike']['mission_success']) + baseline_successes = sum(1 for r in results if r["baseline"]["mission_success"]) + lightlike_successes = sum(1 for r in results if r["lightlike"]["mission_success"]) print(f"MISSION SUCCESS RATE:") - print(f" Baseline: {baseline_successes}/{len(results)} missions successful ({baseline_successes/len(results)*100:.0f}%)") - print(f" Lightlike: {lightlike_successes}/{len(results)} missions successful ({lightlike_successes/len(results)*100:.0f}%)") + print( + f" Baseline: {baseline_successes}/{len(results)} missions successful ({baseline_successes/len(results)*100:.0f}%)" + ) + print( + f" Lightlike: {lightlike_successes}/{len(results)} missions successful ({lightlike_successes/len(results)*100:.0f}%)" + ) print() # Average metrics avg_precision_improvement = sum( - (r['baseline']['final_landing_error_mm'] - r['lightlike']['final_landing_error_mm']) / - r['baseline']['final_landing_error_mm'] * 100 + (r["baseline"]["final_landing_error_mm"] - r["lightlike"]["final_landing_error_mm"]) + / r["baseline"]["final_landing_error_mm"] + * 100 for r in results ) / len(results) total_oscillation_reduction = sum( - r['baseline']['descent_oscillations'] - r['lightlike']['descent_oscillations'] + r["baseline"]["descent_oscillations"] - r["lightlike"]["descent_oscillations"] for r in results ) avg_stability_improvement = sum( - (r['baseline']['descent_stability_mm'] - r['lightlike']['descent_stability_mm']) / - r['baseline']['descent_stability_mm'] * 100 + (r["baseline"]["descent_stability_mm"] - r["lightlike"]["descent_stability_mm"]) + / r["baseline"]["descent_stability_mm"] + * 100 for r in results ) / len(results) @@ -496,23 +534,36 @@ def sample_return_site(t): # Best scenario analysis if len(results) > 0: - best_idx = max(range(len(results)), - key=lambda i: (results[i]['baseline']['final_landing_error_mm'] - - results[i]['lightlike']['final_landing_error_mm'])) + best_idx = max( + range(len(results)), + key=lambda i: ( + results[i]["baseline"]["final_landing_error_mm"] + - results[i]["lightlike"]["final_landing_error_mm"] + ), + ) best = results[best_idx] - best_improvement = (best['baseline']['final_landing_error_mm'] - - best['lightlike']['final_landing_error_mm']) / \ - best['baseline']['final_landing_error_mm'] * 100 + best_improvement = ( + ( + best["baseline"]["final_landing_error_mm"] + - best["lightlike"]["final_landing_error_mm"] + ) + / best["baseline"]["final_landing_error_mm"] + * 100 + ) print() print(f"BEST PERFORMANCE: {best['baseline']['scenario'].replace(' - Baseline', '')}") print(f" Precision improvement: {best_improvement:+.1f}%") - print(f" Oscillations reduced: {best['baseline']['descent_oscillations'] - best['lightlike']['descent_oscillations']} events") + print( + f" Oscillations reduced: {best['baseline']['descent_oscillations'] - best['lightlike']['descent_oscillations']} events" + ) - if 'descent_damping_activations' in best['lightlike']: - print(f" Damping activations during descent: {best['lightlike']['descent_damping_activations']}") + if "descent_damping_activations" in best["lightlike"]: + print( + f" Damping activations during descent: {best['lightlike']['descent_damping_activations']}" + ) print(f" Max damping: {best['lightlike']['max_damping']:.3f}") -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/benchmark_moving_targets.py b/benchmark_moving_targets.py index 2123af5..98753c8 100644 --- a/benchmark_moving_targets.py +++ b/benchmark_moving_targets.py @@ -111,10 +111,12 @@ def generate_moving_target_trajectory( elif trajectory_type == "accelerating": # Accelerating target accel = 0.05 - pos = initial_position + np.array([ - velocity * t + 0.5 * accel * t**2, - velocity * t * 0.3, - ]) + pos = initial_position + np.array( + [ + velocity * t + 0.5 * accel * t**2, + velocity * t * 0.3, + ] + ) else: raise ValueError(f"Unknown trajectory type: {trajectory_type}") diff --git a/benchmark_qec.py b/benchmark_qec.py index f3b4893..a8b3bda 100644 --- a/benchmark_qec.py +++ b/benchmark_qec.py @@ -211,9 +211,7 @@ def run_benchmark(): print() avg_error_improvement = np.mean([r["error_improvement"] for r in all_results.values()]) - avg_success_improvement = np.mean( - [r["success_improvement"] for r in all_results.values()] - ) + avg_success_improvement = np.mean([r["success_improvement"] for r in all_results.values()]) print(f"Average Error Improvement: {avg_error_improvement:+.1f}%") print(f"Average Success Improvement: {avg_success_improvement:+.1f}%") @@ -289,12 +287,8 @@ def plot_results(results: dict, start: np.ndarray, goal: np.ndarray): ) # Mark start and goal - ax.scatter( - start[0], start[1], s=150, marker="*", color="green", label="Start", zorder=5 - ) - ax.scatter( - goal[0], goal[1], s=150, marker="X", color="red", label="Goal", zorder=5 - ) + ax.scatter(start[0], start[1], s=150, marker="*", color="green", label="Start", zorder=5) + ax.scatter(goal[0], goal[1], s=150, marker="X", color="red", label="Goal", zorder=5) ax.set_xlabel("X Position") ax.set_ylabel("Y Position") @@ -327,9 +321,7 @@ def plot_results(results: dict, start: np.ndarray, goal: np.ndarray): # Add value labels for i, (nv, qv) in enumerate(zip(naive_vals, qec_vals)): - ax.text( - i - width / 2, nv, f"{nv:.1f}", ha="center", va="bottom", fontsize=8 - ) + ax.text(i - width / 2, nv, f"{nv:.1f}", ha="center", va="bottom", fontsize=8) ax.text(i + width / 2, qv, f"{qv:.1f}", ha="center", va="bottom", fontsize=8) plt.tight_layout() diff --git a/benchmark_scaling.py b/benchmark_scaling.py index 5baf3ad..83ab4f4 100644 --- a/benchmark_scaling.py +++ b/benchmark_scaling.py @@ -15,7 +15,8 @@ import numpy as np import sys -sys.path.insert(0, '/home/user/Eigen-Geometric-Control') + +sys.path.insert(0, "/home/user/Eigen-Geometric-Control") from src import ( detect_oscillation, @@ -23,7 +24,7 @@ forward_kinematics, compute_ds2, compute_gradient, - compute_change_stability + compute_change_stability, ) import pandas as pd @@ -42,7 +43,7 @@ def run_arm_with_tuned_lightlike( L2=0.9, window=5, # Tuned: longer window = less sensitive threshold=0.98, # Tuned: higher threshold = only strong oscillations - damping_scale=0.5 # Tuned: reduce damping strength + damping_scale=0.5, # Tuned: reduce damping strength ): """Run arm simulation WITH TUNED lightlike observer parameters""" theta1, theta2 = theta_init @@ -88,19 +89,21 @@ def run_arm_with_tuned_lightlike( C, S, ds2_CS = compute_change_stability(delta, eps_change) # Record state - rows.append({ - 'tick': t, - 'theta1_rad': theta1, - 'theta2_rad': theta2, - 'x': x, - 'y': y, - 'ds2_total': ds2_total, - 'grad_norm': grad_norm, - 'd_obs': components['d_obs'], - 'oscillating': oscillating, - 'osc_strength': osc_strength, - 'damping': damping, - }) + rows.append( + { + "tick": t, + "theta1_rad": theta1, + "theta2_rad": theta2, + "x": x, + "y": y, + "ds2_total": ds2_total, + "grad_norm": grad_norm, + "d_obs": components["d_obs"], + "oscillating": oscillating, + "osc_strength": osc_strength, + "damping": damping, + } + ) # Update for next iteration theta1, theta2 = theta1_new, theta2_new @@ -122,6 +125,7 @@ def run_arm_baseline( ): """Run baseline without lightlike observer (for fair comparison)""" from src import run_arm_simulation + df = run_arm_simulation( theta_init=theta_init, target=target, @@ -132,7 +136,7 @@ def run_arm_baseline( Go=Go, lam=lam, L1=L1, - L2=L2 + L2=L2, ) return df @@ -140,40 +144,45 @@ def run_arm_baseline( def analyze_precision(df_baseline, df_lightlike, target, scenario_name): """Analyze accuracy and convergence""" final_dist_base = np.sqrt( - (df_baseline['x'].iloc[-1] - target[0])**2 + - (df_baseline['y'].iloc[-1] - target[1])**2 + (df_baseline["x"].iloc[-1] - target[0]) ** 2 + (df_baseline["y"].iloc[-1] - target[1]) ** 2 ) final_dist_light = np.sqrt( - (df_lightlike['x'].iloc[-1] - target[0])**2 + - (df_lightlike['y'].iloc[-1] - target[1])**2 + (df_lightlike["x"].iloc[-1] - target[0]) ** 2 + + (df_lightlike["y"].iloc[-1] - target[1]) ** 2 ) # How many ticks to reach 1cm accuracy? target_dist_base = np.sqrt( - (df_baseline['x'] - target[0])**2 + (df_baseline['y'] - target[1])**2 + (df_baseline["x"] - target[0]) ** 2 + (df_baseline["y"] - target[1]) ** 2 ) target_dist_light = np.sqrt( - (df_lightlike['x'] - target[0])**2 + (df_lightlike['y'] - target[1])**2 + (df_lightlike["x"] - target[0]) ** 2 + (df_lightlike["y"] - target[1]) ** 2 ) - ticks_base = (target_dist_base < 0.01).idxmax() if (target_dist_base < 0.01).any() else len(df_baseline) - ticks_light = (target_dist_light < 0.01).idxmax() if (target_dist_light < 0.01).any() else len(df_lightlike) + ticks_base = ( + (target_dist_base < 0.01).idxmax() if (target_dist_base < 0.01).any() else len(df_baseline) + ) + ticks_light = ( + (target_dist_light < 0.01).idxmax() + if (target_dist_light < 0.01).any() + else len(df_lightlike) + ) improvement = (final_dist_base - final_dist_light) / final_dist_base * 100 speed_change = (ticks_light - ticks_base) / ticks_base * 100 if ticks_base > 0 else 0 # Lightlike activations - activations = (df_lightlike['damping'] > 0).sum() if 'damping' in df_lightlike.columns else 0 + activations = (df_lightlike["damping"] > 0).sum() if "damping" in df_lightlike.columns else 0 return { - 'scenario': scenario_name, - 'final_dist_baseline_mm': final_dist_base * 1000, - 'final_dist_lightlike_mm': final_dist_light * 1000, - 'accuracy_improvement_pct': improvement, - 'ticks_to_1cm_baseline': ticks_base, - 'ticks_to_1cm_lightlike': ticks_light, - 'speed_change_pct': speed_change, - 'lightlike_activations': activations, + "scenario": scenario_name, + "final_dist_baseline_mm": final_dist_base * 1000, + "final_dist_lightlike_mm": final_dist_light * 1000, + "accuracy_improvement_pct": improvement, + "ticks_to_1cm_baseline": ticks_base, + "ticks_to_1cm_lightlike": ticks_light, + "speed_change_pct": speed_change, + "lightlike_activations": activations, } @@ -198,7 +207,9 @@ def main(): # Scenario 2: Tighter target (precision assembly) print("Scenario 2: High-precision task (assembly)...") target_precise = np.array([1.15, 0.35]) - df_base_2 = run_arm_baseline(target=tuple(target_precise), eta=0.08) # Smaller steps for precision + df_base_2 = run_arm_baseline( + target=tuple(target_precise), eta=0.08 + ) # Smaller steps for precision df_light_2 = run_arm_with_tuned_lightlike(target=tuple(target_precise), eta=0.08) results.append(analyze_precision(df_base_2, df_light_2, target_precise, "2-DOF Precision")) @@ -218,9 +229,7 @@ def main(): # Scenario 5: Long-range motion print("Scenario 5: Long-range motion...") target_far = np.array([1.5, 0.5]) - df_base_5 = run_arm_baseline( - theta_init=(-2.0, 2.0), target=tuple(target_far), n_ticks=200 - ) + df_base_5 = run_arm_baseline(theta_init=(-2.0, 2.0), target=tuple(target_far), n_ticks=200) df_light_5 = run_arm_with_tuned_lightlike( theta_init=(-2.0, 2.0), target=tuple(target_far), n_ticks=200 ) @@ -238,31 +247,35 @@ def main(): print("Final Accuracy (distance to target in mm):") print("-" * 80) for _, row in df_results.iterrows(): - baseline_mm = row['final_dist_baseline_mm'] - lightlike_mm = row['final_dist_lightlike_mm'] - improvement = row['accuracy_improvement_pct'] + baseline_mm = row["final_dist_baseline_mm"] + lightlike_mm = row["final_dist_lightlike_mm"] + improvement = row["accuracy_improvement_pct"] arrow = "↓" if improvement > 0 else "↑" - print(f"{row['scenario']:<25} {baseline_mm:>8.2f} mm → {lightlike_mm:>8.2f} mm " - f"{arrow} {abs(improvement):>5.1f}%") + print( + f"{row['scenario']:<25} {baseline_mm:>8.2f} mm → {lightlike_mm:>8.2f} mm " + f"{arrow} {abs(improvement):>5.1f}%" + ) print() print("Convergence Speed (ticks to reach 1cm accuracy):") print("-" * 80) for _, row in df_results.iterrows(): - base_ticks = row['ticks_to_1cm_baseline'] - light_ticks = row['ticks_to_1cm_lightlike'] - change = row['speed_change_pct'] + base_ticks = row["ticks_to_1cm_baseline"] + light_ticks = row["ticks_to_1cm_lightlike"] + change = row["speed_change_pct"] arrow = "faster" if change < 0 else "slower" - print(f"{row['scenario']:<25} {base_ticks:>4} → {light_ticks:>4} ticks " - f"({abs(change):>5.1f}% {arrow})") + print( + f"{row['scenario']:<25} {base_ticks:>4} → {light_ticks:>4} ticks " + f"({abs(change):>5.1f}% {arrow})" + ) print() print("Lightlike Observer Activations:") print("-" * 80) for _, row in df_results.iterrows(): - activations = row['lightlike_activations'] + activations = row["lightlike_activations"] print(f"{row['scenario']:<25} {activations:>4} activations") print() @@ -272,11 +285,13 @@ def main(): print() # Analyze trends - avg_improvement = df_results['accuracy_improvement_pct'].mean() - best_scenario = df_results.loc[df_results['accuracy_improvement_pct'].idxmax()] + avg_improvement = df_results["accuracy_improvement_pct"].mean() + best_scenario = df_results.loc[df_results["accuracy_improvement_pct"].idxmax()] print(f"Average accuracy improvement: {avg_improvement:+.2f}%") - print(f"Best scenario: {best_scenario['scenario']} ({best_scenario['accuracy_improvement_pct']:+.1f}%)") + print( + f"Best scenario: {best_scenario['scenario']} ({best_scenario['accuracy_improvement_pct']:+.1f}%)" + ) print() if avg_improvement > 0.5: @@ -284,12 +299,18 @@ def main(): print() print("Key insights:") print(f" • Average improvement: {avg_improvement:.1f}%") - print(f" • Best case: {best_scenario['accuracy_improvement_pct']:.1f}% in {best_scenario['scenario']}") + print( + f" • Best case: {best_scenario['accuracy_improvement_pct']:.1f}% in {best_scenario['scenario']}" + ) print() print("For 3D robot control (6-DOF or 7-DOF):") print(f" • Configuration space: 2D → 6D or 7D (3-3.5x more dimensions)") - print(f" • Expected improvement: ~{avg_improvement * 3:.1f}% to {avg_improvement * 3.5:.1f}%") - print(f" • In millimeters: {avg_improvement * 3 / 100 * 50:.1f}mm to {avg_improvement * 3.5 / 100 * 50:.1f}mm on 5cm tolerance") + print( + f" • Expected improvement: ~{avg_improvement * 3:.1f}% to {avg_improvement * 3.5:.1f}%" + ) + print( + f" • In millimeters: {avg_improvement * 3 / 100 * 50:.1f}mm to {avg_improvement * 3.5 / 100 * 50:.1f}mm on 5cm tolerance" + ) print() print(" This could be the difference between:") print(" - Successful vs failed grasp") @@ -309,5 +330,5 @@ def main(): return df_results -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/benchmark_stereo_lorentz.py b/benchmark_stereo_lorentz.py index a423780..f6be1ba 100644 --- a/benchmark_stereo_lorentz.py +++ b/benchmark_stereo_lorentz.py @@ -20,7 +20,8 @@ import numpy as np import sys -sys.path.insert(0, '/home/user/Eigen-Geometric-Control') + +sys.path.insert(0, "/home/user/Eigen-Geometric-Control") from src import ( detect_oscillation, @@ -95,13 +96,11 @@ def run_stereo_lorentz_baseline( x, y = forward_kinematics(theta1, theta2, L1, L2) ds2_total, components = compute_ds2( - theta1, theta2, target, obstacle_center, - obstacle_radius, Go, lam, L1, L2 + theta1, theta2, target, obstacle_center, obstacle_radius, Go, lam, L1, L2 ) grad, grad_norm = compute_gradient( - theta1, theta2, target, obstacle_center, - obstacle_radius, Go, lam, L1, L2 + theta1, theta2, target, obstacle_center, obstacle_radius, Go, lam, L1, L2 ) delta = -eta * grad @@ -110,22 +109,24 @@ def run_stereo_lorentz_baseline( C, S, ds2_CS = compute_change_stability(delta, 1e-3) - distance = np.sqrt((x - target[0])**2 + (y - target[1])**2) - - rows.append({ - 'tick': t, - 'x': x, - 'y': y, - 'distance': distance, - 'stereo_left': left, - 'stereo_right': right, - 't_lorentz': t_lorentz, - 'x_lorentz': x_lorentz, - 'ds2_lorentz': ds2_lorentz, - 'lorentz_regime': regime, - 'grad_norm': grad_norm, - 'd_obs': components['d_obs'], - }) + distance = np.sqrt((x - target[0]) ** 2 + (y - target[1]) ** 2) + + rows.append( + { + "tick": t, + "x": x, + "y": y, + "distance": distance, + "stereo_left": left, + "stereo_right": right, + "t_lorentz": t_lorentz, + "x_lorentz": x_lorentz, + "ds2_lorentz": ds2_lorentz, + "lorentz_regime": regime, + "grad_norm": grad_norm, + "d_obs": components["d_obs"], + } + ) theta1, theta2 = theta1_new, theta2_new @@ -178,13 +179,11 @@ def run_stereo_lorentz_lightlike( state_history.append(robot_state) ds2_total, components = compute_ds2( - theta1, theta2, target, obstacle_center, - obstacle_radius, Go, lam, L1, L2 + theta1, theta2, target, obstacle_center, obstacle_radius, Go, lam, L1, L2 ) grad, grad_norm = compute_gradient( - theta1, theta2, target, obstacle_center, - obstacle_radius, Go, lam, L1, L2 + theta1, theta2, target, obstacle_center, obstacle_radius, Go, lam, L1, L2 ) # LIGHTLIKE OBSERVER IN LORENTZ SPACE @@ -208,25 +207,27 @@ def run_stereo_lorentz_lightlike( C, S, ds2_CS = compute_change_stability(delta, 1e-3) - distance = np.sqrt((x - target[0])**2 + (y - target[1])**2) - - rows.append({ - 'tick': t, - 'x': x, - 'y': y, - 'distance': distance, - 'stereo_left': left, - 'stereo_right': right, - 't_lorentz': t_lorentz, - 'x_lorentz': x_lorentz, - 'ds2_lorentz': ds2_lorentz, - 'lorentz_regime': regime, - 'grad_norm': grad_norm, - 'd_obs': components['d_obs'], - 'oscillating': oscillating, - 'osc_strength': osc_strength, - 'damping': damping, - }) + distance = np.sqrt((x - target[0]) ** 2 + (y - target[1]) ** 2) + + rows.append( + { + "tick": t, + "x": x, + "y": y, + "distance": distance, + "stereo_left": left, + "stereo_right": right, + "t_lorentz": t_lorentz, + "x_lorentz": x_lorentz, + "ds2_lorentz": ds2_lorentz, + "lorentz_regime": regime, + "grad_norm": grad_norm, + "d_obs": components["d_obs"], + "oscillating": oscillating, + "osc_strength": osc_strength, + "damping": damping, + } + ) theta1, theta2 = theta1_new, theta2_new @@ -235,44 +236,44 @@ def run_stereo_lorentz_lightlike( def analyze_lorentz_control(df, scenario_name): """Analyze control with Lorentz stereo integration""" - final_error = df['distance'].iloc[-1] * 1000 # mm - mean_error = df['distance'].mean() * 1000 - tracking_variance = np.std(df['distance'].values) * 1000 + final_error = df["distance"].iloc[-1] * 1000 # mm + mean_error = df["distance"].mean() * 1000 + tracking_variance = np.std(df["distance"].values) * 1000 # Lorentz regime stability regime_changes = sum( - 1 for i in range(1, len(df)) - if df['lorentz_regime'].iloc[i] != df['lorentz_regime'].iloc[i-1] + 1 + for i in range(1, len(df)) + if df["lorentz_regime"].iloc[i] != df["lorentz_regime"].iloc[i - 1] ) # Stereo disparity variance (measure of sensor instability) - disparities = np.abs(df['stereo_left'].values - df['stereo_right'].values) + disparities = np.abs(df["stereo_left"].values - df["stereo_right"].values) disparity_variance = np.std(disparities) # Control oscillations - grad_norms = df['grad_norm'].values + grad_norms = df["grad_norm"].values oscillations = sum( - 1 for i in range(10, len(grad_norms)) - if grad_norms[i] > grad_norms[i-1] * 1.5 + 1 for i in range(10, len(grad_norms)) if grad_norms[i] > grad_norms[i - 1] * 1.5 ) metrics = { - 'scenario': scenario_name, - 'final_error_mm': final_error, - 'mean_error_mm': mean_error, - 'tracking_variance_mm': tracking_variance, - 'regime_changes': regime_changes, - 'disparity_variance': disparity_variance, - 'oscillations': oscillations, + "scenario": scenario_name, + "final_error_mm": final_error, + "mean_error_mm": mean_error, + "tracking_variance_mm": tracking_variance, + "regime_changes": regime_changes, + "disparity_variance": disparity_variance, + "oscillations": oscillations, } - if 'damping' in df.columns: - metrics['damping_activations'] = (df['damping'] > 0).sum() - metrics['max_damping'] = df['damping'].max() + if "damping" in df.columns: + metrics["damping_activations"] = (df["damping"] > 0).sum() + metrics["max_damping"] = df["damping"].max() # Lorentz-space oscillations detected - lorentz_osc_detected = (df['oscillating']).sum() - metrics['lorentz_oscillations_detected'] = lorentz_osc_detected + lorentz_osc_detected = (df["oscillating"]).sum() + metrics["lorentz_oscillations_detected"] = lorentz_osc_detected return metrics @@ -302,21 +303,17 @@ def main(): for name, noise in noise_levels: print(f"Testing {name} (noise={noise})...") - df_base = run_stereo_lorentz_baseline( - stereo_noise=noise, - n_ticks=180 - ) + df_base = run_stereo_lorentz_baseline(stereo_noise=noise, n_ticks=180) - df_light = run_stereo_lorentz_lightlike( - stereo_noise=noise, - n_ticks=180 - ) + df_light = run_stereo_lorentz_lightlike(stereo_noise=noise, n_ticks=180) - results.append({ - 'baseline': analyze_lorentz_control(df_base, f"{name} - Baseline"), - 'lightlike': analyze_lorentz_control(df_light, f"{name} - Lightlike"), - 'noise_level': noise, - }) + results.append( + { + "baseline": analyze_lorentz_control(df_base, f"{name} - Baseline"), + "lightlike": analyze_lorentz_control(df_light, f"{name} - Lightlike"), + "noise_level": noise, + } + ) # Display results print() @@ -328,53 +325,67 @@ def main(): print("Final Tracking Error (mm):") print("-" * 80) for r in results: - base = r['baseline'] - light = r['lightlike'] - improvement = (base['final_error_mm'] - light['final_error_mm']) / base['final_error_mm'] * 100 + base = r["baseline"] + light = r["lightlike"] + improvement = ( + (base["final_error_mm"] - light["final_error_mm"]) / base["final_error_mm"] * 100 + ) arrow = "✓" if improvement > 0 else "✗" - scenario = base['scenario'].replace(' - Baseline', '') - print(f"{scenario:<30} {base['final_error_mm']:>7.1f}mm → {light['final_error_mm']:>7.1f}mm " - f"{arrow} {abs(improvement):>5.1f}%") + scenario = base["scenario"].replace(" - Baseline", "") + print( + f"{scenario:<30} {base['final_error_mm']:>7.1f}mm → {light['final_error_mm']:>7.1f}mm " + f"{arrow} {abs(improvement):>5.1f}%" + ) print() print("Tracking Stability (std dev mm):") print("-" * 80) for r in results: - base = r['baseline'] - light = r['lightlike'] - improvement = (base['tracking_variance_mm'] - light['tracking_variance_mm']) / base['tracking_variance_mm'] * 100 + base = r["baseline"] + light = r["lightlike"] + improvement = ( + (base["tracking_variance_mm"] - light["tracking_variance_mm"]) + / base["tracking_variance_mm"] + * 100 + ) arrow = "↓" if improvement > 0 else "↑" - scenario = base['scenario'].replace(' - Baseline', '') - print(f"{scenario:<30} {base['tracking_variance_mm']:>7.1f}mm → {light['tracking_variance_mm']:>7.1f}mm " - f"{arrow} {abs(improvement):>5.1f}%") + scenario = base["scenario"].replace(" - Baseline", "") + print( + f"{scenario:<30} {base['tracking_variance_mm']:>7.1f}mm → {light['tracking_variance_mm']:>7.1f}mm " + f"{arrow} {abs(improvement):>5.1f}%" + ) print() print("Lorentz Regime Stability (fewer changes = better):") print("-" * 80) for r in results: - base = r['baseline'] - light = r['lightlike'] - reduction = base['regime_changes'] - light['regime_changes'] + base = r["baseline"] + light = r["lightlike"] + reduction = base["regime_changes"] - light["regime_changes"] status = "BETTER" if reduction > 0 else "SAME" if reduction == 0 else "WORSE" - scenario = base['scenario'].replace(' - Baseline', '') - print(f"{scenario:<30} {base['regime_changes']:>4} → {light['regime_changes']:>4} changes " - f"{status:>6} ({reduction:+d})") + scenario = base["scenario"].replace(" - Baseline", "") + print( + f"{scenario:<30} {base['regime_changes']:>4} → {light['regime_changes']:>4} changes " + f"{status:>6} ({reduction:+d})" + ) print() print("Control Oscillations:") print("-" * 80) for r in results: - base = r['baseline'] - light = r['lightlike'] - reduction = base['oscillations'] - light['oscillations'] + base = r["baseline"] + light = r["lightlike"] + reduction = base["oscillations"] - light["oscillations"] status = "BETTER" if reduction > 0 else "SAME" if reduction == 0 else "WORSE" - scenario = base['scenario'].replace(' - Baseline', '') - print(f"{scenario:<30} {base['oscillations']:>4} → {light['oscillations']:>4} events " - f"{status:>6} ({reduction:+d})") + scenario = base["scenario"].replace(" - Baseline", "") + print( + f"{scenario:<30} {base['oscillations']:>4} → {light['oscillations']:>4} events " + f"{status:>6} ({reduction:+d})" + ) print() print("=" * 80) @@ -384,20 +395,21 @@ def main(): # Calculate improvements avg_accuracy = sum( - (r['baseline']['final_error_mm'] - r['lightlike']['final_error_mm']) / - r['baseline']['final_error_mm'] * 100 + (r["baseline"]["final_error_mm"] - r["lightlike"]["final_error_mm"]) + / r["baseline"]["final_error_mm"] + * 100 for r in results ) / len(results) avg_stability = sum( - (r['baseline']['tracking_variance_mm'] - r['lightlike']['tracking_variance_mm']) / - r['baseline']['tracking_variance_mm'] * 100 + (r["baseline"]["tracking_variance_mm"] - r["lightlike"]["tracking_variance_mm"]) + / r["baseline"]["tracking_variance_mm"] + * 100 for r in results ) / len(results) total_osc_reduction = sum( - r['baseline']['oscillations'] - r['lightlike']['oscillations'] - for r in results + r["baseline"]["oscillations"] - r["lightlike"]["oscillations"] for r in results ) print(f"Average tracking accuracy improvement: {avg_accuracy:+.1f}%") @@ -407,8 +419,7 @@ def main(): # Check Lorentz-space detection lorentz_detections = sum( - r['lightlike'].get('lorentz_oscillations_detected', 0) - for r in results + r["lightlike"].get("lorentz_oscillations_detected", 0) for r in results ) print(f"Lorentz-space oscillations detected: {lorentz_detections} ticks") print() @@ -448,24 +459,33 @@ def main(): # Best case if results: - best_idx = max(range(len(results)), - key=lambda i: (results[i]['baseline']['final_error_mm'] - - results[i]['lightlike']['final_error_mm'])) + best_idx = max( + range(len(results)), + key=lambda i: ( + results[i]["baseline"]["final_error_mm"] - results[i]["lightlike"]["final_error_mm"] + ), + ) best = results[best_idx] - best_improvement = (best['baseline']['final_error_mm'] - - best['lightlike']['final_error_mm']) / \ - best['baseline']['final_error_mm'] * 100 + best_improvement = ( + (best["baseline"]["final_error_mm"] - best["lightlike"]["final_error_mm"]) + / best["baseline"]["final_error_mm"] + * 100 + ) print() print(f"BEST PERFORMANCE: {best['baseline']['scenario'].replace(' - Baseline', '')}") print(f" Noise level: {best['noise_level']}") print(f" Accuracy improvement: {best_improvement:+.1f}%") - print(f" Oscillations: {best['baseline']['oscillations'] - best['lightlike']['oscillations']:+d}") + print( + f" Oscillations: {best['baseline']['oscillations'] - best['lightlike']['oscillations']:+d}" + ) - if 'lorentz_oscillations_detected' in best['lightlike']: - print(f" Lorentz oscillations detected: {best['lightlike']['lorentz_oscillations_detected']}") + if "lorentz_oscillations_detected" in best["lightlike"]: + print( + f" Lorentz oscillations detected: {best['lightlike']['lorentz_oscillations_detected']}" + ) print(f" Observer activations: {best['lightlike']['damping_activations']}") -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/benchmark_terminal_descent.py b/benchmark_terminal_descent.py index 30dc597..d33c0c8 100644 --- a/benchmark_terminal_descent.py +++ b/benchmark_terminal_descent.py @@ -18,7 +18,8 @@ import numpy as np import sys -sys.path.insert(0, '/home/user/Eigen-Geometric-Control') + +sys.path.insert(0, "/home/user/Eigen-Geometric-Control") from src import ( detect_oscillation, @@ -26,7 +27,7 @@ forward_kinematics, compute_ds2, compute_gradient, - compute_change_stability + compute_change_stability, ) import pandas as pd @@ -71,9 +72,7 @@ def run_terminal_descent( osc_strength = 0.0 if use_lightlike and len(state_history) >= 4: - oscillating, osc_strength = detect_oscillation( - state_history, window=4, threshold=0.92 - ) + oscillating, osc_strength = detect_oscillation(state_history, window=4, threshold=0.92) if oscillating: damping = lightlike_damping_factor(osc_strength) * 0.85 @@ -84,17 +83,19 @@ def run_terminal_descent( C, S, ds2_CS = compute_change_stability(delta, 1e-3) - rows.append({ - 'tick': t, - 'x': x, - 'y': y, - 'distance_to_target': np.sqrt((x - target[0])**2 + (y - target[1])**2), - 'ds2_total': ds2_total, - 'grad_norm': grad_norm, - 'd_obs': components['d_obs'], - 'oscillating': oscillating if use_lightlike else False, - 'damping': damping if use_lightlike else 0.0, - }) + rows.append( + { + "tick": t, + "x": x, + "y": y, + "distance_to_target": np.sqrt((x - target[0]) ** 2 + (y - target[1]) ** 2), + "ds2_total": ds2_total, + "grad_norm": grad_norm, + "d_obs": components["d_obs"], + "oscillating": oscillating if use_lightlike else False, + "damping": damping if use_lightlike else 0.0, + } + ) theta1, theta2 = theta1_new, theta2_new @@ -124,47 +125,47 @@ def main(): # Baseline df_base = run_terminal_descent( - eta=config['eta'], - n_ticks=config['ticks'], - use_lightlike=False + eta=config["eta"], n_ticks=config["ticks"], use_lightlike=False ) # With lightlike df_light = run_terminal_descent( - eta=config['eta'], - n_ticks=config['ticks'], - use_lightlike=True + eta=config["eta"], n_ticks=config["ticks"], use_lightlike=True ) # Analyze - base_final = df_base['distance_to_target'].iloc[-1] * 1000 # mm - light_final = df_light['distance_to_target'].iloc[-1] * 1000 # mm + base_final = df_base["distance_to_target"].iloc[-1] * 1000 # mm + light_final = df_light["distance_to_target"].iloc[-1] * 1000 # mm improvement = (base_final - light_final) / base_final * 100 if base_final > 0 else 0 # Count oscillations in final 25% final_start = int(len(df_base) * 0.75) - base_grad = df_base['grad_norm'].values[final_start:] - light_grad = df_light['grad_norm'].values[final_start:] + base_grad = df_base["grad_norm"].values[final_start:] + light_grad = df_light["grad_norm"].values[final_start:] - base_osc = sum(1 for i in range(1, len(base_grad)) if base_grad[i] > base_grad[i-1] * 1.4) - light_osc = sum(1 for i in range(1, len(light_grad)) if light_grad[i] > light_grad[i-1] * 1.4) + base_osc = sum(1 for i in range(1, len(base_grad)) if base_grad[i] > base_grad[i - 1] * 1.4) + light_osc = sum( + 1 for i in range(1, len(light_grad)) if light_grad[i] > light_grad[i - 1] * 1.4 + ) # Smoothness - base_smooth = np.std(df_base['distance_to_target'].values[final_start:]) * 1000 - light_smooth = np.std(df_light['distance_to_target'].values[final_start:]) * 1000 - - results.append({ - 'name': config['name'], - 'eta': config['eta'], - 'ticks': config['ticks'], - 'base_final_mm': base_final, - 'light_final_mm': light_final, - 'improvement_pct': improvement, - 'base_oscillations': base_osc, - 'light_oscillations': light_osc, - 'base_smoothness': base_smooth, - 'light_smoothness': light_smooth, - }) + base_smooth = np.std(df_base["distance_to_target"].values[final_start:]) * 1000 + light_smooth = np.std(df_light["distance_to_target"].values[final_start:]) * 1000 + + results.append( + { + "name": config["name"], + "eta": config["eta"], + "ticks": config["ticks"], + "base_final_mm": base_final, + "light_final_mm": light_final, + "improvement_pct": improvement, + "base_oscillations": base_osc, + "light_oscillations": light_osc, + "base_smoothness": base_smooth, + "light_smoothness": light_smooth, + } + ) # Display results print() @@ -175,13 +176,17 @@ def main(): print("Final Landing Precision (mm):") print("-" * 80) - print(f"{'Descent Mode':<25} {'eta':<8} {'Ticks':<8} {'Baseline':<12} {'Lightlike':<12} {'Improvement':<12}") + print( + f"{'Descent Mode':<25} {'eta':<8} {'Ticks':<8} {'Baseline':<12} {'Lightlike':<12} {'Improvement':<12}" + ) print("-" * 80) for r in results: - arrow = "✓" if r['improvement_pct'] > 0 else "✗" - print(f"{r['name']:<25} {r['eta']:<8.3f} {r['ticks']:<8} " - f"{r['base_final_mm']:<12.2f} {r['light_final_mm']:<12.2f} " - f"{arrow} {r['improvement_pct']:>6.1f}%") + arrow = "✓" if r["improvement_pct"] > 0 else "✗" + print( + f"{r['name']:<25} {r['eta']:<8.3f} {r['ticks']:<8} " + f"{r['base_final_mm']:<12.2f} {r['light_final_mm']:<12.2f} " + f"{arrow} {r['improvement_pct']:>6.1f}%" + ) print() print("Final Approach Oscillations (last 25% of descent):") @@ -189,10 +194,12 @@ def main(): print(f"{'Descent Mode':<25} {'Baseline':<12} {'Lightlike':<12} {'Reduction':<12}") print("-" * 80) for r in results: - reduction = r['base_oscillations'] - r['light_oscillations'] + reduction = r["base_oscillations"] - r["light_oscillations"] status = "SAFER" if reduction > 0 else "SAME" if reduction == 0 else "MORE" - print(f"{r['name']:<25} {r['base_oscillations']:<12} {r['light_oscillations']:<12} " - f"{status:>6} ({reduction:+d})") + print( + f"{r['name']:<25} {r['base_oscillations']:<12} {r['light_oscillations']:<12} " + f"{status:>6} ({reduction:+d})" + ) print() print("Approach Smoothness (std dev in mm during final 25%):") @@ -200,10 +207,16 @@ def main(): print(f"{'Descent Mode':<25} {'Baseline':<12} {'Lightlike':<12} {'Improvement':<12}") print("-" * 80) for r in results: - smooth_improvement = (r['base_smoothness'] - r['light_smoothness']) / r['base_smoothness'] * 100 if r['base_smoothness'] > 0 else 0 + smooth_improvement = ( + (r["base_smoothness"] - r["light_smoothness"]) / r["base_smoothness"] * 100 + if r["base_smoothness"] > 0 + else 0 + ) arrow = "↓" if smooth_improvement > 0 else "↑" - print(f"{r['name']:<25} {r['base_smoothness']:<12.2f} {r['light_smoothness']:<12.2f} " - f"{arrow} {abs(smooth_improvement):>6.1f}%") + print( + f"{r['name']:<25} {r['base_smoothness']:<12.2f} {r['light_smoothness']:<12.2f} " + f"{arrow} {abs(smooth_improvement):>6.1f}%" + ) print() print("=" * 80) @@ -212,8 +225,8 @@ def main(): print() # Find best precision - best = min(results, key=lambda r: r['light_final_mm']) - worst = max(results, key=lambda r: r['base_final_mm']) + best = min(results, key=lambda r: r["light_final_mm"]) + worst = max(results, key=lambda r: r["base_final_mm"]) print("KEY FINDINGS:") print() @@ -224,8 +237,8 @@ def main(): print() print(f"2. PRECISION SCALING:") - fast_precision = results[0]['base_final_mm'] - slow_precision = results[-1]['base_final_mm'] + fast_precision = results[0]["base_final_mm"] + slow_precision = results[-1]["base_final_mm"] precision_gain = (fast_precision - slow_precision) / fast_precision * 100 print(f" - Fast descent: {fast_precision:.2f}mm") print(f" - Ultra-slow descent: {slow_precision:.2f}mm") @@ -233,25 +246,25 @@ def main(): print() print(f"3. LIGHTLIKE OBSERVER BENEFIT:") - avg_improvement = sum(r['improvement_pct'] for r in results) / len(results) + avg_improvement = sum(r["improvement_pct"] for r in results) / len(results) print(f" - Average improvement across all speeds: {avg_improvement:+.1f}%") # Check if slower = better with lightlike - slow_configs = [r for r in results if r['eta'] <= 0.04] + slow_configs = [r for r in results if r["eta"] <= 0.04] if slow_configs: - slow_avg = sum(r['improvement_pct'] for r in slow_configs) / len(slow_configs) + slow_avg = sum(r["improvement_pct"] for r in slow_configs) / len(slow_configs) print(f" - Average improvement for slow descent: {slow_avg:+.1f}%") print() # Oscillation analysis - total_osc_reduction = sum(r['base_oscillations'] - r['light_oscillations'] for r in results) + total_osc_reduction = sum(r["base_oscillations"] - r["light_oscillations"] for r in results) print(f"4. STABILITY:") print(f" - Total oscillation reduction: {total_osc_reduction:+d} events") print() print("CONCLUSION:") print() - if best['light_final_mm'] < 20.0: # Sub-20mm precision + if best["light_final_mm"] < 20.0: # Sub-20mm precision print(f"★ SUCCESS: Achieved sub-20mm precision ({best['light_final_mm']:.1f}mm)") print() print("Your hypothesis is VALIDATED:") @@ -264,7 +277,7 @@ def main(): print(" 2. Allocate more computation (400-500 iterations)") print(" 3. Deploy lightlike observer for stability") print(" 4. Result: Mission-capable precision") - elif best['light_final_mm'] < 40.0: + elif best["light_final_mm"] < 40.0: print(f"GOOD PROGRESS: Achieved {best['light_final_mm']:.1f}mm precision") print() print("Hypothesis partially validated:") @@ -288,5 +301,5 @@ def main(): print("=" * 80) -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/benchmark_visual_servoing.py b/benchmark_visual_servoing.py index d91811e..4a24461 100644 --- a/benchmark_visual_servoing.py +++ b/benchmark_visual_servoing.py @@ -20,7 +20,8 @@ import numpy as np import sys -sys.path.insert(0, '/home/user/Eigen-Geometric-Control') + +sys.path.insert(0, "/home/user/Eigen-Geometric-Control") from src import ( detect_oscillation, @@ -28,7 +29,7 @@ forward_kinematics, compute_ds2, compute_gradient, - compute_change_stability + compute_change_stability, ) import pandas as pd @@ -75,21 +76,18 @@ def run_visual_servoing_baseline( # Simulate stereo vision measurement (noisy) measured_target = add_stereo_noise( - true_target, noise_level=stereo_noise, - occlusion_prob=occlusion_prob, tick=t + true_target, noise_level=stereo_noise, occlusion_prob=occlusion_prob, tick=t ) # Robot uses measured position for control x, y = forward_kinematics(theta1, theta2, L1, L2) ds2_total, components = compute_ds2( - theta1, theta2, measured_target, obstacle_center, - obstacle_radius, Go, lam, L1, L2 + theta1, theta2, measured_target, obstacle_center, obstacle_radius, Go, lam, L1, L2 ) grad, grad_norm = compute_gradient( - theta1, theta2, measured_target, obstacle_center, - obstacle_radius, Go, lam, L1, L2 + theta1, theta2, measured_target, obstacle_center, obstacle_radius, Go, lam, L1, L2 ) # Standard update @@ -100,24 +98,28 @@ def run_visual_servoing_baseline( C, S, ds2_CS = compute_change_stability(delta, 1e-3) # Track both true and measured error - true_distance = np.sqrt((x - true_target[0])**2 + (y - true_target[1])**2) - measured_distance = np.sqrt((x - measured_target[0])**2 + (y - measured_target[1])**2) - - rows.append({ - 'tick': t, - 'x': x, - 'y': y, - 'true_target_x': true_target[0], - 'true_target_y': true_target[1], - 'measured_target_x': measured_target[0], - 'measured_target_y': measured_target[1], - 'true_distance': true_distance, - 'measured_distance': measured_distance, - 'vision_error': np.sqrt((true_target[0] - measured_target[0])**2 + - (true_target[1] - measured_target[1])**2), - 'grad_norm': grad_norm, - 'd_obs': components['d_obs'], - }) + true_distance = np.sqrt((x - true_target[0]) ** 2 + (y - true_target[1]) ** 2) + measured_distance = np.sqrt((x - measured_target[0]) ** 2 + (y - measured_target[1]) ** 2) + + rows.append( + { + "tick": t, + "x": x, + "y": y, + "true_target_x": true_target[0], + "true_target_y": true_target[1], + "measured_target_x": measured_target[0], + "measured_target_y": measured_target[1], + "true_distance": true_distance, + "measured_distance": measured_distance, + "vision_error": np.sqrt( + (true_target[0] - measured_target[0]) ** 2 + + (true_target[1] - measured_target[1]) ** 2 + ), + "grad_norm": grad_norm, + "d_obs": components["d_obs"], + } + ) theta1, theta2 = theta1_new, theta2_new @@ -155,8 +157,7 @@ def run_visual_servoing_lightlike( # Simulate stereo vision measurement (noisy) measured_target = add_stereo_noise( - true_target, noise_level=stereo_noise, - occlusion_prob=occlusion_prob, tick=t + true_target, noise_level=stereo_noise, occlusion_prob=occlusion_prob, tick=t ) # Robot state @@ -165,13 +166,11 @@ def run_visual_servoing_lightlike( state_history.append(state) ds2_total, components = compute_ds2( - theta1, theta2, measured_target, obstacle_center, - obstacle_radius, Go, lam, L1, L2 + theta1, theta2, measured_target, obstacle_center, obstacle_radius, Go, lam, L1, L2 ) grad, grad_norm = compute_gradient( - theta1, theta2, measured_target, obstacle_center, - obstacle_radius, Go, lam, L1, L2 + theta1, theta2, measured_target, obstacle_center, obstacle_radius, Go, lam, L1, L2 ) # LIGHTLIKE OBSERVER (stabilizes noisy vision) @@ -194,27 +193,31 @@ def run_visual_servoing_lightlike( C, S, ds2_CS = compute_change_stability(delta, 1e-3) # Track both true and measured error - true_distance = np.sqrt((x - true_target[0])**2 + (y - true_target[1])**2) - measured_distance = np.sqrt((x - measured_target[0])**2 + (y - measured_target[1])**2) - - rows.append({ - 'tick': t, - 'x': x, - 'y': y, - 'true_target_x': true_target[0], - 'true_target_y': true_target[1], - 'measured_target_x': measured_target[0], - 'measured_target_y': measured_target[1], - 'true_distance': true_distance, - 'measured_distance': measured_distance, - 'vision_error': np.sqrt((true_target[0] - measured_target[0])**2 + - (true_target[1] - measured_target[1])**2), - 'grad_norm': grad_norm, - 'd_obs': components['d_obs'], - 'oscillating': oscillating, - 'osc_strength': osc_strength, - 'damping': damping, - }) + true_distance = np.sqrt((x - true_target[0]) ** 2 + (y - true_target[1]) ** 2) + measured_distance = np.sqrt((x - measured_target[0]) ** 2 + (y - measured_target[1]) ** 2) + + rows.append( + { + "tick": t, + "x": x, + "y": y, + "true_target_x": true_target[0], + "true_target_y": true_target[1], + "measured_target_x": measured_target[0], + "measured_target_y": measured_target[1], + "true_distance": true_distance, + "measured_distance": measured_distance, + "vision_error": np.sqrt( + (true_target[0] - measured_target[0]) ** 2 + + (true_target[1] - measured_target[1]) ** 2 + ), + "grad_norm": grad_norm, + "d_obs": components["d_obs"], + "oscillating": oscillating, + "osc_strength": osc_strength, + "damping": damping, + } + ) theta1, theta2 = theta1_new, theta2_new @@ -224,34 +227,35 @@ def run_visual_servoing_lightlike( def analyze_visual_servoing(df, scenario_name): """Analyze visual servoing performance""" # Final true accuracy (what matters) - final_true_error = df['true_distance'].iloc[-1] * 1000 # mm - mean_true_error = df['true_distance'].mean() * 1000 + final_true_error = df["true_distance"].iloc[-1] * 1000 # mm + mean_true_error = df["true_distance"].mean() * 1000 # Vision noise impact - mean_vision_error = df['vision_error'].mean() * 1000 - max_vision_error = df['vision_error'].max() * 1000 + mean_vision_error = df["vision_error"].mean() * 1000 + max_vision_error = df["vision_error"].max() * 1000 # Stability (tracking smoothness) - tracking_variance = np.std(df['true_distance'].values) * 1000 + tracking_variance = np.std(df["true_distance"].values) * 1000 # Oscillations - grad_norms = df['grad_norm'].values - oscillations = sum(1 for i in range(10, len(grad_norms)) - if grad_norms[i] > grad_norms[i-1] * 1.5) + grad_norms = df["grad_norm"].values + oscillations = sum( + 1 for i in range(10, len(grad_norms)) if grad_norms[i] > grad_norms[i - 1] * 1.5 + ) metrics = { - 'scenario': scenario_name, - 'final_true_error_mm': final_true_error, - 'mean_true_error_mm': mean_true_error, - 'tracking_variance_mm': tracking_variance, - 'mean_vision_error_mm': mean_vision_error, - 'max_vision_error_mm': max_vision_error, - 'oscillations': oscillations, + "scenario": scenario_name, + "final_true_error_mm": final_true_error, + "mean_true_error_mm": mean_true_error, + "tracking_variance_mm": tracking_variance, + "mean_vision_error_mm": mean_vision_error, + "max_vision_error_mm": max_vision_error, + "oscillations": oscillations, } - if 'damping' in df.columns: - metrics['damping_activations'] = (df['damping'] > 0).sum() - metrics['max_damping'] = df['damping'].max() + if "damping" in df.columns: + metrics["damping_activations"] = (df["damping"] > 0).sum() + metrics["max_damping"] = df["damping"].max() return metrics @@ -271,26 +275,23 @@ def main(): # Scenario 1: Static target with stereo noise print("Scenario 1: Static target with stereo noise...") + def static_target(t): return [1.2, 0.3] df_base_1 = run_visual_servoing_baseline( - target_trajectory=static_target, - n_ticks=180, - stereo_noise=0.015, - occlusion_prob=0.0 + target_trajectory=static_target, n_ticks=180, stereo_noise=0.015, occlusion_prob=0.0 ) df_light_1 = run_visual_servoing_lightlike( - target_trajectory=static_target, - n_ticks=180, - stereo_noise=0.015, - occlusion_prob=0.0 + target_trajectory=static_target, n_ticks=180, stereo_noise=0.015, occlusion_prob=0.0 ) - results.append({ - 'baseline': analyze_visual_servoing(df_base_1, "Stereo Noise - Baseline"), - 'lightlike': analyze_visual_servoing(df_light_1, "Stereo Noise - Lightlike"), - }) + results.append( + { + "baseline": analyze_visual_servoing(df_base_1, "Stereo Noise - Baseline"), + "lightlike": analyze_visual_servoing(df_light_1, "Stereo Noise - Lightlike"), + } + ) # Scenario 2: Static target with occlusions print("Scenario 2: Static target with occlusions...") @@ -298,65 +299,64 @@ def static_target(t): target_trajectory=static_target, n_ticks=180, stereo_noise=0.015, - occlusion_prob=0.05 # 5% occlusion rate + occlusion_prob=0.05, # 5% occlusion rate ) df_light_2 = run_visual_servoing_lightlike( - target_trajectory=static_target, - n_ticks=180, - stereo_noise=0.015, - occlusion_prob=0.05 + target_trajectory=static_target, n_ticks=180, stereo_noise=0.015, occlusion_prob=0.05 ) - results.append({ - 'baseline': analyze_visual_servoing(df_base_2, "With Occlusions - Baseline"), - 'lightlike': analyze_visual_servoing(df_light_2, "With Occlusions - Lightlike"), - }) + results.append( + { + "baseline": analyze_visual_servoing(df_base_2, "With Occlusions - Baseline"), + "lightlike": analyze_visual_servoing(df_light_2, "With Occlusions - Lightlike"), + } + ) # Scenario 3: Moving target with stereo noise print("Scenario 3: Moving target tracking with stereo noise...") + def moving_target(t): - return [1.1 + 0.002*t, 0.3 + 0.001*t] + return [1.1 + 0.002 * t, 0.3 + 0.001 * t] df_base_3 = run_visual_servoing_baseline( target_trajectory=moving_target, n_ticks=180, stereo_noise=0.020, # Higher noise - occlusion_prob=0.0 + occlusion_prob=0.0, ) df_light_3 = run_visual_servoing_lightlike( - target_trajectory=moving_target, - n_ticks=180, - stereo_noise=0.020, - occlusion_prob=0.0 + target_trajectory=moving_target, n_ticks=180, stereo_noise=0.020, occlusion_prob=0.0 ) - results.append({ - 'baseline': analyze_visual_servoing(df_base_3, "Moving Target - Baseline"), - 'lightlike': analyze_visual_servoing(df_light_3, "Moving Target - Lightlike"), - }) + results.append( + { + "baseline": analyze_visual_servoing(df_base_3, "Moving Target - Baseline"), + "lightlike": analyze_visual_servoing(df_light_3, "Moving Target - Lightlike"), + } + ) # Scenario 4: Oscillating target (periodic motion) print("Scenario 4: Oscillating target with high noise...") + def oscillating_target(t): - return [1.2 + 0.08*np.sin(t*0.2), 0.35 + 0.06*np.cos(t*0.15)] + return [1.2 + 0.08 * np.sin(t * 0.2), 0.35 + 0.06 * np.cos(t * 0.15)] df_base_4 = run_visual_servoing_baseline( target_trajectory=oscillating_target, n_ticks=180, stereo_noise=0.025, # Very noisy - occlusion_prob=0.03 + occlusion_prob=0.03, ) df_light_4 = run_visual_servoing_lightlike( - target_trajectory=oscillating_target, - n_ticks=180, - stereo_noise=0.025, - occlusion_prob=0.03 + target_trajectory=oscillating_target, n_ticks=180, stereo_noise=0.025, occlusion_prob=0.03 ) - results.append({ - 'baseline': analyze_visual_servoing(df_base_4, "Oscillating + Noise - Baseline"), - 'lightlike': analyze_visual_servoing(df_light_4, "Oscillating + Noise - Lightlike"), - }) + results.append( + { + "baseline": analyze_visual_servoing(df_base_4, "Oscillating + Noise - Baseline"), + "lightlike": analyze_visual_servoing(df_light_4, "Oscillating + Noise - Lightlike"), + } + ) # Display results print() @@ -368,40 +368,54 @@ def oscillating_target(t): print("Final True Tracking Error (mm) - ACTUAL PERFORMANCE:") print("-" * 80) for r in results: - base = r['baseline'] - light = r['lightlike'] - improvement = (base['final_true_error_mm'] - light['final_true_error_mm']) / base['final_true_error_mm'] * 100 + base = r["baseline"] + light = r["lightlike"] + improvement = ( + (base["final_true_error_mm"] - light["final_true_error_mm"]) + / base["final_true_error_mm"] + * 100 + ) arrow = "✓" if improvement > 0 else "✗" - scenario = base['scenario'].replace(' - Baseline', '') - print(f"{scenario:<35} {base['final_true_error_mm']:>7.1f}mm → {light['final_true_error_mm']:>7.1f}mm " - f"{arrow} {abs(improvement):>5.1f}%") + scenario = base["scenario"].replace(" - Baseline", "") + print( + f"{scenario:<35} {base['final_true_error_mm']:>7.1f}mm → {light['final_true_error_mm']:>7.1f}mm " + f"{arrow} {abs(improvement):>5.1f}%" + ) print() print("Tracking Stability (std dev in mm):") print("-" * 80) for r in results: - base = r['baseline'] - light = r['lightlike'] - improvement = (base['tracking_variance_mm'] - light['tracking_variance_mm']) / base['tracking_variance_mm'] * 100 + base = r["baseline"] + light = r["lightlike"] + improvement = ( + (base["tracking_variance_mm"] - light["tracking_variance_mm"]) + / base["tracking_variance_mm"] + * 100 + ) arrow = "↓" if improvement > 0 else "↑" - scenario = base['scenario'].replace(' - Baseline', '') - print(f"{scenario:<35} {base['tracking_variance_mm']:>7.1f}mm → {light['tracking_variance_mm']:>7.1f}mm " - f"{arrow} {abs(improvement):>5.1f}%") + scenario = base["scenario"].replace(" - Baseline", "") + print( + f"{scenario:<35} {base['tracking_variance_mm']:>7.1f}mm → {light['tracking_variance_mm']:>7.1f}mm " + f"{arrow} {abs(improvement):>5.1f}%" + ) print() print("Control Oscillations (induced by vision noise):") print("-" * 80) for r in results: - base = r['baseline'] - light = r['lightlike'] - reduction = base['oscillations'] - light['oscillations'] + base = r["baseline"] + light = r["lightlike"] + reduction = base["oscillations"] - light["oscillations"] status = "BETTER" if reduction > 0 else "SAME" if reduction == 0 else "WORSE" - scenario = base['scenario'].replace(' - Baseline', '') - print(f"{scenario:<35} {base['oscillations']:>4} → {light['oscillations']:>4} events " - f"{status:>6} ({reduction:+d})") + scenario = base["scenario"].replace(" - Baseline", "") + print( + f"{scenario:<35} {base['oscillations']:>4} → {light['oscillations']:>4} events " + f"{status:>6} ({reduction:+d})" + ) print() print("=" * 80) @@ -411,20 +425,21 @@ def oscillating_target(t): # Calculate improvements avg_accuracy_improvement = sum( - (r['baseline']['final_true_error_mm'] - r['lightlike']['final_true_error_mm']) / - r['baseline']['final_true_error_mm'] * 100 + (r["baseline"]["final_true_error_mm"] - r["lightlike"]["final_true_error_mm"]) + / r["baseline"]["final_true_error_mm"] + * 100 for r in results ) / len(results) avg_stability_improvement = sum( - (r['baseline']['tracking_variance_mm'] - r['lightlike']['tracking_variance_mm']) / - r['baseline']['tracking_variance_mm'] * 100 + (r["baseline"]["tracking_variance_mm"] - r["lightlike"]["tracking_variance_mm"]) + / r["baseline"]["tracking_variance_mm"] + * 100 for r in results ) / len(results) total_oscillation_reduction = sum( - r['baseline']['oscillations'] - r['lightlike']['oscillations'] - for r in results + r["baseline"]["oscillations"] - r["lightlike"]["oscillations"] for r in results ) print(f"Average tracking accuracy improvement: {avg_accuracy_improvement:+.1f}%") @@ -465,23 +480,31 @@ def oscillating_target(t): # Best scenario if results: - best_idx = max(range(len(results)), - key=lambda i: (results[i]['baseline']['final_true_error_mm'] - - results[i]['lightlike']['final_true_error_mm'])) + best_idx = max( + range(len(results)), + key=lambda i: ( + results[i]["baseline"]["final_true_error_mm"] + - results[i]["lightlike"]["final_true_error_mm"] + ), + ) best = results[best_idx] - best_improvement = (best['baseline']['final_true_error_mm'] - - best['lightlike']['final_true_error_mm']) / \ - best['baseline']['final_true_error_mm'] * 100 + best_improvement = ( + (best["baseline"]["final_true_error_mm"] - best["lightlike"]["final_true_error_mm"]) + / best["baseline"]["final_true_error_mm"] + * 100 + ) print() print(f"BEST PERFORMANCE: {best['baseline']['scenario'].replace(' - Baseline', '')}") print(f" Accuracy improvement: {best_improvement:+.1f}%") - print(f" Oscillations reduced: {best['baseline']['oscillations'] - best['lightlike']['oscillations']} events") + print( + f" Oscillations reduced: {best['baseline']['oscillations'] - best['lightlike']['oscillations']} events" + ) - if 'damping_activations' in best['lightlike']: + if "damping_activations" in best["lightlike"]: print(f" Observer activations: {best['lightlike']['damping_activations']}") print(f" Max damping: {best['lightlike']['max_damping']:.3f}") -if __name__ == '__main__': - main() \ No newline at end of file +if __name__ == "__main__": + main() diff --git a/benchmark_weak_stereo.py b/benchmark_weak_stereo.py index d252bd1..e9546f1 100644 --- a/benchmark_weak_stereo.py +++ b/benchmark_weak_stereo.py @@ -153,13 +153,9 @@ def run_benchmark(): strong = results["Strong Measurement (0.9)"] coherence_improvement = ( - (weak["coherence_mean"] - strong["coherence_mean"]) - / strong["coherence_mean"] - * 100 - ) - error_improvement = ( - (strong["error_mean"] - weak["error_mean"]) / strong["error_mean"] * 100 + (weak["coherence_mean"] - strong["coherence_mean"]) / strong["coherence_mean"] * 100 ) + error_improvement = (strong["error_mean"] - weak["error_mean"]) / strong["error_mean"] * 100 print(f"Coherence Improvement: {coherence_improvement:+.1f}%") print(f" Weak: {weak['coherence_mean']:.4f} (smoother trajectory)") @@ -218,9 +214,7 @@ def plot_results(results: dict, target: np.ndarray): # Mark start and target ax.scatter([5.0], [5.0], s=200, marker="*", color="green", label="Start", zorder=5) - ax.scatter( - [target[0]], [target[1]], s=200, marker="X", color="red", label="Target", zorder=5 - ) + ax.scatter([target[0]], [target[1]], s=200, marker="X", color="red", label="Target", zorder=5) ax.legend(loc="upper right") ax.set_aspect("equal") diff --git a/benchmark_weak_stereo_noisy.py b/benchmark_weak_stereo_noisy.py index 4530434..f9b6ee6 100644 --- a/benchmark_weak_stereo_noisy.py +++ b/benchmark_weak_stereo_noisy.py @@ -74,9 +74,7 @@ def simulate_noisy_stereo_control( # Measure smoothness (via velocity changes) velocities = [trajectory[i] - trajectory[i - 1] for i in range(1, len(trajectory))] - accelerations = [ - velocities[i] - velocities[i - 1] for i in range(1, len(velocities)) - ] + accelerations = [velocities[i] - velocities[i - 1] for i in range(1, len(velocities))] jitter = np.mean([np.linalg.norm(a) for a in accelerations]) return trajectory, coherence, final_error, jitter @@ -142,9 +140,15 @@ def run_noisy_benchmark(): "color": config["color"], } - print(f" Coherence: {results[config['name']]['coherence_mean']:.4f} ± {results[config['name']]['coherence_std']:.4f}") - print(f" Final Error: {results[config['name']]['error_mean']:.4f} ± {results[config['name']]['error_std']:.4f}") - print(f" Jitter: {results[config['name']]['jitter_mean']:.4f} ± {results[config['name']]['jitter_std']:.4f}") + print( + f" Coherence: {results[config['name']]['coherence_mean']:.4f} ± {results[config['name']]['coherence_std']:.4f}" + ) + print( + f" Final Error: {results[config['name']]['error_mean']:.4f} ± {results[config['name']]['error_std']:.4f}" + ) + print( + f" Jitter: {results[config['name']]['jitter_mean']:.4f} ± {results[config['name']]['jitter_std']:.4f}" + ) print() # Summary @@ -159,16 +163,12 @@ def run_noisy_benchmark(): # Compare weakest vs strong coherence_improvement = ( - (weakest["coherence_mean"] - strong["coherence_mean"]) - / strong["coherence_mean"] - * 100 + (weakest["coherence_mean"] - strong["coherence_mean"]) / strong["coherence_mean"] * 100 ) jitter_reduction = ( (strong["jitter_mean"] - weakest["jitter_mean"]) / strong["jitter_mean"] * 100 ) - error_improvement = ( - (strong["error_mean"] - weakest["error_mean"]) / strong["error_mean"] * 100 - ) + error_improvement = (strong["error_mean"] - weakest["error_mean"]) / strong["error_mean"] * 100 print(f"WEAK (0.05) vs STRONG (0.9):") print("-" * 40) @@ -232,9 +232,7 @@ def plot_noisy_results(results: dict, target: np.ndarray): ) ax.scatter([8.0], [8.0], s=200, marker="*", color="green", label="Start", zorder=5) - ax.scatter( - [target[0]], [target[1]], s=200, marker="X", color="red", label="Target", zorder=5 - ) + ax.scatter([target[0]], [target[1]], s=200, marker="X", color="red", label="Target", zorder=5) ax.legend(loc="upper right", fontsize=10) ax.set_aspect("equal") @@ -294,9 +292,7 @@ def plot_noisy_results(results: dict, target: np.ndarray): final_errors = [results[name]["error_mean"] for name in names] errors = [results[name]["error_std"] for name in names] - bars = ax.bar( - range(len(names)), final_errors, yerr=errors, color=colors, alpha=0.7 - ) + bars = ax.bar(range(len(names)), final_errors, yerr=errors, color=colors, alpha=0.7) ax.set_xticks(range(len(names))) ax.set_xticklabels(names, fontsize=10) ax.grid(True, axis="y", alpha=0.3) diff --git a/benchmark_xor.py b/benchmark_xor.py index 50fd9fd..9909c94 100644 --- a/benchmark_xor.py +++ b/benchmark_xor.py @@ -8,7 +8,8 @@ import numpy as np import sys -sys.path.insert(0, '/home/user/Eigen-Geometric-Control') + +sys.path.insert(0, "/home/user/Eigen-Geometric-Control") from src import ( run_xor_simulation, @@ -17,7 +18,7 @@ forward_kinematics, compute_ds2, compute_gradient, - compute_change_stability + compute_change_stability, ) import pandas as pd @@ -32,7 +33,7 @@ def run_xor_with_lightlike_damping( eps_change=1e-3, L1=0.9, L2=0.9, - window=3 + window=3, ): """Run XOR simulation WITH lightlike observer damping""" theta1, theta2 = theta_init @@ -78,23 +79,25 @@ def run_xor_with_lightlike_damping( C, S, ds2_CS = compute_change_stability(delta, eps_change) # Record state - rows.append({ - 'tick': t, - 'theta1_rad': theta1, - 'theta2_rad': theta2, - 'x': x, - 'y': y, - 'ds2_total': ds2_total, - 'grad_norm': grad_norm, - 'C': C, - 'S': S, - 'ds2_CS': ds2_CS, - 'delta_theta1': delta[0], - 'delta_theta2': delta[1], - 'oscillating': oscillating, - 'osc_strength': osc_strength, - 'damping': damping, - }) + rows.append( + { + "tick": t, + "theta1_rad": theta1, + "theta2_rad": theta2, + "x": x, + "y": y, + "ds2_total": ds2_total, + "grad_norm": grad_norm, + "C": C, + "S": S, + "ds2_CS": ds2_CS, + "delta_theta1": delta[0], + "delta_theta2": delta[1], + "oscillating": oscillating, + "osc_strength": osc_strength, + "damping": damping, + } + ) # Update for next iteration theta1, theta2 = theta1_new, theta2_new @@ -111,15 +114,16 @@ def detect_period_2_loop(df, tolerance=0.01): recent = df.tail(10) # Check if states alternate between two configurations - theta1_vals = recent['theta1_rad'].values - theta2_vals = recent['theta2_rad'].values + theta1_vals = recent["theta1_rad"].values + theta2_vals = recent["theta2_rad"].values # Check for A-B-A-B pattern period_2 = True for i in range(len(theta1_vals) - 2): # State should match state 2 ticks ago - dist = np.sqrt((theta1_vals[i] - theta1_vals[i+2])**2 + - (theta2_vals[i] - theta2_vals[i+2])**2) + dist = np.sqrt( + (theta1_vals[i] - theta1_vals[i + 2]) ** 2 + (theta2_vals[i] - theta2_vals[i + 2]) ** 2 + ) if dist > tolerance: period_2 = False break @@ -163,10 +167,12 @@ def main(): print("BASELINE (v1.0.0):") print(f" Initial ds²: {df_baseline['ds2_total'].iloc[0]:.4f}") print(f" Final ds²: {df_baseline['ds2_total'].iloc[-1]:.4f}") - print(f" ds² change: {df_baseline['ds2_total'].iloc[-1] - df_baseline['ds2_total'].iloc[0]:.4f}") + print( + f" ds² change: {df_baseline['ds2_total'].iloc[-1] - df_baseline['ds2_total'].iloc[0]:.4f}" + ) # Check for oscillation - ds2_variance = df_baseline['ds2_total'].tail(10).std() + ds2_variance = df_baseline["ds2_total"].tail(10).std() is_period_2 = detect_period_2_loop(df_baseline) print(f" ds² variance (last 10): {ds2_variance:.4f}") @@ -183,18 +189,20 @@ def main(): print("WITH LIGHTLIKE OBSERVER:") print(f" Initial ds²: {df_lightlike['ds2_total'].iloc[0]:.4f}") print(f" Final ds²: {df_lightlike['ds2_total'].iloc[-1]:.4f}") - print(f" ds² change: {df_lightlike['ds2_total'].iloc[-1] - df_lightlike['ds2_total'].iloc[0]:.4f}") + print( + f" ds² change: {df_lightlike['ds2_total'].iloc[-1] - df_lightlike['ds2_total'].iloc[0]:.4f}" + ) - ds2_variance_light = df_lightlike['ds2_total'].tail(10).std() + ds2_variance_light = df_lightlike["ds2_total"].tail(10).std() is_period_2_light = detect_period_2_loop(df_lightlike) print(f" ds² variance (last 10): {ds2_variance_light:.4f}") print(f" Period-2 detected: {is_period_2_light}") # Check observer activations - damping_used = (df_lightlike['damping'] > 0).sum() - max_damping = df_lightlike['damping'].max() - mean_osc_strength = df_lightlike['osc_strength'].mean() + damping_used = (df_lightlike["damping"] > 0).sum() + max_damping = df_lightlike["damping"].max() + mean_osc_strength = df_lightlike["osc_strength"].mean() print(f" Observer activations: {damping_used} / {len(df_lightlike)} ticks") print(f" Max damping: {max_damping:.4f}") @@ -232,11 +240,19 @@ def main(): print("Trajectory comparison (last 10 ticks):") print() print("BASELINE:") - print(df_baseline[['tick', 'theta1_rad', 'theta2_rad', 'ds2_total']].tail(10).to_string(index=False)) + print( + df_baseline[["tick", "theta1_rad", "theta2_rad", "ds2_total"]] + .tail(10) + .to_string(index=False) + ) print() print("WITH LIGHTLIKE:") - print(df_lightlike[['tick', 'theta1_rad', 'theta2_rad', 'ds2_total', 'damping']].tail(10).to_string(index=False)) + print( + df_lightlike[["tick", "theta1_rad", "theta2_rad", "ds2_total", "damping"]] + .tail(10) + .to_string(index=False) + ) -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/src/eigen_adaptive.py b/src/eigen_adaptive.py index 1979390..e7a77f2 100644 --- a/src/eigen_adaptive.py +++ b/src/eigen_adaptive.py @@ -67,8 +67,7 @@ def estimate_characteristic_velocity( # Compute velocities velocities = [ - trajectory_history[i] - trajectory_history[i - 1] - for i in range(1, len(trajectory_history)) + trajectory_history[i] - trajectory_history[i - 1] for i in range(1, len(trajectory_history)) ] # Velocity magnitudes @@ -125,8 +124,7 @@ def estimate_oscillation_threshold( # Compute velocities velocities = [ - trajectory_history[i] - trajectory_history[i - 1] - for i in range(1, len(trajectory_history)) + trajectory_history[i] - trajectory_history[i - 1] for i in range(1, len(trajectory_history)) ] if len(velocities) < 2: @@ -190,8 +188,7 @@ def estimate_natural_timescale( # Convert to velocity signal velocities = [ - trajectory_history[i] - trajectory_history[i - 1] - for i in range(1, len(trajectory_history)) + trajectory_history[i] - trajectory_history[i - 1] for i in range(1, len(trajectory_history)) ] if len(velocities) < 3: diff --git a/src/eigen_decomposition.py b/src/eigen_decomposition.py index 79a8177..5bbf9a0 100644 --- a/src/eigen_decomposition.py +++ b/src/eigen_decomposition.py @@ -85,9 +85,7 @@ def compute_autocorrelation( continue # Correlation between x[t] and x[t-lag] - corr = np.mean( - np.sum(signal_centered[lag:] * signal_centered[: N - lag], axis=1) - ) + corr = np.mean(np.sum(signal_centered[lag:] * signal_centered[: N - lag], axis=1)) autocorr[lag] = corr / var @@ -129,9 +127,7 @@ def coherence_score( if len(trajectory) < 2: return 0.5 - velocities = [ - trajectory[i] - trajectory[i - 1] for i in range(1, len(trajectory)) - ] + velocities = [trajectory[i] - trajectory[i - 1] for i in range(1, len(trajectory))] signal = np.array(velocities) # Compute autocorrelation @@ -356,9 +352,7 @@ def coherent_control_step( diagnostics = { **decomp_info, "filtered_obs": filtered_obs, - "noise_reduction": float( - np.linalg.norm(observation - filtered_obs) - ), + "noise_reduction": float(np.linalg.norm(observation - filtered_obs)), } return new_state, diagnostics diff --git a/src/eigen_geodesic.py b/src/eigen_geodesic.py index b89b97e..83695d9 100644 --- a/src/eigen_geodesic.py +++ b/src/eigen_geodesic.py @@ -198,8 +198,10 @@ def christoffel_symbols( for rho in range(d): # Sum over sigma for sigma in range(d): - Gamma[mu, nu, rho] += 0.5 * g_inv[mu, sigma] * ( - dg[sigma, nu, rho] + dg[sigma, rho, nu] - dg[nu, rho, sigma] + Gamma[mu, nu, rho] += ( + 0.5 + * g_inv[mu, sigma] + * (dg[sigma, nu, rho] + dg[sigma, rho, nu] - dg[nu, rho, sigma]) ) return Gamma @@ -424,9 +426,7 @@ def geodesic_control_step( elif dist < obs.radius * 3.0: # Repulsion range # Repulsion magnitude: stronger when closer # F_repel ~ 1/dist² (like Coulomb force) - repulsion_magnitude = ( - obs.strength * repulsion_strength / (dist**2 + 0.1) - ) + repulsion_magnitude = obs.strength * repulsion_strength / (dist**2 + 0.1) # Direction: away from obstacle repulsion_direction = diff / dist repulsive_force += repulsion_magnitude * repulsion_direction diff --git a/src/eigen_meta_control.py b/src/eigen_meta_control.py index 54cfb34..9fd935e 100644 --- a/src/eigen_meta_control.py +++ b/src/eigen_meta_control.py @@ -70,8 +70,7 @@ def observe_parameter_performance( for i in range(1, len(recent_trajectory)) ] velocity_changes = [ - np.linalg.norm(velocities[i] - velocities[i - 1]) - for i in range(1, len(velocities)) + np.linalg.norm(velocities[i] - velocities[i - 1]) for i in range(1, len(velocities)) ] smoothness = 1.0 / (1.0 + np.mean(velocity_changes)) else: @@ -82,9 +81,7 @@ def observe_parameter_performance( convergence_score = max(0.0, convergence_rate) if converging else 0.0 performance_score = ( - (1.0 - oscillation_penalty) * 0.4 - + convergence_score * 0.4 - + smoothness * 0.2 + (1.0 - oscillation_penalty) * 0.4 + convergence_score * 0.4 + smoothness * 0.2 ) # Diagnose what needs adjustment @@ -227,9 +224,7 @@ def self_tuning_control_step( # If lightlike observer detected issues, adjust parameters if needs_adjustment: - updated_params = meta_update_parameters( - control_params, diagnosis, meta_eta=meta_eta - ) + updated_params = meta_update_parameters(control_params, diagnosis, meta_eta=meta_eta) meta_info["adjustment_made"] = True meta_info["param_changes"] = { key: { diff --git a/src/eigen_multi_agent.py b/src/eigen_multi_agent.py index ad0a2ee..cdf7777 100644 --- a/src/eigen_multi_agent.py +++ b/src/eigen_multi_agent.py @@ -201,9 +201,7 @@ def multi_agent_step( # 4. UPDATE AGENT STATES # Apply gradient descent with paradox-mediated damping new_states = [] - for i, (state, gradient, damping) in enumerate( - zip(agent_states, gradients, dampings) - ): + for i, (state, gradient, damping) in enumerate(zip(agent_states, gradients, dampings)): # Gradient step velocity = eta * gradient @@ -242,8 +240,7 @@ def check_convergence( - distances: List[float] - Distance of each agent to its target """ distances = [ - float(np.linalg.norm(state - target)) - for state, target in zip(agent_states, agent_targets) + float(np.linalg.norm(state - target)) for state, target in zip(agent_states, agent_targets) ] all_converged = all(d < tolerance for d in distances) diff --git a/src/eigen_qec.py b/src/eigen_qec.py index 9a2144f..08eedeb 100644 --- a/src/eigen_qec.py +++ b/src/eigen_qec.py @@ -334,9 +334,7 @@ def correct_measurement_errors( # Correct measurement if error_location is not None: # Error detected - exclude faulty sensor - valid_measurements = [ - m for i, m in enumerate(measurements) if i != error_location - ] + valid_measurements = [m for i, m in enumerate(measurements) if i != error_location] corrected_state = np.mean(valid_measurements, axis=0) correction_applied = True error_detected = True @@ -421,9 +419,7 @@ def qec_control_step( # Update state estimate (blend current state with corrected measurement) blend_factor = 0.5 # Weight for measurement vs state estimate - updated_state = ( - blend_factor * corrected_measurement + (1 - blend_factor) * current_state - ) + updated_state = blend_factor * corrected_measurement + (1 - blend_factor) * current_state # Control: move toward target direction = target - updated_state diff --git a/src/eigen_weak_measurement.py b/src/eigen_weak_measurement.py index 2036c03..fd20e45 100644 --- a/src/eigen_weak_measurement.py +++ b/src/eigen_weak_measurement.py @@ -166,9 +166,7 @@ def stereo_weak_measurement( # Apply weak measurement to each eye separately left_measured, left_value = apply_weak_measurement(left_state, measurement_strength) - right_measured, right_value = apply_weak_measurement( - right_state, measurement_strength - ) + right_measured, right_value = apply_weak_measurement(right_state, measurement_strength) # Depth emerges from INTERFERENCE of the two measurements # This is the key: timelike (left) XOR spacelike (right) @@ -249,7 +247,9 @@ def weak_stereo_control_step( # Strong depth estimate from accumulated weak measurements # Recency-weighted: more recent measurements weighted higher - weights = [0.5 ** (len(accumulated_measurements) - 1 - i) for i in range(len(accumulated_measurements))] + weights = [ + 0.5 ** (len(accumulated_measurements) - 1 - i) for i in range(len(accumulated_measurements)) + ] strong_depth = accumulate_weak_measurements(accumulated_measurements, weights) @@ -261,9 +261,7 @@ def weak_stereo_control_step( # Visual servoing correction (weighted by depth confidence) # Closer objects (larger depth value) → stronger correction - depth_confidence = 1.0 / (1.0 + strong_depth) if strong_depth < float( - "inf" - ) else 0.0 + depth_confidence = 1.0 / (1.0 + strong_depth) if strong_depth < float("inf") else 0.0 # Combine visual information if len(left_measured) == len(current_state): @@ -303,13 +301,9 @@ def coherence_metric( # Coherence = smoothness of trajectory # Compute second derivative (acceleration/jitter) - velocities = [ - state_history[i] - state_history[i - 1] for i in range(1, len(state_history)) - ] + velocities = [state_history[i] - state_history[i - 1] for i in range(1, len(state_history))] - accelerations = [ - velocities[i] - velocities[i - 1] for i in range(1, len(velocities)) - ] + accelerations = [velocities[i] - velocities[i - 1] for i in range(1, len(velocities))] # Low acceleration = high coherence (smooth motion) # High acceleration = low coherence (jittery motion) diff --git a/tests/test_coherent_decomposition.py b/tests/test_coherent_decomposition.py index ce5dbc2..8e67c10 100644 --- a/tests/test_coherent_decomposition.py +++ b/tests/test_coherent_decomposition.py @@ -191,9 +191,7 @@ def test_coherent_signal_minimal_filtering(self): history = [np.array([float(i)]) for i in range(10)] observation = np.array([10.0]) # Continues trend - filtered, info = filtered_observation( - observation, history, noise_suppression=0.8 - ) + filtered, info = filtered_observation(observation, history, noise_suppression=0.8) # Should be classified as coherent assert info["is_coherent"] == True @@ -208,9 +206,7 @@ def test_noisy_observation_gets_filtered(self): # Add large noise to observation observation = np.array([10.0 + 3.0]) # Expected 10, noise +3 - filtered, info = filtered_observation( - observation, history, noise_suppression=0.8 - ) + filtered, info = filtered_observation(observation, history, noise_suppression=0.8) # Should detect coherence assert info["coherence_score"] > 0.5 @@ -227,9 +223,7 @@ def test_incoherent_signal_high_suppression(self): history = [np.random.randn(2) for _ in range(10)] observation = np.random.randn(2) - filtered, info = filtered_observation( - observation, history, noise_suppression=0.9 - ) + filtered, info = filtered_observation(observation, history, noise_suppression=0.9) # Should be classified as incoherent assert info["is_coherent"] == False @@ -242,9 +236,7 @@ def test_zero_suppression_no_filtering(self): history = [np.array([float(i)]) for i in range(5)] observation = np.array([5.0 + 1.0]) - filtered, info = filtered_observation( - observation, history, noise_suppression=0.0 - ) + filtered, info = filtered_observation(observation, history, noise_suppression=0.0) # Should return original observation assert np.allclose(filtered, observation) @@ -260,9 +252,7 @@ def test_control_moves_toward_target(self): observation = current.copy() history = [current.copy()] - new_state, info = coherent_control_step( - current, target, observation, history, eta=0.2 - ) + new_state, info = coherent_control_step(current, target, observation, history, eta=0.2) # Should move closer to target dist_before = np.linalg.norm(current - target) diff --git a/tests/test_dynamic_invariant.py b/tests/test_dynamic_invariant.py index c67399a..b24889f 100644 --- a/tests/test_dynamic_invariant.py +++ b/tests/test_dynamic_invariant.py @@ -293,9 +293,7 @@ def test_manual_eta_overrides_detection(self): target = np.array([0.0, 0.0]) history = [np.array([10.0 - i * 0.5, 10.0 - i * 0.5]) for i in range(30)] - new_state, info = adaptive_control_step( - current, target, history, eta=0.25, auto_tune=True - ) + new_state, info = adaptive_control_step(current, target, history, eta=0.25, auto_tune=True) # Should use manual eta assert info["eta_used"] == 0.25 @@ -306,9 +304,7 @@ def test_auto_tune_disabled_uses_defaults(self): target = np.array([0.0, 0.0]) history = [np.array([10.0 - i * 0.5, 10.0 - i * 0.5]) for i in range(30)] - new_state, info = adaptive_control_step( - current, target, history, auto_tune=False - ) + new_state, info = adaptive_control_step(current, target, history, auto_tune=False) assert info["parameters_detected"] == False @@ -378,18 +374,14 @@ def test_adaptation_helps_different_scales(self): # Test on slow system current_slow = np.array([1.0, 1.0]) target_slow = np.array([0.0, 0.0]) - history_slow = [ - np.array([2.0 - i * 0.05, 2.0 - i * 0.05]) for i in range(30) - ] + history_slow = [np.array([2.0 - i * 0.05, 2.0 - i * 0.05]) for i in range(30)] _, info_slow = adaptive_control_step(current_slow, target_slow, history_slow) # Test on fast system current_fast = np.array([10.0, 10.0]) target_fast = np.array([0.0, 0.0]) - history_fast = [ - np.array([20.0 - i * 0.5, 20.0 - i * 0.5]) for i in range(30) - ] + history_fast = [np.array([20.0 - i * 0.5, 20.0 - i * 0.5]) for i in range(30)] _, info_fast = adaptive_control_step(current_fast, target_fast, history_fast) diff --git a/tests/test_moving_targets.py b/tests/test_moving_targets.py index 672ff6d..fc51c0a 100644 --- a/tests/test_moving_targets.py +++ b/tests/test_moving_targets.py @@ -161,9 +161,7 @@ def test_stationary_target_acts_like_standard_control(self): np.array([0.0, 0.0]), # Stationary ] - new_robot, velocity, beta = moving_target_control_step( - robot, target, history, eta=0.1 - ) + new_robot, velocity, beta = moving_target_control_step(robot, target, history, eta=0.1) # Should move toward target dist_before = np.linalg.norm(robot - target) @@ -188,9 +186,7 @@ def test_moving_target_anticipates_motion(self): np.array([0.0, 0.0]), # Moving right ] - new_robot_1, velocity, beta = moving_target_control_step( - robot, target, history, eta=0.2 - ) + new_robot_1, velocity, beta = moving_target_control_step(robot, target, history, eta=0.2) # Velocity should be detected as [1.0, 0.0] assert np.allclose(velocity, [1.0, 0.0], atol=1e-6) @@ -299,9 +295,7 @@ def test_moving_target_preserves_physics(self): ] # This should not violate physics - new_robot, velocity, beta = moving_target_control_step( - robot, target, history, eta=0.5 - ) + new_robot, velocity, beta = moving_target_control_step(robot, target, history, eta=0.5) # Beta must be < 1 (subluminal) or equal to velocity if <= 0.99 assert beta < 1.0 diff --git a/tests/test_paradox_coordination.py b/tests/test_paradox_coordination.py index c4953d4..6b56e7c 100644 --- a/tests/test_paradox_coordination.py +++ b/tests/test_paradox_coordination.py @@ -32,9 +32,7 @@ def test_single_agent_no_oscillation(self): ] states = [np.array([0.4, 0.4])] - collective_osc, agent_osc, collective_sim = collective_lightlike_observer( - states, history - ) + collective_osc, agent_osc, collective_sim = collective_lightlike_observer(states, history) # Should not detect oscillation assert collective_osc == False @@ -54,9 +52,7 @@ def test_single_agent_oscillating(self): ] states = [np.array([1.0, 0.0])] - collective_osc, agent_osc, collective_sim = collective_lightlike_observer( - states, history - ) + collective_osc, agent_osc, collective_sim = collective_lightlike_observer(states, history) # Should detect oscillation assert collective_osc == True @@ -73,9 +69,7 @@ def test_two_agents_independent(self): states = [np.array([1.0, 0.0]), np.array([-1.0, 10.0])] histories = [history_0[0], history_1[0]] - collective_osc, agent_osc, collective_sim = collective_lightlike_observer( - states, histories - ) + collective_osc, agent_osc, collective_sim = collective_lightlike_observer(states, histories) # Agents moving in opposite directions - not high similarity assert collective_sim <= 1.0 # Just verify it's a valid value @@ -92,9 +86,7 @@ def test_two_agents_conflicting(self): states = [np.array([0.6, 0.0]), np.array([0.0, 0.6])] histories = [history_0[0], history_1[0]] - collective_osc, agent_osc, collective_sim = collective_lightlike_observer( - states, histories - ) + collective_osc, agent_osc, collective_sim = collective_lightlike_observer(states, histories) # Observer should detect some level of interaction assert isinstance(collective_sim, float) @@ -334,9 +326,7 @@ def test_shared_observer_creates_coordination(self): # Key test: Did coordination happen? # Evidence: collective_oscillating flag or dampings > 0 - coordination_events = sum( - info["coordination_active"] for info in info_history - ) + coordination_events = sum(info["coordination_active"] for info in info_history) # At some point, there should be coordination # (or they converged so fast it wasn't needed - both are success) @@ -348,8 +338,7 @@ def test_emergent_behavior_scales(self): # All agents start at different corners initial_states = [ - np.array([np.cos(i * 2 * np.pi / n_agents), np.sin(i * 2 * np.pi / n_agents)]) - * 5.0 + np.array([np.cos(i * 2 * np.pi / n_agents), np.sin(i * 2 * np.pi / n_agents)]) * 5.0 for i in range(n_agents) ] diff --git a/tests/test_qec.py b/tests/test_qec.py index 6d8cf1a..ab122d9 100644 --- a/tests/test_qec.py +++ b/tests/test_qec.py @@ -72,9 +72,7 @@ def test_high_error_rate_causes_differences(self): np.random.seed(42) encoded = [np.array([1.0, 2.0]) for _ in range(10)] - noisy = apply_measurement_errors( - encoded, error_rate=0.8, error_magnitude=5.0 - ) + noisy = apply_measurement_errors(encoded, error_rate=0.8, error_magnitude=5.0) # At least some should have large errors differences = [np.linalg.norm(noisy[i] - encoded[i]) for i in range(10)] @@ -85,9 +83,7 @@ def test_zero_error_rate_small_noise(self): np.random.seed(42) encoded = [np.array([1.0, 2.0]) for _ in range(10)] - noisy = apply_measurement_errors( - encoded, error_rate=0.0, error_magnitude=1.0 - ) + noisy = apply_measurement_errors(encoded, error_rate=0.0, error_magnitude=1.0) # All should be close to original (only small noise) for i in range(10): diff --git a/tests/test_weak_stereo.py b/tests/test_weak_stereo.py index 1e0b1ae..24e2e94 100644 --- a/tests/test_weak_stereo.py +++ b/tests/test_weak_stereo.py @@ -161,9 +161,7 @@ def test_weak_measurement_preserves_left_right(self): left = np.array([1.0, 0.5]) right = np.array([0.8, 0.5]) - left_m, right_m, _ = stereo_weak_measurement( - left, right, measurement_strength=0.05 - ) + left_m, right_m, _ = stereo_weak_measurement(left, right, measurement_strength=0.05) # Should be similar to original assert np.linalg.norm(left_m - left) < 0.5 @@ -198,9 +196,7 @@ def test_measurements_accumulate(self): right_obs = np.array([0.8, 1.0]) # First step - _, _, measurements1 = weak_stereo_control_step( - current, target, left_obs, right_obs - ) + _, _, measurements1 = weak_stereo_control_step(current, target, left_obs, right_obs) # Second step with accumulated measurements _, _, measurements2 = weak_stereo_control_step( @@ -244,9 +240,7 @@ def test_smooth_trajectory_high_coherence(self): def test_jittery_trajectory_low_coherence(self): """Jittery motion should have low coherence.""" # Oscillating trajectory (not smooth) - trajectory = [ - np.array([float(i % 2), float(i % 2)]) for i in range(10) - ] + trajectory = [np.array([float(i % 2), float(i % 2)]) for i in range(10)] coherence = coherence_metric(trajectory)