Trajectory Optimization
=======================

The local path planner solves a time-optimal trajectory optimization problem to find the fastest path between the ordered left and right track boundaries. This page describes the mathematical formulation and implementation.

Optimization Problem
--------------------

**Objective**: Minimize total time to traverse the path

.. math::

    \min_{t, \psi, v, \theta, a, \dot{\theta}, \Delta t} \sum_{i=1}^N \Delta t_i

**Subject to**:

1. Vehicle dynamics constraints
2. Track boundary constraints
3. Velocity limits
4. Acceleration limits
5. Steering angle limits
6. Steering rate limits
7. Friction circle constraint

Decision Variables
------------------

For each waypoint i = 1, ..., N:

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

   * - Variable
     - Description
     - Bounds
   * - :math:`t_i`
     - Track position parameter (interpolation between left/right)
     - [0.45, 0.55]
   * - :math:`\psi_i`
     - Heading angle
     - unrestricted
   * - :math:`v_i`
     - Velocity
     - [V_MIN, V_MAX]
   * - :math:`\theta_i`
     - Steering angle
     - [-DF_MAX, DF_MAX]
   * - :math:`a_i`
     - Acceleration
     - [ACC_MIN, ACC_MAX]
   * - :math:`\dot{\theta}_i`
     - Steering rate
     - [-DF_DOT_MAX, DF_DOT_MAX]
   * - :math:`\Delta t_i`
     - Time step duration
     - [0, 2.0] seconds

**Total**: 7 variables × N waypoints

Track Position Parameter
~~~~~~~~~~~~~~~~~~~~~~~~

The variable :math:`t_i \in [0.45, 0.55]` determines where the vehicle drives between the left and right boundaries:

.. math::

    \mathbf{p}_i = (1 - t_i) \cdot \mathbf{left}_i + t_i \cdot \mathbf{right}_i

- :math:`t_i = 0.45`: Closer to left boundary (20% from center toward left)
- :math:`t_i = 0.50`: Centerline (exact middle)
- :math:`t_i = 0.55`: Closer to right boundary (20% from center toward right)

The parameter is constrained near 0.5 to prevent the vehicle from driving too close to the cones. The exact value at each waypoint is optimized to find the fastest racing line.

**Boundary conditions:**

- :math:`t_1 = 0.5` (start on centerline)
- :math:`t_N = 0.5` (end on centerline)

This ensures smooth entry and exit from the planned section.

Vehicle Dynamics Constraints
-----------------------------

The optimization enforces that the predicted states follow the bicycle dynamics model:

.. math::

    \mathbf{x}_{i+1} = F(\mathbf{x}_i, \mathbf{u}_i, \Delta t_i)

Where:

- :math:`\mathbf{x}_i = [x_i, y_i, \psi_i, v_i, \theta_i]` is the state at waypoint i
- :math:`\mathbf{u}_i = [a_i, \dot{\theta}_i]` is the control input
- :math:`F(\cdot)` is the discrete-time dynamics function (RK4 integration)

**See:** :doc:`../mpc/vehicle_model` for details on the bicycle model.

The dynamics function is computed using a custom integrator with 3 substeps for accuracy:

.. code-block:: python

    self.dynamics = discrete_custom_integrator(n=3, **car_params)

Angle Wrapping
~~~~~~~~~~~~~~

To handle heading angles near ±π, the dynamics error uses a wrapped difference:

.. math::

    \text{error}_\psi = \sin(\psi_i / 2)

This prevents discontinuities when the heading crosses the ±π boundary.

**See:** ``local_path/local_path/local_opt_compiled.py:142-160``

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

Simple box constraints on velocity:

.. math::

    V_{\text{MIN}} \leq v_i \leq V_{\text{MAX}}

**Default**: [0.0, 25.0] m/s

The optimizer will naturally maximize velocity where possible to minimize time.

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

Minimum (Braking)
~~~~~~~~~~~~~~~~~

.. math::

    a_i \geq A_{\text{MIN}}

**Default**: -3.0 m/s² (moderate braking)

Maximum (Throttle)
~~~~~~~~~~~~~~~~~~

The maximum acceleration can be velocity-dependent (to model motor torque curves):

.. math::

    a_i \leq A_{\text{MAX}}(v_i)

**Default**: 2.0 m/s² (constant)

For more realistic motor modeling, this can be a function: ``ACC_MAX_FN = Function('acc_max', [v], [torque(v) / mass])``

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

Steering Angle
~~~~~~~~~~~~~~

.. math::

    -\text{DF\_MAX} \leq \theta_i \leq \text{DF\_MAX}

**Default**: ±0.5 rad (±28.6°)

Steering Rate
~~~~~~~~~~~~~

.. math::

    -\text{DF\_DOT\_MAX} \leq \dot{\theta}_i \leq \text{DF\_DOT\_MAX}

**Default**: ±0.5 rad/s

This limits how quickly the steering can change, improving stability.

Friction Circle Constraint
---------------------------

The most important constraint for cornering performance is the friction circle, which limits combined longitudinal and lateral acceleration:

.. math::

    \sqrt{a_{\text{long}}^2 + a_{\text{lat}}^2} \leq \mu \cdot g

Where:

- :math:`a_{\text{long}} = a_i` (longitudinal acceleration from control input)
- :math:`a_{\text{lat}} = \frac{v_i^2}{L_r} \sin(\beta_i)` (lateral/centripetal acceleration)
- :math:`\beta_i = \arctan\left(\frac{L_r}{L_f + L_r} \tan(\theta_i)\right)` (slip angle)
- :math:`\mu \cdot g` is the maximum friction (FRIC_MAX parameter)

**Implementation**:

