Configuration and Tuning
========================

All MPC parameters are configured in the ``all_settings`` package. This page explains the available settings and provides guidance on tuning for different scenarios.

Settings Structure
------------------

The MPC module uses the ``MPCSettings`` class from ``all_settings/all_settings/all_settings.py``:

.. code-block:: python

    class MPCSettings(metaclass=Settings):
        """Settings for Model Predictive Control"""
        N: int = 10
        DT: float = 0.1
        L_F: float = 1.58/2
        L_R: float = 1.58/2
        V_MIN: float = 0.0
        V_MAX: float = 10.0
        A_MIN: float = -5.0
        A_MAX: float = 3.0
        DF_MIN: float = -0.6
        DF_MAX: float = 0.6
        A_DOT_MIN: float = -10.0
        A_DOT_MAX: float = 10.0
        DF_DOT_MIN: float = -0.5
        DF_DOT_MAX: float = 0.5
        Q: list = [5., 5., 3., 0., 0.]
        R: list = [0., 0.]
        P: list = [5., 5., 100., 0.3, 0.1]
        RUNTIME_FREQUENCY: float = 100
        nlpsolver = FATROPSolver
        ivpsolver = RK4Solver
        PUBLISH: bool = True
        USE_WARMSTARTING: bool = False

Prediction Horizon Parameters
------------------------------

N (Prediction Horizon Length)
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

**Type:** ``int``

**Default:** ``10``

**Description:** Number of time steps to predict into the future.

**Impact:**

- **Larger N**: Better long-term planning, sees farther ahead, but slower computation
- **Smaller N**: Faster computation, more reactive, but shorter lookahead

**Tuning guidance:**

- At 10 m/s with N=10 and DT=0.1s, the MPC looks ahead 10 meters
- Increase N if the vehicle overshoots corners (not planning far enough ahead)
- Decrease N if solve times exceed 5-10ms consistently
- Typical range: 8-15 steps

DT (Time Step)
~~~~~~~~~~~~~~

**Type:** ``float``

**Default:** ``0.1`` seconds

**Description:** Time between prediction steps in the MPC horizon.

**Impact:**

- **Larger DT**: Looks farther ahead with same N, but less accurate dynamics integration
- **Smaller DT**: More accurate integration, finer control resolution, but more computation

**Tuning guidance:**

- Should match the temporal resolution of the reference trajectory
- 100ms (10 Hz) is standard for autonomous racing at moderate speeds
- Generally shouldn't need to change unless vehicle dynamics are very fast
- Must be larger than control loop period (currently 10ms = 100 Hz)

Vehicle Parameters
------------------

L_F and L_R (Wheelbase)
~~~~~~~~~~~~~~~~~~~~~~~

**Type:** ``float``

**Default:** ``0.79`` meters (1.58/2)

**Description:** Distance from center of gravity to front/rear axles.

**Impact:**

- Affects turning radius and vehicle dynamics prediction
- Incorrect values lead to systematic tracking errors

**Tuning guidance:**

- Measure from the physical vehicle
- Sum should equal total wheelbase: L_F + L_R = 1.58m
- CG position can be estimated or measured with scales
- For symmetric weight distribution: L_F ≈ L_R

Velocity Constraints
--------------------

V_MIN and V_MAX
~~~~~~~~~~~~~~~

**Type:** ``float``

**Default:** V_MIN = ``0.0`` m/s, V_MAX = ``10.0`` m/s

**Description:** Minimum and maximum allowed velocities.

**Impact:**

- V_MIN = 0 prevents reverse driving
- V_MAX limits top speed for safety

**Tuning guidance:**

- Set V_MAX based on vehicle capabilities and safety requirements
- Can reduce V_MAX during testing or on tight tracks
- Consider tire grip limits and downforce at high speeds

Acceleration Constraints
------------------------

A_MIN and A_MAX
~~~~~~~~~~~~~~~

**Type:** ``float``

**Default:** A_MIN = ``-5.0`` m/s², A_MAX = ``3.0`` m/s²

**Description:** Minimum (braking) and maximum (acceleration) limits.

