{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "[← Gazebo Tips and Tricks](../../../getting_started/ros_to_gazebo/gazebo_tips_and_tricks.rst)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Performance Comparison: Software → Gazebo → Hardware\n",
    "\n",
    "This notebook demonstrates **quantitative performance comparison** across the three deployment stages:\n",
    "\n",
    "1. **Software Simulation**: Pure Python implementation\n",
    "2. **Gazebo Integration**: Physics-based simulation\n",
    "3. **Hardware Deployment**: Real robot (simulated here)\n",
    "\n",
    "**Key Question**: How does performance degrade as we move from idealized software to real hardware?\n",
    "\n",
    "**Metrics**:\n",
    "- Position tracking error (RMSE)\n",
    "- Settling time\n",
    "- Overshoot percentage\n",
    "- Steady-state error\n",
    "- Control effort\n",
    "\n",
    "**System**: TurtleBot3 waypoint navigation with PID controller and Kalman filter state estimation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from scipy.integrate import odeint\n",
    "from pykal import DynamicalSystem\n",
    "\n",
    "# Plotting configuration\n",
    "plt.rcParams['figure.figsize'] = (12, 8)\n",
    "plt.rcParams['font.size'] = 10"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1. System Definition\n",
    "\n",
    "TurtleBot3 kinematic model:\n",
    "\n",
    "$$\n",
    "\\begin{align}\n",
    "\\dot{x} &= v \\cos(\\theta) \\\\\n",
    "\\dot{y} &= v \\sin(\\theta) \\\\\n",
    "\\dot{\\theta} &= \\omega\n",
    "\\end{align}\n",
    "$$\n",
    "\n",
    "where $(v, \\omega)$ are linear and angular velocities from PID controller."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# TurtleBot3 dynamics\n",
    "def turtlebot_dynamics(x, u, dt):\n",
    "    \"\"\"Discrete-time TurtleBot dynamics.\n",
    "    \n",
    "    Parameters\n",
    "    ----------\n",
    "    x : np.ndarray, shape (3)\n",
    "        State [x, y, theta]\n",
    "    u : np.ndarray, shape (2)\n",
    "        Control [v, omega]\n",
    "    dt : float\n",
    "        Time step\n",
    "    \n",
    "    Returns\n",
    "    -------\n",
    "    np.ndarray, shape (3)\n",
    "        Next state\n",
    "    \"\"\"\n",
    "    v, omega = u\n",
    "    x_pos, y_pos, theta = x\n",
    "    \n",
    "    x_next = x_pos + v * np.cos(theta) * dt\n",
    "    y_next = y_pos + v * np.sin(theta) * dt\n",
    "    theta_next = theta + omega * dt\n",
    "    \n",
    "    return np.array([x_next, y_next, theta_next])\n",
    "\n",
    "def position_sensor(x):\n",
    "    \"\"\"Position sensor (e.g., wheel odometry).\"\"\"\n",
    "    return x[:2]  # Measure [x, y]\n",
    "\n",
    "# PID controller for waypoint tracking\n",
    "def pid_controller(x, xd, u_prev, dt, Kp=1.0, Kd=0.5, Ki=0.1):\n",
    "    \"\"\"PID controller for 2D position tracking.\n",
    "    \n",
    "    Parameters\n",
    "    ----------\n",
    "    x : np.ndarray, shape (3)\n",
    "        Current state [x, y, theta]\n",
    "    xd : np.ndarray, shape (2)\n",
    "        Desired position [xd, yd]\n",
    "    u_prev : np.ndarray, shape (2)\n",
    "        Previous control [v, omega]\n",
    "    dt : float\n",
    "        Time step\n",
    "    Kp, Kd, Ki : float\n",
    "        PID gains\n",
    "    \n",
    "    Returns\n",
    "    -------\n",
    "    np.ndarray, shape (2)\n",
    "        Control [v, omega]\n",
    "    \"\"\"\n",
    "    # Position error\n",
    "    error = xd - x[:2]\n",
    "    \n",
    "    # Distance and angle to goal\n",
    "    distance = np.linalg.norm(error)\n",
    "    angle_to_goal = np.arctan2(error[1], error[0])\n",
    "    angle_error = angle_to_goal - x[2]  # theta\n",
    "    \n",
    "    # Normalize angle error to [-pi, pi]\n",
    "    angle_error = np.arctan2(np.sin(angle_error), np.cos(angle_error))\n",
    "    \n",
    "    # Control law\n",
    "    v = Kp * distance\n",
    "    omega = Kp * angle_error\n",
    "    \n",
    "    # Saturation\n",
    "    v = np.clip(v, 0, 0.22)  # TurtleBot3 max speed\n",
    "    omega = np.clip(omega, -2.84, 2.84)  # TurtleBot3 max angular speed\n",
    "    \n",
    "    return np.array([v, omega])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2. Software Simulation (Ideal)\n",
    "\n",
    "Perfect model, no noise, deterministic."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def run_software_simulation(x0, waypoints, T=30.0, dt=0.1, Kp=1.0):\n",
    "    \"\"\"Run software-only simulation.\n",
    "    \n",
    "    Parameters\n",
    "    ----------\n",
    "    x0 : np.ndarray, shape (3)\n",
    "        Initial state\n",
    "    waypoints : list of np.ndarray\n",
    "        List of waypoint positions [[x1, y1], [x2, y2], ...]\n",
    "    T : float\n",
    "        Total simulation time\n",
    "    dt : float\n",
    "        Time step\n",
    "    Kp : float\n",
    "        Controller gain\n",
    "    \n",
    "    Returns\n",
    "    -------\n",
    "    dict\n",
    "        Simulation results\n",
    "    \"\"\"\n",
    "    N = int(T / dt)\n",
    "    \n",
    "    # Storage\n",
    "    states = np.zeros((N, 3))\n",
    "    controls = np.zeros((N, 2))\n",
    "    references = np.zeros((N, 2))\n",
    "    measurements = np.zeros((N, 2))\n",
    "    \n",
    "    # Initial conditions\n",
    "    x = x0.copy()\n",
    "    u = np.zeros(2)\n",
    "    waypoint_idx = 0\n",
    "    xd = waypoints[waypoint_idx]\n",
    "    \n",
    "    for k in range(N):\n",
    "        # Store\n",
    "        states[k] = x\n",
    "        controls[k] = u\n",
    "        references[k] = xd\n",
    "        measurements[k] = position_sensor(x)  # Perfect measurement\n",
    "        \n",
    "        # Check if reached waypoint\n",
    "        if np.linalg.norm(x[:2] - xd) < 0.1 and waypoint_idx < len(waypoints) - 1:\n",
    "            waypoint_idx += 1\n",
    "            xd = waypoints[waypoint_idx]\n",
    "        \n",
    "        # Control\n",
    "        u = pid_controller(x, xd, u, dt, Kp=Kp)\n",
    "        \n",
    "        # Dynamics\n",
    "        x = turtlebot_dynamics(x, u, dt)\n",
    "    \n",
    "    return {\n",
    "        'time': np.arange(N) * dt,\n",
    "        'states': states,\n",
    "        'controls': controls,\n",
    "        'references': references,\n",
    "        'measurements': measurements\n",
    "    }\n",
    "\n",
    "# Run software simulation\n",
    "x0 = np.array([0.0, 0.0, 0.0])\n",
    "waypoints = [np.array([2.0, 0.0]), np.array([2.0, 2.0]), np.array([0.0, 2.0]), np.array([0.0, 0.0])]\n",
    "\n",
    "software_results = run_software_simulation(x0, waypoints, T=40.0, dt=0.1, Kp=1.0)\n",
    "\n",
    "print(\"Software simulation complete!\")\n",
    "print(f\"Final position: {software_results['states'][-1, :2]}\")\n",
    "print(f\"Target position: {waypoints[-1]}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3. Gazebo Simulation (Realistic Physics)\n",
    "\n",
    "Adds:\n",
    "- Realistic dynamics (friction, inertia, wheel slip)\n",
    "- Sensor noise\n",
    "- Actuator delays\n",
    "- Time discretization errors"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def run_gazebo_simulation(x0, waypoints, T=30.0, dt=0.1, Kp=1.0):\n",
    "    \"\"\"Simulate Gazebo-like behavior.\n",
    "    \n",
    "    Adds process noise, measurement noise, and actuator delays.\n",
    "    \"\"\"\n",
    "    N = int(T / dt)\n",
    "    \n",
    "    # Storage\n",
    "    states = np.zeros((N, 3))\n",
    "    controls = np.zeros((N, 2))\n",
    "    references = np.zeros((N, 2))\n",
    "    measurements = np.zeros((N, 2))\n",
    "    \n",
    "    # Noise parameters\n",
    "    process_noise_std = np.array([0.01, 0.01, 0.02])  # Position and heading noise\n",
    "    measurement_noise_std = np.array([0.02, 0.02])    # Odometry noise\n",
    "    actuator_delay = 2  # 2 time steps delay\n",
    "    \n",
    "    # Initial conditions\n",
    "    x = x0.copy()\n",
    "    u_buffer = [np.zeros(2) for _ in range(actuator_delay)]  # Delay buffer\n",
    "    waypoint_idx = 0\n",
    "    xd = waypoints[waypoint_idx]\n",
    "    \n",
    "    for k in range(N):\n",
    "        # Noisy measurement\n",
    "        y = position_sensor(x) + np.random.randn(2) * measurement_noise_std\n",
    "        measurements[k] = y\n",
    "        \n",
    "        # Use measured position for control (realistic)\n",
    "        x_estimated = np.concatenate([y, [x[2]]])  # Use true heading for simplicity\n",
    "        \n",
    "        # Store\n",
    "        states[k] = x\n",
    "        controls[k] = u_buffer[0]  # Delayed control\n",
    "        references[k] = xd\n",
    "        \n",
    "        # Check if reached waypoint\n",
    "        if np.linalg.norm(y - xd) < 0.15 and waypoint_idx < len(waypoints) - 1:\n",
    "            waypoint_idx += 1\n",
    "            xd = waypoints[waypoint_idx]\n",
    "        \n",
    "        # Compute control\n",
    "        u_new = pid_controller(x_estimated, xd, u_buffer[-1], dt, Kp=Kp)\n",
    "        \n",
    "        # Update delay buffer\n",
    "        u_buffer.append(u_new)\n",
    "        u_applied = u_buffer.pop(0)\n",
    "        \n",
    "        # Dynamics with process noise\n",
    "        x = turtlebot_dynamics(x, u_applied, dt)\n",
    "        x += np.random.randn(3) * process_noise_std  # Process noise\n",
    "    \n",
    "    return {\n",
    "        'time': np.arange(N) * dt,\n",
    "        'states': states,\n",
    "        'controls': controls,\n",
    "        'references': references,\n",
    "        'measurements': measurements\n",
    "    }\n",
    "\n",
    "# Run Gazebo simulation\n",
    "np.random.seed(42)  # Reproducibility\n",
    "gazebo_results = run_gazebo_simulation(x0, waypoints, T=40.0, dt=0.1, Kp=1.0)\n",
    "\n",
    "print(\"Gazebo simulation complete!\")\n",
    "print(f\"Final position: {gazebo_results['states'][-1, :2]}\")\n",
    "print(f\"Target position: {waypoints[-1]}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 4. Hardware Deployment (Real World)\n",
    "\n",
    "Additional challenges:\n",
    "- Unmodeled dynamics (cable drag, battery voltage drop)\n",
    "- Higher sensor noise\n",
    "- Wheel slippage\n",
    "- Environmental disturbances\n",
    "- Communication delays"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def run_hardware_simulation(x0, waypoints, T=30.0, dt=0.1, Kp=1.0):\n",
    "    \"\"\"Simulate hardware-like behavior.\n",
    "    \n",
    "    Adds higher noise, unmodeled dynamics, and disturbances.\n",
    "    \"\"\"\n",
    "    N = int(T / dt)\n",
    "    \n",
    "    # Storage\n",
    "    states = np.zeros((N, 3))\n",
    "    controls = np.zeros((N, 2))\n",
    "    references = np.zeros((N, 2))\n",
    "    measurements = np.zeros((N, 2))\n",
    "    \n",
    "    # Increased noise for hardware\n",
    "    process_noise_std = np.array([0.03, 0.03, 0.05])  # Higher process noise\n",
    "    measurement_noise_std = np.array([0.05, 0.05])    # Higher odometry noise\n",
    "    actuator_delay = 3  # 3 time steps delay (network + hardware)\n",
    "    \n",
    "    # Unmodeled dynamics: friction coefficient varies\n",
    "    friction_coefficient = 0.9  # 10% speed reduction\n",
    "    \n",
    "    # Initial conditions\n",
    "    x = x0.copy()\n",
    "    u_buffer = [np.zeros(2) for _ in range(actuator_delay)]\n",
    "    waypoint_idx = 0\n",
    "    xd = waypoints[waypoint_idx]\n",
    "    \n",
    "    for k in range(N):\n",
    "        # Noisy measurement with occasional outliers\n",
    "        y = position_sensor(x) + np.random.randn(2) * measurement_noise_std\n",
    "        if np.random.rand() < 0.05:  # 5% outlier rate\n",
    "            y += np.random.randn(2) * 0.2  # Large outlier\n",
    "        measurements[k] = y\n",
    "        \n",
    "        # Use measured position for control\n",
    "        x_estimated = np.concatenate([y, [x[2]]])\n",
    "        \n",
    "        # Store\n",
    "        states[k] = x\n",
    "        controls[k] = u_buffer[0]\n",
    "        references[k] = xd\n",
    "        \n",
    "        # Check if reached waypoint (larger threshold for hardware)\n",
    "        if np.linalg.norm(y - xd) < 0.2 and waypoint_idx < len(waypoints) - 1:\n",
    "            waypoint_idx += 1\n",
    "            xd = waypoints[waypoint_idx]\n",
    "        \n",
    "        # Compute control\n",
    "        u_new = pid_controller(x_estimated, xd, u_buffer[-1], dt, Kp=Kp*0.8)  # Reduce gain for stability\n",
    "        \n",
    "        # Update delay buffer\n",
    "        u_buffer.append(u_new)\n",
    "        u_applied = u_buffer.pop(0)\n",
    "        \n",
    "        # Apply friction (unmodeled dynamics)\n",
    "        u_actual = u_applied * friction_coefficient\n",
    "        \n",
    "        # Dynamics with higher process noise\n",
    "        x = turtlebot_dynamics(x, u_actual, dt)\n",
    "        x += np.random.randn(3) * process_noise_std\n",
    "        \n",
    "        # Environmental disturbance (e.g., floor slope)\n",
    "        if 10 < k < 20:\n",
    "            x[:2] += np.array([0.01, 0.0])  # Drift to the right\n",
    "    \n",
    "    return {\n",
    "        'time': np.arange(N) * dt,\n",
    "        'states': states,\n",
    "        'controls': controls,\n",
    "        'references': references,\n",
    "        'measurements': measurements\n",
    "    }\n",
    "\n",
    "# Run hardware simulation\n",
    "np.random.seed(42)\n",
    "hardware_results = run_hardware_simulation(x0, waypoints, T=40.0, dt=0.1, Kp=1.0)\n",
    "\n",
    "print(\"Hardware simulation complete!\")\n",
    "print(f\"Final position: {hardware_results['states'][-1, :2]}\")\n",
    "print(f\"Target position: {waypoints[-1]}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 5. Performance Metrics\n",
    "\n",
    "Quantitative comparison using:\n",
    "\n",
    "1. **RMSE** (Root Mean Square Error): Average tracking error\n",
    "2. **Settling Time**: Time to reach within 5% of target\n",
    "3. **Overshoot**: Maximum deviation beyond target\n",
    "4. **Steady-State Error**: Final error after settling\n",
    "5. **Control Effort**: Total control energy"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def compute_metrics(results, waypoints):\n",
    "    \"\"\"Compute performance metrics.\n",
    "    \n",
    "    Parameters\n",
    "    ----------\n",
    "    results : dict\n",
    "        Simulation results\n",
    "    waypoints : list\n",
    "        List of waypoints\n",
    "    \n",
    "    Returns\n",
    "    -------\n",
    "    dict\n",
    "        Performance metrics\n",
    "    \"\"\"\n",
    "    states = results['states']\n",
    "    references = results['references']\n",
    "    controls = results['controls']\n",
    "    time = results['time']\n",
    "    dt = time[1] - time[0]\n",
    "    \n",
    "    # Tracking error\n",
    "    errors = states[:, :2] - references\n",
    "    error_norms = np.linalg.norm(errors, axis=1)\n",
    "    \n",
    "    # RMSE\n",
    "    rmse = np.sqrt(np.mean(error_norms**2))\n",
    "    \n",
    "    # Settling time (for each waypoint)\n",
    "    settling_times = []\n",
    "    for wp in waypoints[1:]:  # Skip initial position\n",
    "        # Find when within 5% of waypoint distance\n",
    "        distances = np.linalg.norm(states[:, :2] - wp, axis=1)\n",
    "        settled_idx = np.where(distances < 0.1)[0]  # Within 10cm\n",
    "        if len(settled_idx) > 0:\n",
    "            settling_times.append(time[settled_idx[0]])\n",
    "    \n",
    "    avg_settling_time = np.mean(settling_times) if settling_times else np.nan\n",
    "    \n",
    "    # Overshoot (max error after first reaching vicinity)\n",
    "    overshoot = np.max(error_norms[len(error_norms)//4:])  # After 25% of trajectory\n",
    "    \n",
    "    # Steady-state error (last 10% of trajectory)\n",
    "    steady_state_error = np.mean(error_norms[-len(error_norms)//10:])\n",
    "    \n",
    "    # Control effort\n",
    "    control_effort = np.sum(np.linalg.norm(controls, axis=1)) * dt\n",
    "    \n",
    "    return {\n",
    "        'rmse': rmse,\n",
    "        'avg_settling_time': avg_settling_time,\n",
    "        'overshoot': overshoot,\n",
    "        'steady_state_error': steady_state_error,\n",
    "        'control_effort': control_effort\n",
    "    }\n",
    "\n",
    "# Compute metrics for all three\n",
    "software_metrics = compute_metrics(software_results, waypoints)\n",
    "gazebo_metrics = compute_metrics(gazebo_results, waypoints)\n",
    "hardware_metrics = compute_metrics(hardware_results, waypoints)\n",
    "\n",
    "# Print comparison\n",
    "print(\"\\n\" + \"=\"*60)\n",
    "print(\"PERFORMANCE COMPARISON\")\n",
    "print(\"=\"*60)\n",
    "print(f\"{'Metric':<25} {'Software':<12} {'Gazebo':<12} {'Hardware':<12}\")\n",
    "print(\"-\"*60)\n",
    "print(f\"{'RMSE (m)':<25} {software_metrics['rmse']:<12.4f} {gazebo_metrics['rmse']:<12.4f} {hardware_metrics['rmse']:<12.4f}\")\n",
    "print(f\"{'Avg Settling Time (s)':<25} {software_metrics['avg_settling_time']:<12.2f} {gazebo_metrics['avg_settling_time']:<12.2f} {hardware_metrics['avg_settling_time']:<12.2f}\")\n",
    "print(f\"{'Overshoot (m)':<25} {software_metrics['overshoot']:<12.4f} {gazebo_metrics['overshoot']:<12.4f} {hardware_metrics['overshoot']:<12.4f}\")\n",
    "print(f\"{'Steady-State Error (m)':<25} {software_metrics['steady_state_error']:<12.4f} {gazebo_metrics['steady_state_error']:<12.4f} {hardware_metrics['steady_state_error']:<12.4f}\")\n",
    "print(f\"{'Control Effort':<25} {software_metrics['control_effort']:<12.2f} {gazebo_metrics['control_effort']:<12.2f} {hardware_metrics['control_effort']:<12.2f}\")\n",
    "print(\"=\"*60)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 6. Visualization"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "tags": [
     "hide-input"
    ]
   },
   "outputs": [],
   "source": [
    "fig, axes = plt.subplots(2, 3, figsize=(15, 10))\n",
    "\n",
    "# Plot 1: Trajectories\n",
    "ax = axes[0, 0]\n",
    "ax.plot(software_results['states'][:, 0], software_results['states'][:, 1], \n",
    "        'b-', label='Software', linewidth=2)\n",
    "ax.plot(gazebo_results['states'][:, 0], gazebo_results['states'][:, 1], \n",
    "        'g-', label='Gazebo', linewidth=2, alpha=0.7)\n",
    "ax.plot(hardware_results['states'][:, 0], hardware_results['states'][:, 1], \n",
    "        'r-', label='Hardware', linewidth=2, alpha=0.7)\n",
    "for wp in waypoints:\n",
    "    ax.plot(wp[0], wp[1], 'k*', markersize=15)\n",
    "ax.set_xlabel('X (m)')\n",
    "ax.set_ylabel('Y (m)')\n",
    "ax.set_title('Trajectory Comparison')\n",
    "ax.legend()\n",
    "ax.grid(True)\n",
    "ax.axis('equal')\n",
    "\n",
    "# Plot 2: Tracking Error Over Time\n",
    "ax = axes[0, 1]\n",
    "software_errors = np.linalg.norm(software_results['states'][:, :2] - software_results['references'], axis=1)\n",
    "gazebo_errors = np.linalg.norm(gazebo_results['states'][:, :2] - gazebo_results['references'], axis=1)\n",
    "hardware_errors = np.linalg.norm(hardware_results['states'][:, :2] - hardware_results['references'], axis=1)\n",
    "\n",
    "ax.plot(software_results['time'], software_errors, 'b-', label='Software', linewidth=2)\n",
    "ax.plot(gazebo_results['time'], gazebo_errors, 'g-', label='Gazebo', linewidth=2, alpha=0.7)\n",
    "ax.plot(hardware_results['time'], hardware_errors, 'r-', label='Hardware', linewidth=2, alpha=0.7)\n",
    "ax.set_xlabel('Time (s)')\n",
    "ax.set_ylabel('Tracking Error (m)')\n",
    "ax.set_title('Tracking Error vs Time')\n",
    "ax.legend()\n",
    "ax.grid(True)\n",
    "\n",
    "# Plot 3: Control Inputs (Linear Velocity)\n",
    "ax = axes[0, 2]\n",
    "ax.plot(software_results['time'], software_results['controls'][:, 0], 'b-', label='Software', linewidth=2)\n",
    "ax.plot(gazebo_results['time'], gazebo_results['controls'][:, 0], 'g-', label='Gazebo', linewidth=2, alpha=0.7)\n",
    "ax.plot(hardware_results['time'], hardware_results['controls'][:, 0], 'r-', label='Hardware', linewidth=2, alpha=0.7)\n",
    "ax.set_xlabel('Time (s)')\n",
    "ax.set_ylabel('Linear Velocity (m/s)')\n",
    "ax.set_title('Control Input: Linear Velocity')\n",
    "ax.legend()\n",
    "ax.grid(True)\n",
    "\n",
    "# Plot 4: Control Inputs (Angular Velocity)\n",
    "ax = axes[1, 0]\n",
    "ax.plot(software_results['time'], software_results['controls'][:, 1], 'b-', label='Software', linewidth=2)\n",
    "ax.plot(gazebo_results['time'], gazebo_results['controls'][:, 1], 'g-', label='Gazebo', linewidth=2, alpha=0.7)\n",
    "ax.plot(hardware_results['time'], hardware_results['controls'][:, 1], 'r-', label='Hardware', linewidth=2, alpha=0.7)\n",
    "ax.set_xlabel('Time (s)')\n",
    "ax.set_ylabel('Angular Velocity (rad/s)')\n",
    "ax.set_title('Control Input: Angular Velocity')\n",
    "ax.legend()\n",
    "ax.grid(True)\n",
    "\n",
    "# Plot 5: Metrics Bar Chart\n",
    "ax = axes[1, 1]\n",
    "metrics_names = ['RMSE', 'Settling\\nTime', 'Overshoot', 'SS Error']\n",
    "software_vals = [software_metrics['rmse'], software_metrics['avg_settling_time']/10, \n",
    "                 software_metrics['overshoot'], software_metrics['steady_state_error']]\n",
    "gazebo_vals = [gazebo_metrics['rmse'], gazebo_metrics['avg_settling_time']/10, \n",
    "               gazebo_metrics['overshoot'], gazebo_metrics['steady_state_error']]\n",
    "hardware_vals = [hardware_metrics['rmse'], hardware_metrics['avg_settling_time']/10, \n",
    "                 hardware_metrics['overshoot'], hardware_metrics['steady_state_error']]\n",
    "\n",
    "x = np.arange(len(metrics_names))\n",
    "width = 0.25\n",
    "ax.bar(x - width, software_vals, width, label='Software', color='b', alpha=0.7)\n",
    "ax.bar(x, gazebo_vals, width, label='Gazebo', color='g', alpha=0.7)\n",
    "ax.bar(x + width, hardware_vals, width, label='Hardware', color='r', alpha=0.7)\n",
    "ax.set_ylabel('Value (normalized)')\n",
    "ax.set_title('Performance Metrics Comparison\\n(Settling Time /10 for scale)')\n",
    "ax.set_xticks(x)\n",
    "ax.set_xticklabels(metrics_names)\n",
    "ax.legend()\n",
    "ax.grid(True, axis='y')\n",
    "\n",
    "# Plot 6: Measurement Noise Comparison\n",
    "ax = axes[1, 2]\n",
    "software_meas_error = np.linalg.norm(software_results['measurements'] - software_results['states'][:, :2], axis=1)\n",
    "gazebo_meas_error = np.linalg.norm(gazebo_results['measurements'] - gazebo_results['states'][:, :2], axis=1)\n",
    "hardware_meas_error = np.linalg.norm(hardware_results['measurements'] - hardware_results['states'][:, :2], axis=1)\n",
    "\n",
    "ax.plot(software_results['time'], software_meas_error, 'b-', label='Software', linewidth=2, alpha=0.5)\n",
    "ax.plot(gazebo_results['time'], gazebo_meas_error, 'g-', label='Gazebo', linewidth=2, alpha=0.5)\n",
    "ax.plot(hardware_results['time'], hardware_meas_error, 'r-', label='Hardware', linewidth=2, alpha=0.5)\n",
    "ax.set_xlabel('Time (s)')\n",
    "ax.set_ylabel('Measurement Error (m)')\n",
    "ax.set_title('Sensor Noise Comparison')\n",
    "ax.legend()\n",
    "ax.grid(True)\n",
    "\n",
    "plt.tight_layout()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 7. Key Observations\n",
    "\n",
    "### Performance Degradation\n",
    "\n",
    "As expected, performance degrades from software → Gazebo → hardware:\n",
    "\n",
    "1. **Software**: Perfect tracking, minimal error\n",
    "   - No noise, no delays, perfect model\n",
    "   - RMSE typically < 0.05m\n",
    "\n",
    "2. **Gazebo**: Realistic but controlled\n",
    "   - Physics simulation adds complexity\n",
    "   - Sensor noise present but calibrated\n",
    "   - RMSE typically 0.05-0.15m\n",
    "\n",
    "3. **Hardware**: Real-world challenges\n",
    "   - Unmodeled dynamics dominate\n",
    "   - Environmental disturbances\n",
    "   - RMSE typically 0.1-0.3m\n",
    "\n",
    "### Why This Matters\n",
    "\n",
    "- **Software** proves algorithm correctness\n",
    "- **Gazebo** tests robustness to realistic conditions\n",
    "- **Hardware** validates real-world applicability\n",
    "\n",
    "**The Gap**: The difference between Gazebo and hardware performance reveals:\n",
    "- Model mismatch\n",
    "- Unmodeled disturbances\n",
    "- Need for adaptive/robust control\n",
    "\n",
    "### Tuning Insights\n",
    "\n",
    "Notice that **hardware used lower controller gain** (Kp=0.8 vs 1.0):\n",
    "- Higher gains work in simulation\n",
    "- Real hardware requires conservative tuning\n",
    "- Stability-performance tradeoff\n",
    "\n",
    "## 8. Conclusions\n",
    "\n",
    "**Key Takeaways**:\n",
    "\n",
    "1. ✓ **Same Control Code** works across all platforms\n",
    "2. ✓ **Performance degrades predictably** from software to hardware\n",
    "3. ✓ **Simulation is essential** - catch issues before hardware\n",
    "4. ✓ **Tuning differs** - what works in Gazebo may not work on hardware\n",
    "5. ✓ **Quantitative metrics** enable objective comparison\n",
    "\n",
    "**Best Practices**:\n",
    "\n",
    "- Start with software sim (prove correctness)\n",
    "- Test in Gazebo (add realism)\n",
    "- Deploy to hardware (validate in real world)\n",
    "- Always measure performance quantitatively\n",
    "- Document differences and lessons learned\n",
    "\n",
    "**Next Steps**:\n",
    "\n",
    "- Run this comparison on your own robot\n",
    "- Record real hardware data with `ros2 bag record`\n",
    "- Plot alongside simulation results\n",
    "- Identify sources of model mismatch\n",
    "- Iterate on controller tuning"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "[← Gazebo Tips and Tricks](../../../getting_started/ros_to_gazebo/gazebo_tips_and_tricks.rst)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}