.. math::

    a_i^2 + a_{\text{centripetal}}^2 \leq \text{FRIC\_MAX}^2

**Default**: FRIC_MAX = 12.0 m/s² (~1.2g)

This constraint prevents the optimizer from planning trajectories that would exceed tire grip limits.

**See:** ``local_path/local_path/local_opt_compiled.py:266-273``

Initial Condition Constraints
------------------------------

The optimization must respect the current vehicle state:

**Velocity Tolerance**
~~~~~~~~~~~~~~~~~~~~~~

.. math::

    |v_1 - v_{\text{current}}| \leq 0.2 \text{ m/s}

Allows small deviation to account for state estimation noise.

**Heading Tolerance**
~~~~~~~~~~~~~~~~~~~~~

.. math::

    |\psi_1 - \psi_{\text{current}}| \leq \frac{\pi}{16} \text{ rad} \approx 11.25°

**Steering Angle**
~~~~~~~~~~~~~~~~~~

.. math::

    \theta_1 = \theta_{\text{current}}

Must match exactly (no tolerance) to ensure smooth handoff to MPC.

**See:** ``local_path/local_path/local_opt_compiled.py:228-257``

Terminal Constraints
--------------------

To ensure the path can be safely extended:

**Final Velocity**
~~~~~~~~~~~~~~~~~~

.. math::

    0.5 \leq v_N \leq 1.0 \text{ m/s}

Prevents planning paths that end at zero velocity (would require infinite time to restart).

**Final Control**
~~~~~~~~~~~~~~~~~

.. math::

    a_N = 0, \quad \dot{\theta}_N = 0

Zero control inputs at the end for smooth continuation.

**See:** ``local_path/local_path/local_opt_compiled.py:235-245``

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

The optimization uses IPOPT (Interior Point OPTimizer) with MA57 linear solver:

.. code-block:: python

    DEFAULT_SOPTS = {
        'ipopt': {
            'ipopt.linear_solver': 'ma57',
            'expand': True,
            'honor_original_bounds': True,
            'acceptable_tol': 1e-2,
            'acceptable_obj_change_tol': 1e-3,
        }
    }

**Key settings:**

- ``expand=True``: Expands MX expressions to SX for faster evaluation
- ``acceptable_tol=1e-2``: Relaxed tolerance for faster convergence
- ``acceptable_obj_change_tol=1e-3``: Stop when objective changes by <0.001s

Warm-Starting
~~~~~~~~~~~~~

The solver uses the previous solution to initialize each new solve. If no previous solution exists, an initial guess is constructed:

.. code-block:: python

    # Initial guess for first solve
    x0 = [
        t_i = 0.5 for all i,              # Start on centerline
        psi_i = cumulative angles from cones,
        v_i = min(current_velocity, 0.1),  # Conservative initial speed
        theta_i = current_steering for all i,
        a_i = 0 for all i,
        thdot_i = 0 for all i,
        dt_i = 0.1s for all i
    ]

The heading angles are initialized from the geometric direction between consecutive centerline points.

**See:** ``local_path/local_path/local_opt_compiled.py:426-436``

Solver Performance
------------------

Typical solve times on modern hardware:

- **Warm-started**: 50-100ms
- **Cold start**: 100-200ms
- **Iteration count**: 10-50 iterations

The solver tracks status and can detect failures:

.. code-block:: python

    if solver.stats()['return_status'] == 'Invalid_Number_Detected':
        # Cone ordering produced overlapping boundaries
        raise AssertionError('cone ordering failed!')

    if not solver.stats()['success']:
        # Optimization failed to converge
        raise AssertionError("Solver failed to converge")

Path Interpolation
------------------

The optimization produces N waypoints with non-uniform time spacing :math:`\Delta t_i`. For MPC compatibility, this is converted to a uniform time grid:

.. code-block:: python

    def to_constant_tgrid(dt, z, u, t):
        # dt = 0.01s (100 Hz for MPC)
        # t = cumulative times [0, t1, t1+t2, ...]

        current_time = 0
        states = []
        controls = []

        while current_time < t[-1]:
            # Find which segment we're in
            idx = find_segment(current_time, t)

            # Integrate from segment start to current_time
            extra_time = current_time - t[idx]
            state = dynamics(z[idx], u[idx], extra_time)
            states.append(state)
            controls.append(u[idx])

            current_time += dt

        return states, controls

This produces a path at constant 100 Hz that the MPC can directly use.

The path is also padded to ensure sufficient horizon:

.. code-block:: python

    # Pad until at least 2× MPC horizon
    while len(states) * dt < MPC_N * MPC_DT * 2:
        # Continue straight with zero acceleration
        states.append(integrate(states[-1], [0, u_last[1]], dt))
        controls.append([0, u_last[1]])

**See:** ``local_path/local_path/local_opt_compiled.py:365-397``

Debugging
---------

Constraint Violation
~~~~~~~~~~~~~~~~~~~~

If the solver fails, check which constraints are violated:

.. code-block:: python

    # Access constraint table
    self.gtable = {
        'dynamics': (0, 45),      # Indices 0-44
        'vel': (45, 55),          # Indices 45-54
        'acc_min': (55, 65),      # etc.
        ...
    }

    # Check specific constraint
    lam_g = solver_result['lam_g']  # Lagrange multipliers
    dynamics_multipliers = lam_g[0:45]

    # Large multipliers indicate active/violated constraints

Visualization
~~~~~~~~~~~~~

The local planner publishes the optimized path for visualization:

**Topics:**

- ``/path/local/viz`` (PointCloud): Path waypoints as 3D points
- ``/path/local/vizpath`` (Path): Path with full pose information

These can be displayed in RViz to inspect the planned trajectory.