**Impact:**

- Defines braking and acceleration performance envelope
- Asymmetric limits reflect that braking is typically stronger than acceleration

**Tuning guidance:**

- Measure actual vehicle capabilities with data logging
- Braking: Limited by tire grip (~0.8-1.0g on dry pavement)
- Acceleration: Limited by motor torque and grip
- Include safety margins (e.g., if vehicle can brake at -7 m/s², use -5 m/s²)

A_DOT_MIN and A_DOT_MAX (Jerk Limits)
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

**Type:** ``float``

**Default:** A_DOT_MIN = ``-10.0`` m/s³, A_DOT_MAX = ``10.0`` m/s³

**Description:** Rate of change of acceleration (jerk) limits.

**Impact:**

- Prevents sudden changes in acceleration
- Improves passenger comfort and mechanical stress

**Note:** Currently commented out in the implementation (``mpc_controller.py:138``) but included for future use.

Steering Constraints
--------------------

DF_MIN and DF_MAX
~~~~~~~~~~~~~~~~~

**Type:** ``float``

**Default:** DF_MIN = ``-0.6`` rad, DF_MAX = ``0.6`` rad

**Description:** Minimum and maximum steering angles (±34 degrees).

**Impact:**

- Defines maximum turning radius
- Prevents mechanical damage to steering system

**Tuning guidance:**

- Set based on physical steering limits
- Include mechanical safety margins
- Consider that larger angles may not be effective due to tire slip

DF_DOT_MIN and DF_DOT_MAX
~~~~~~~~~~~~~~~~~~~~~~~~~~

**Type:** ``float``

**Default:** DF_DOT_MIN = ``-0.5`` rad/s, DF_DOT_MAX = ``0.5`` rad/s

**Description:** Maximum rate of change of steering angle.

**Impact:**

- Prevents violent steering motions
- Ensures smooth, stable control
- Accounts for steering actuator speed limits

**Tuning guidance:**

- Set based on steering motor capabilities
- Smaller values = smoother but slower steering response
- Larger values = faster response but potential instability
- Test empirically: increase until oscillations appear, then reduce by 20-30%

Cost Function Weights
----------------------

Q (State Weights)
~~~~~~~~~~~~~~~~~

**Type:** ``list[float]`` (length 5)

**Default:** ``[5.0, 5.0, 3.0, 0.0, 0.0]``

**Description:** Weights for tracking errors in ``[x, y, heading, velocity, steering_angle]``.

**Impact:**

- Higher weights = more aggressive tracking of that state
- Zero weights = don't care about tracking that state

**Default interpretation:**

- **x, y (5.0)**: High priority on position tracking (stay on path)
- **heading (3.0)**: Moderate priority on orientation (point in right direction)
- **velocity, steering (0.0)**: No penalty for deviating from reference values

**Tuning guidance:**

.. list-table::
   :widths: 20 80
   :header-rows: 1

   * - Symptom
     - Adjustment
   * - Cuts corners too much
     - Increase Q[0] and Q[1] (position weights)
   * - Oscillates side-to-side
     - Decrease Q[0] and Q[1], or increase R weights
   * - Doesn't face forward in turns
     - Increase Q[2] (heading weight)
   * - Too slow through corners
     - Leave Q[3] = 0 to allow speed variation

R (Control Weights)
~~~~~~~~~~~~~~~~~~~

**Type:** ``list[float]`` (length 2)

**Default:** ``[0.0, 0.0]``

**Description:** Weights for control effort in ``[acceleration, steering_rate]``.

**Impact:**

- Higher weights = smoother, less aggressive control
- Zero weights = no penalty for large control inputs

**Default interpretation:**

- Both zero = prioritize performance over smoothness
- Appropriate for racing where maximum performance is desired

**Tuning guidance:**

.. list-table::
   :widths: 20 80
   :header-rows: 1

   * - Symptom
     - Adjustment
   * - Jerky acceleration
     - Increase R[0] (e.g., 0.1-1.0)
   * - Violent steering motions
     - Increase R[1] (e.g., 0.1-1.0)
   * - Too conservative/slow
     - Decrease R weights (toward 0)

**Example values:**

.. code-block:: python

    # Aggressive racing (default)
    R = [0.0, 0.0]

    # Smooth comfort mode
    R = [1.0, 5.0]

    # Balanced performance
    R = [0.1, 0.5]

P (Terminal Cost Weights)
~~~~~~~~~~~~~~~~~~~~~~~~~~

**Type:** ``list[float]`` (length 5)

**Default:** ``[5.0, 5.0, 100.0, 0.3, 0.1]``

**Description:** Weights for terminal state (end of prediction horizon) in ``[x, y, heading, velocity, steering]``.

**Impact:**

- Encourages vehicle to be "headed in the right direction" at end of horizon
- Improves stability and long-term planning

**Default interpretation:**

- **heading (100.0)**: Very high priority on pointing correctly at horizon end
- **x, y (5.0)**: Same as stage cost Q
- **velocity, steering (0.3, 0.1)**: Small penalty to avoid extreme values

**Tuning guidance:**

- Generally P[2] (heading) should be much larger than Q[2]
- P[0], P[1] typically match Q[0], Q[1]
- Advanced: Compute P from CARE equation (see ``mpc_controller.py:98-109``)

Solver Configuration
--------------------

nlpsolver
~~~~~~~~~

**Type:** Solver class reference

**Default:** ``FATROPSolver``

**Options:**

- ``FATROPSolver``: Fast structure-exploiting solver (recommended)
- ``IPOPTSolver``: General-purpose interior-point solver (slower but more robust)

**FATROP options:**

.. code-block:: python

    FATROPSolver.opts = {
        'structure_detection': 'auto',  # Automatically detect problem structure
        'expand': False,                 # Don't expand MX to SX (faster)
        'debug': False,                  # Disable debug output
        'fatrop.print_level': -1,       # Suppress solver output
        'print_time': False              # Don't print solve times
    }

**IPOPT options:**

.. code-block:: python

    IPOPTSolver.opts = {
        'expand': False,                 # Don't expand MX to SX
        'ipopt.linear_solver': 'MA27',   # Linear solver (MA27, MA57, MUMPS)
        'print_time': False,             # Don't print solve times
        'ipopt.print_level': 0           # Suppress solver output
    }

**Tuning guidance:**

- Use FATROP for best performance (2-5x faster than IPOPT)
- Switch to IPOPT if FATROP fails to converge
- For IPOPT, try different linear solvers if one fails

ivpsolver
~~~~~~~~~

**Type:** Integrator class reference

**Default:** ``RK4Solver``

**Options:**

- ``RK4Solver``: 4th-order Runge-Kutta (recommended)
- ``MidpointSolver``: 2nd-order midpoint method (faster but less accurate)

**RK4 options:**

.. code-block:: python

    RK4Solver.n = 1       # Number of substeps per DT
    RK4Solver.method = 'rk4'

**Midpoint options:**

.. code-block:: python

    MidpointSolver.n = 3  # Number of substeps per DT
    MidpointSolver.method = 'midpoint'

**Tuning guidance:**

- RK4 with n=1 is usually sufficient and fast
- Increase n for more accurate integration (e.g., n=2 or 3)
- Midpoint method is rarely needed (only if computation is extremely tight)

Runtime Configuration
---------------------

RUNTIME_FREQUENCY
~~~~~~~~~~~~~~~~~

**Type:** ``float``

**Default:** ``100`` Hz

**Description:** Frequency at which the control loop runs.

**Impact:**

- Higher frequency = more responsive control
- Must be significantly faster than DT (currently 100Hz vs 10Hz)

**Note:** Affects the rate limit scaling for the first timestep in jerk constraints.

PUBLISH
~~~~~~~

**Type:** ``bool``

**Default:** ``True``

**Description:** Enable/disable publishing of control commands.

**Usage:**

- Set to ``False`` for testing/debugging without actuating the vehicle
- Set to ``True`` for normal operation

USE_WARMSTARTING
~~~~~~~~~~~~~~~~

**Type:** ``bool``

**Default:** ``False``

**Description:** Use previous solution to initialize current optimization.

**Impact:**

- When ``True``: Faster convergence (~30% speedup) but may get stuck in local minima
- When ``False``: Slower but more robust

**Tuning guidance:**

- Enable after verifying basic functionality works
- Disable if solver fails intermittently
- Currently disabled by default for robustness

Example Configurations
----------------------

Aggressive Racing
~~~~~~~~~~~~~~~~~

.. code-block:: python

    Q = [10., 10., 5., 0., 0.]  # High tracking priority
    R = [0., 0.]                 # No smoothing
    V_MAX = 12.0                 # Higher speed limit
    A_MAX = 4.0                  # More aggressive acceleration
    DF_DOT_MAX = 0.8             # Faster steering

Conservative Testing
~~~~~~~~~~~~~~~~~~~~

.. code-block:: python

    Q = [3., 3., 2., 0., 0.]    # Moderate tracking
    R = [0.5, 2.0]               # Smooth control
    V_MAX = 5.0                  # Reduced speed
    A_MAX = 2.0                  # Gentle acceleration
    DF_DOT_MAX = 0.3             # Slow steering

Tight Track
~~~~~~~~~~~

.. code-block:: python

    N = 12                       # Longer horizon for planning
    Q = [8., 8., 10., 0., 0.]   # Emphasize heading tracking
    V_MAX = 8.0                  # Moderate speed
    DF_DOT_MAX = 0.6             # Quick steering response

High-Speed Oval
~~~~~~~~~~~~~~~

.. code-block:: python

    N = 15                       # Long lookahead
    Q = [5., 5., 1., 0., 0.]    # Less heading tracking (wide turns)
    V_MAX = 15.0                 # High speed
    A_MAX = 2.5                  # Moderate acceleration
    DF_DOT_MAX = 0.3             # Gentle steering (high speed)

Performance Optimization
------------------------

C Code Generation
~~~~~~~~~~~~~~~~~

For maximum performance, pre-compile the solver to C:

.. code-block:: bash

    cd mpc/mpc
    python compile_solver.py

This generates ``mpc.c`` and compiles it to ``mpc.so``, providing ~10x speedup.

**See:** ``mpc/mpc/compile_solver.py``

Compiler Flags
~~~~~~~~~~~~~~

When compiling, you can adjust optimization level in ``mpc_controller.py:196``:

.. code-block:: python

    gcc_opt_flag='-Ofast'  # Aggressive (recommended)
    # gcc_opt_flag='-O3'   # Conservative
    # gcc_opt_flag='-O2'   # Balanced
    # gcc_opt_flag='-O1'   # Debug

Typical solve times:

- Python interpreted: 10-20 ms
- C compiled with ``-O2``: 3-8 ms
- C compiled with ``-Ofast``: 1-5 ms

ROS Topics
----------

**Subscribed Topics:**

.. list-table::
   :widths: 40 30 30
   :header-rows: 1

   * - Topic
     - Type
     - Description
   * - ``/slam/state``
     - feb_msgs/State
     - Current vehicle state
   * - ``/path/global``
     - feb_msgs/FebPath
     - Reference trajectory (full lap)
   * - ``/path/local``
     - feb_msgs/FebPath
     - Reference trajectory (exploration)
   * - ``/ground_truth/wheel_speeds``
     - eufs_msgs/WheelSpeedsStamped
     - Current steering angle
   * - ``/end_race``
     - std_msgs/Bool
     - Emergency stop signal

**Published Topics:**

.. list-table::
   :widths: 40 30 30
   :header-rows: 1

   * - Topic
     - Type
     - Description
   * - ``/cmd``
     - ackermann_msgs/AckermannDriveStamped
     - Control commands (acceleration, steering)
   * - ``/control/throttle``
     - std_msgs/Float64
     - Acceleration command
   * - ``/control/steer``
     - std_msgs/Float64
     - Steering angle command
   * - ``/mpc/viz``
     - geometry_msgs/PoseArray
     - Predicted and reference trajectories