A TEB trajectory is a discrete, time-stamped curve \( \mathcal{B} = \{ (\boldsymbol{\xi}_0, \Delta t_0), (\boldsymbol{\xi}_1, \Delta t_1), \dots, (\boldsymbol{\xi}_N) \} \) where each pose \( \boldsymbol{\xi}_i = (x_i, y_i, \theta_i) \in \mathbb{R}^2 \times (-\pi,\pi] \) and each \( \Delta t_i > 0 \) is the traversal time between consecutive poses. The planner receives a coarse path (from the global planner) and refines it by minimizing a weighted sum of objectives while softly enforcing feasibility (speed, acceleration, curvature/turning constraints) and safety (clearance from obstacles).

flowchart TD
  GP["Global path (waypoints in map frame)"] --> INIT["Initialize band (poses + nominal time gaps)"]
  INIT --> G["Build sparse objective (edges between neighbors + obstacle edges)"]
  G --> OPT["Iterative solve (linearize + update)"]
  OPT --> CHK["Check constraints (v,w,a) and clearance"]
  CHK -->|ok| CMD["Output short-horizon command (v,w)"]
  CHK -->|not ok| RET["Retiming / adjust weights / reinitialize"]
  CMD --> FB["Robot executes; new odom + costmap"]
  FB --> INIT
        

Intuition: classic “elastic bands” treat a path like a string under tension that is repelled by obstacles. TEB adds time elasticity so that the optimizer can stretch/compress time to satisfy bounds such as \( |v| < v_{\max} \) and \( |\omega| < \omega_{\max} \) while also seeking short traversal time.

For adjacent poses, define the displacement and heading increment: \( \Delta x_i = x_{i+1}-x_i \), \( \Delta y_i = y_{i+1}-y_i \), \( \Delta \theta_i = \mathrm{wrap}(\theta_{i+1}-\theta_i) \). Using the time gap \( \Delta t_i \), the discrete body speed and yaw-rate proxies are

\[ v_i \;=\; \frac{\sqrt{(\Delta x_i)^2 + (\Delta y_i)^2} }{\Delta t_i}, \qquad \omega_i \;=\; \frac{\Delta \theta_i}{\Delta t_i}, \qquad i=0,\dots,N-1. \]

These are not a full dynamics model; they are the local-planning quantities we can constrain without re-deriving robot dynamics (already known from robotics dynamics). In TEB we primarily impose: \( |v_i| < v_{\max} \), \( |\omega_i| < \omega_{\max} \), and discrete acceleration limits. A common symmetric discrete acceleration is

\[ a_i \;=\; \frac{v_{i+1}-v_i}{\tfrac{1}{2}(\Delta t_i + \Delta t_{i+1})}, \qquad \alpha_i \;=\; \frac{\omega_{i+1}-\omega_i}{\tfrac{1}{2}(\Delta t_i + \Delta t_{i+1})}, \qquad i=0,\dots,N-2. \]

To encode nonholonomic consistency without introducing a full control sequence, a useful residual aligns the pose orientation \( \theta_i \) with the segment direction: let \( \varphi_i = \mathrm{atan2}(\Delta y_i, \Delta x_i) \). Then penalize \( \sin(\theta_i - \varphi_i) \) (zero when aligned).

TEB is commonly implemented as a sparse nonlinear least-squares problem: find decision vector \( \mathbf{z} \) (stacking all free poses and time intervals) that minimizes

\[ \min_{\mathbf{z} } \; \frac{1}{2}\,\|\mathbf{r}(\mathbf{z})\|^2 \;=\; \frac{1}{2}\sum_{k} r_k(\mathbf{z})^2, \]

where each residual \( r_k \) corresponds to a “constraint” or “objective” term. The key advantage is that the Jacobian is sparse because most terms only couple neighboring poses and time intervals.

3.1 Smoothness (Elasticity) Residuals

The band should not be jagged. A standard discrete approximation of curvature/smoothness uses a second difference (“discrete Laplacian”) on positions:

\[ \Delta^2 \mathbf{p}_i \;=\; \mathbf{p}_{i+1} - 2\mathbf{p}_i + \mathbf{p}_{i-1}, \quad \mathbf{p}_i = (x_i, y_i). \]

Residuals: \( r^{\text{smooth} }_{i,x} = \sqrt{w_s}\, (\Delta^2 x_i) \), \( r^{\text{smooth} }_{i,y} = \sqrt{w_s}\, (\Delta^2 y_i) \).

3.2 Time-Optimality and Retiming

A simple time objective minimizes traversal time: \( J_{\text{time} } = w_t \sum_i \Delta t_i \). In residual form: \( r^{\text{time} }_i = \sqrt{w_t}\,\Delta t_i \). Additionally, time gaps are bounded: \( \Delta t_i > 0 \) and \( \Delta t_{\min} < \Delta t_i < \Delta t_{\max} \), typically encoded softly with hinge penalties.

3.3 Obstacle Clearance via Distance-to-Obstacle

Let \( d(\mathbf{p}_i) \) be the distance from pose position \( \mathbf{p}_i \) to the nearest obstacle boundary, computed from a distance transform (or from geometric primitives in a demo). For robot radius \( r_{\text{rob} } \), we define effective clearance \( \bar{d}(\mathbf{p}_i)=d(\mathbf{p}_i)-r_{\text{rob} } \). Enforce a minimum safety distance \( d_{\min} \) with a hinge:

\[ r^{\text{obst} }_i \;=\; \sqrt{w_o}\,\max\big(0,\; d_{\min} - \bar{d}(\mathbf{p}_i)\big). \]

3.4 Kinodynamic Soft Constraints (Bounds on v, ω, a, α)

With the discrete proxies from Section 2, enforce bounds using hinge residuals, e.g. \( r^{v}_i = \sqrt{w_v}\,\max(0, v_i - v_{\max}) \), \( r^{\omega}_i = \sqrt{w_\omega}\,\max(0, |\omega_i| - \omega_{\max}) \), and similarly for \( |a_i| < a_{\max} \) and \( |\alpha_i| < \alpha_{\max} \).

3.5 Nonholonomic Consistency Proxy

For wheeled robots, sideways velocity should be small. In a purely geometric discretization, we can penalize misalignment between pose orientation and segment direction: \( r^{\text{align} }_i = \sqrt{w_\text{align} }\,\sin(\theta_i - \varphi_i) \). This enforces \( \theta_i \approx \varphi_i \) up to \( \pi \) wrapping.

flowchart TD
  P0["Pose 0"] --- DT0["dt 0"] --- P1["Pose 1"] --- DT1["dt 1"] --- P2["Pose 2"] --- DT2["dt 2"] --- P3["Pose 3"]
  P1 --> ESM1["smoothness edge"]
  P2 --> ESM2["smoothness edge"]
  P1 --> EOB1["obstacle edge"]
  P2 --> EOB2["obstacle edge"]
  P1 --> EK1["v,w,a bounds edge"]
  P2 --> EK2["v,w,a bounds edge"]
        

The sparse chain structure is why TEB can run online: each edge touches only a few variables (local coupling), yielding a sparse normal matrix.

Let \( \mathbf{r}(\mathbf{z}) \in \mathbb{R}^m \) be the stacked residual vector. Around a current iterate \( \mathbf{z} \), linearize: \( \mathbf{r}(\mathbf{z}+\delta) \approx \mathbf{r}(\mathbf{z}) + \mathbf{J}\delta \), where \( \mathbf{J} = \tfrac{\partial \mathbf{r} }{\partial \mathbf{z} } \).

\[ \delta^* \;=\; \arg\min_\delta \; \frac{1}{2}\|\mathbf{r} + \mathbf{J}\delta\|^2. \]

Proposition (Normal Equations). The minimizer of the linearized problem satisfies \( (\mathbf{J}^\top\mathbf{J})\delta^* = -\mathbf{J}^\top \mathbf{r} \).

Proof: Expand the quadratic objective: \( \tfrac{1}{2}(\mathbf{r}+\mathbf{J}\delta)^\top(\mathbf{r}+\mathbf{J}\delta) \). Differentiate w.r.t. \( \delta \) and set to zero:

\[ \nabla_\delta \Big( \tfrac{1}{2}\|\mathbf{r}+\mathbf{J}\delta\|^2 \Big) = \mathbf{J}^\top(\mathbf{r}+\mathbf{J}\delta) = \mathbf{0} \;\;\Longrightarrow\;\; \mathbf{J}^\top\mathbf{J}\,\delta = -\mathbf{J}^\top\mathbf{r}. \quad \blacksquare \]

Levenberg–Marquardt damping. To improve robustness (especially when far from a good solution), solve the damped system \( (\mathbf{J}^\top\mathbf{J} + \lambda\mathbf{I})\delta = -\mathbf{J}^\top\mathbf{r} \) with \( \lambda > 0 \). This is the backbone of many TEB implementations, often via a graph-optimization backend.

In a navigation stack, TEB sits between the global planner and the low-level controller: it consumes (i) a global plan, (ii) a local costmap / obstacle list, and (iii) robot limits. Each cycle, it optimizes a short horizon and outputs a velocity command \( (v,\omega) \) or a short trajectory segment.

• Initialization: TEB performance depends on the initial band. A common strategy is to resample the global path to match a desired spatial resolution and assign a nominal \( \Delta t \).
• Retiming: If obstacle avoidance requires detours, time gaps stretch; if the path is clear, the time objective shrinks them, but bounds \( \Delta t_{\min} < \Delta t_i < \Delta t_{\max} \) keep numerics stable.
• Topologies: Practical TEB variants maintain multiple homotopy classes (distinct ways around obstacles) and optimize each, selecting the lowest-cost feasible solution.
• Soft vs hard constraints: Most constraints are “soft” residuals; if a strict safety guarantee is needed, you add conservative inflation in the costmap and enforce tighter \( d_{\min} \).

The following educational implementations solve a small TEB problem with circular obstacles, using a damped Gauss-Newton solver with a numerical Jacobian. They are designed to match the residual definitions in Sections 2–4. In real robots, you typically use sparse analytic Jacobians and a graph-optimization library.

6.1 Python Implementation

File: Chapter15_Lesson4.py

# Chapter15_Lesson4.py
# Timed-Elastic-Band (TEB) local planning — educational implementation (2D, SE(2))
# Author: Abolfazl Mohammadijoo (course scaffold)
#
# This script builds a small TEB optimization problem:
#   - Variables: poses (x_i, y_i, theta_i) and time gaps dt_i
#   - Residuals: smoothness, time, obstacle clearance, speed bounds, yaw-rate bounds, accel bounds, nonholonomic alignment
#   - Solver: damped Gauss-Newton (Levenberg–Marquardt style) with numerical Jacobian
#
# NOTE: This is a teaching implementation. Real TEB uses sparse analytic Jacobians and a graph-optimizer backend.

from __future__ import annotations

from dataclasses import dataclass
from typing import List, Tuple, Dict, Callable, Optional
import math
import numpy as np


def wrap_angle(a: float) -> float:
    """Wrap angle to (-pi, pi]."""
    return (a + math.pi) % (2.0 * math.pi) - math.pi


@dataclass
class CircleObstacle:
    cx: float
    cy: float
    r: float


@dataclass
class Limits:
    v_max: float = 0.7          # m/s
    w_max: float = 1.2          # rad/s
    a_max: float = 0.8          # m/s^2
    alpha_max: float = 2.0      # rad/s^2
    dt_min: float = 0.05        # s
    dt_max: float = 0.60        # s


@dataclass
class Weights:
    smooth: float = 2.0
    time: float = 0.5
    obst: float = 4.0
    v_bound: float = 2.0
    w_bound: float = 1.5
    a_bound: float = 1.0
    alpha_bound: float = 0.8
    align: float = 0.6
    dt_bound: float = 2.0


def distance_to_nearest_obstacle(px: float, py: float, obstacles: List[CircleObstacle]) -> float:
    """Distance from point to nearest obstacle boundary (positive outside)."""
    d_min = float("inf")
    for ob in obstacles:
        d = math.hypot(px - ob.cx, py - ob.cy) - ob.r
        d_min = min(d_min, d)
    return d_min


def unpack_z(z: np.ndarray, N: int) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
    """
    z packs: [x0..xN, y0..yN, th0..thN, dt0..dt_{N-1}]
    returns (x,y,th,dt)
    """
    x = z[0:N+1]
    y = z[N+1:2*(N+1)]
    th = z[2*(N+1):3*(N+1)]
    dt = z[3*(N+1):3*(N+1) + N]
    return x, y, th, dt


def pack_z(x: np.ndarray, y: np.ndarray, th: np.ndarray, dt: np.ndarray) -> np.ndarray:
    return np.concatenate([x, y, th, dt], axis=0)


def build_residuals(
    z: np.ndarray,
    N: int,
    obstacles: List[CircleObstacle],
    robot_radius: float,
    d_min: float,
    limits: Limits,
    weights: Weights,
) -> np.ndarray:
    """
    Build stacked residual vector r(z). Each residual includes sqrt(weight).
    """
    x, y, th, dt = unpack_z(z, N)

    r_list: List[float] = []

    # --- Smoothness: second differences on x,y (i = 1..N-1)
    ws = math.sqrt(weights.smooth)
    for i in range(1, N):
        ddx = x[i+1] - 2.0*x[i] + x[i-1]
        ddy = y[i+1] - 2.0*y[i] + y[i-1]
        r_list.append(ws * ddx)
        r_list.append(ws * ddy)

    # --- Time objective: sum dt_i
    wt = math.sqrt(weights.time)
    for i in range(N):
        r_list.append(wt * dt[i])

    # --- dt bounds as hinge penalties
    wdt = math.sqrt(weights.dt_bound)
    for i in range(N):
        # dt_min < dt < dt_max
        r_list.append(wdt * max(0.0, limits.dt_min - dt[i]))
        r_list.append(wdt * max(0.0, dt[i] - limits.dt_max))

    # --- Segment kinematics: v_i, w_i
    v = np.zeros(N)
    w = np.zeros(N)
    for i in range(N):
        dx = x[i+1] - x[i]
        dy = y[i+1] - y[i]
        dth = wrap_angle(th[i+1] - th[i])
        seg = math.hypot(dx, dy)
        v[i] = seg / max(1e-9, dt[i])
        w[i] = dth / max(1e-9, dt[i])

    # --- Speed bound residuals (hinge)
    wv = math.sqrt(weights.v_bound)
    for i in range(N):
        r_list.append(wv * max(0.0, v[i] - limits.v_max))

    # --- Yaw-rate bound residuals (hinge on |w|)
    ww = math.sqrt(weights.w_bound)
    for i in range(N):
        r_list.append(ww * max(0.0, abs(w[i]) - limits.w_max))

    # --- Acceleration bounds (i = 0..N-2), symmetric dt average
    wa = math.sqrt(weights.a_bound)
    wal = math.sqrt(weights.alpha_bound)
    for i in range(N-1):
        dt_avg = 0.5 * (dt[i] + dt[i+1])
        dt_avg = max(1e-9, dt_avg)
        a_i = (v[i+1] - v[i]) / dt_avg
        alpha_i = (w[i+1] - w[i]) / dt_avg
        r_list.append(wa * max(0.0, abs(a_i) - limits.a_max))
        r_list.append(wal * max(0.0, abs(alpha_i) - limits.alpha_max))

    # --- Obstacle clearance: hinge on (d_min - (dist - robot_radius))
    wo = math.sqrt(weights.obst)
    for i in range(N+1):
        dist = distance_to_nearest_obstacle(x[i], y[i], obstacles)
        clearance = dist - robot_radius
        r_list.append(wo * max(0.0, d_min - clearance))

    # --- Nonholonomic alignment proxy: sin(theta_i - atan2(dy,dx))
    wa2 = math.sqrt(weights.align)
    for i in range(N):
        dx = x[i+1] - x[i]
        dy = y[i+1] - y[i]
        phi = math.atan2(dy, dx)
        r_list.append(wa2 * math.sin(wrap_angle(th[i] - phi)))

    return np.array(r_list, dtype=float)


def numerical_jacobian(fun: Callable[[np.ndarray], np.ndarray], z: np.ndarray, eps: float = 1e-6) -> np.ndarray:
    """Dense numerical Jacobian by forward differences (teaching simplicity)."""
    r0 = fun(z)
    m = r0.size
    n = z.size
    J = np.zeros((m, n), dtype=float)
    for j in range(n):
        zp = z.copy()
        zp[j] += eps
        rp = fun(zp)
        J[:, j] = (rp - r0) / eps
    return J


def solve_lm(
    fun: Callable[[np.ndarray], np.ndarray],
    z0: np.ndarray,
    max_iters: int = 20,
    lam0: float = 1e-2,
    verbose: bool = True
) -> Tuple[np.ndarray, Dict[str, List[float]]]:
    """
    Simple Levenberg–Marquardt: (J^T J + lam I) d = -J^T r
    Accept step if it decreases cost; otherwise increase lam.
    """
    z = z0.copy()
    lam = lam0
    hist = {"cost": [], "lam": []}

    for it in range(max_iters):
        r = fun(z)
        cost = 0.5 * float(r @ r)
        hist["cost"].append(cost)
        hist["lam"].append(lam)

        if verbose:
            print(f"iter={it:02d}  cost={cost:.6f}  lam={lam:.3e}")

        J = numerical_jacobian(fun, z)
        A = J.T @ J + lam * np.eye(z.size)
        b = -J.T @ r

        try:
            d = np.linalg.solve(A, b)
        except np.linalg.LinAlgError:
            # Increase damping and continue
            lam *= 10.0
            continue

        z_new = z + d
        r_new = fun(z_new)
        cost_new = 0.5 * float(r_new @ r_new)

        if cost_new < cost:
            z = z_new
            lam = max(lam / 2.0, 1e-9)
        else:
            lam = min(lam * 5.0, 1e6)

        # crude stop
        if np.linalg.norm(d) < 1e-6:
            break

    return z, hist


def initialize_band_from_waypoints(waypoints: List[Tuple[float, float]], dt_nominal: float = 0.2) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
    """
    Initialize x,y from waypoints; theta from segment headings; dt all dt_nominal.
    """
    pts = np.array(waypoints, dtype=float)
    x = pts[:, 0].copy()
    y = pts[:, 1].copy()
    N = len(waypoints) - 1
    th = np.zeros(N + 1, dtype=float)
    for i in range(N):
        th[i] = math.atan2(y[i+1] - y[i], x[i+1] - x[i])
    th[N] = th[N-1]
    dt = np.full(N, dt_nominal, dtype=float)
    return x, y, th, dt


def optimize_teb_demo() -> None:
    """
    Demo scenario: start -> goal, with two circular obstacles.
    """
    # Global path (coarse). In real stacks, this comes from the global planner.
    waypoints = [
        (0.0, 0.0),
        (0.7, 0.1),
        (1.5, 0.2),
        (2.2, 0.1),
        (3.0, 0.0),
        (3.8, 0.1),
        (4.6, 0.0),
        (5.2, 0.0),
    ]
    x, y, th, dt = initialize_band_from_waypoints(waypoints, dt_nominal=0.25)
    N = len(waypoints) - 1

    obstacles = [
        CircleObstacle(cx=2.2, cy=0.15, r=0.35),
        CircleObstacle(cx=3.9, cy=-0.10, r=0.35),
    ]

    robot_radius = 0.22
    d_min = 0.15
    limits = Limits(v_max=0.8, w_max=1.4, a_max=1.0, alpha_max=2.5, dt_min=0.05, dt_max=0.60)
    weights = Weights(
        smooth=2.0, time=0.4, obst=6.0, v_bound=3.0, w_bound=2.0, a_bound=1.0, alpha_bound=0.8, align=0.6, dt_bound=2.0
    )

    # Fix endpoints (start and goal) by not optimizing them: simplest method is to keep them in z but
    # overwrite after each update. This is a teaching shortcut.
    z0 = pack_z(x, y, th, dt)

    def residual_fun(z: np.ndarray) -> np.ndarray:
        # enforce dt positivity by clipping for numerical stability (still penalized by dt bounds)
        x_, y_, th_, dt_ = unpack_z(z, N)
        dt_clipped = np.clip(dt_, 1e-3, 10.0)
        zc = pack_z(x_, y_, th_, dt_clipped)
        r = build_residuals(zc, N, obstacles, robot_radius, d_min, limits, weights)
        return r

    z_opt, hist = solve_lm(residual_fun, z0, max_iters=18, lam0=1e-2, verbose=True)

    x_opt, y_opt, th_opt, dt_opt = unpack_z(z_opt, N)

    print("\nOptimized dt stats:", float(np.min(dt_opt)), float(np.max(dt_opt)), "sum=", float(np.sum(dt_opt)))

    # Produce an approximate first command (v0, w0) from the first segment
    dx0 = x_opt[1] - x_opt[0]
    dy0 = y_opt[1] - y_opt[0]
    dth0 = wrap_angle(th_opt[1] - th_opt[0])
    seg0 = math.hypot(dx0, dy0)
    v0 = seg0 / max(1e-9, dt_opt[0])
    w0 = dth0 / max(1e-9, dt_opt[0])
    print("Approx first cmd (v,w):", v0, w0)

    # Optional visualization (requires matplotlib)
    try:
        import matplotlib.pyplot as plt

        plt.figure()
        plt.plot(x, y, "o--", label="init band")
        plt.plot(x_opt, y_opt, "o-", label="optimized band")
        for ob in obstacles:
            ang = np.linspace(0, 2*np.pi, 200)
            plt.plot(ob.cx + ob.r*np.cos(ang), ob.cy + ob.r*np.sin(ang), "--")
            # also plot clearance ring
            rr = ob.r + robot_radius + d_min
            plt.plot(ob.cx + rr*np.cos(ang), ob.cy + rr*np.sin(ang), ":")
        plt.axis("equal")
        plt.grid(True)
        plt.legend()
        plt.title("TEB demo (educational)")
        plt.show()
    except Exception as e:
        print("Plot skipped:", e)


if __name__ == "__main__":
    optimize_teb_demo()
      

File: Chapter15_Lesson4_Ex1.py

# Chapter15_Lesson4_Ex1.py
# Exercise: Change weights/limits and observe how the optimized band changes.
#
# Task:
#   1) Increase weights.obst (e.g., from 6.0 to 15.0) and re-run.
#   2) Decrease limits.v_max and re-run.
#   3) Explain (in your report) how dt_i and the path deformation respond.

from Chapter15_Lesson4 import optimize_teb_demo

if __name__ == "__main__":
    optimize_teb_demo()
      

6.2 C++ Implementation (Eigen)

File: Chapter15_Lesson4.cpp

// Chapter15_Lesson4.cpp
// Timed-Elastic-Band (TEB) local planning — educational implementation (2D, SE(2))
// Dense numerical Jacobian + Levenberg–Marquardt (teaching simplicity).
//
// Build:
//   g++ -O2 -std=c++17 Chapter15_Lesson4.cpp -I /path/to/eigen -o teb_demo
//
// Run:
//   ./teb_demo
//
// NOTE: This is not a production TEB; it is a didactic optimizer consistent with the lesson math.

#include 
#include 
#include 
#include 
#include 
#include 

struct CircleObstacle {
  double cx, cy, r;
};

static double wrap_angle(double a) {
  a = std::fmod(a + M_PI, 2.0 * M_PI);
  if (a < 0) a += 2.0 * M_PI;
  return a - M_PI;
}

struct Limits {
  double v_max = 0.8;
  double w_max = 1.4;
  double a_max = 1.0;
  double alpha_max = 2.5;
  double dt_min = 0.05;
  double dt_max = 0.60;
};

struct Weights {
  double smooth = 2.0;
  double time = 0.4;
  double obst = 6.0;
  double v_bound = 3.0;
  double w_bound = 2.0;
  double a_bound = 1.0;
  double alpha_bound = 0.8;
  double align = 0.6;
  double dt_bound = 2.0;
};

static double dist_to_nearest_obstacle(double px, double py, const std::vector& obs) {
  double dmin = std::numeric_limits::infinity();
  for (const auto& o : obs) {
    double d = std::hypot(px - o.cx, py - o.cy) - o.r;
    dmin = std::min(dmin, d);
  }
  return dmin;
}

// z packs: [x0..xN, y0..yN, th0..thN, dt0..dt_{N-1}]
static void unpack(const Eigen::VectorXd& z, int N,
                   Eigen::VectorXd& x, Eigen::VectorXd& y, Eigen::VectorXd& th, Eigen::VectorXd& dt) {
  x = z.segment(0, N+1);
  y = z.segment(N+1, N+1);
  th = z.segment(2*(N+1), N+1);
  dt = z.segment(3*(N+1), N);
}

static Eigen::VectorXd residuals(
  const Eigen::VectorXd& z_in,
  int N,
  const std::vector& obstacles,
  double robot_radius,
  double d_min,
  const Limits& lim,
  const Weights& w
) {
  Eigen::VectorXd x, y, th, dt;
  unpack(z_in, N, x, y, th, dt);

  // clip dt for numerical stability (still penalized by hinge bounds)
  for (int i = 0; i < N; ++i) dt[i] = std::clamp(dt[i], 1e-3, 10.0);

  std::vector r;
  r.reserve(1000);

  // smoothness (i=1..N-1)
  double ws = std::sqrt(w.smooth);
  for (int i = 1; i < N; ++i) {
    double ddx = x[i+1] - 2.0*x[i] + x[i-1];
    double ddy = y[i+1] - 2.0*y[i] + y[i-1];
    r.push_back(ws * ddx);
    r.push_back(ws * ddy);
  }

  // time
  double wt = std::sqrt(w.time);
  for (int i = 0; i < N; ++i) r.push_back(wt * dt[i]);

  // dt bounds
  double wdt = std::sqrt(w.dt_bound);
  for (int i = 0; i < N; ++i) {
    r.push_back(wdt * std::max(0.0, lim.dt_min - dt[i]));
    r.push_back(wdt * std::max(0.0, dt[i] - lim.dt_max));
  }

  // v,w
  Eigen::VectorXd v(N), om(N);
  for (int i = 0; i < N; ++i) {
    double dx = x[i+1] - x[i];
    double dy = y[i+1] - y[i];
    double dth = wrap_angle(th[i+1] - th[i]);
    double seg = std::hypot(dx, dy);
    v[i] = seg / std::max(1e-9, dt[i]);
    om[i] = dth / std::max(1e-9, dt[i]);
  }

  // v bound
  double wv = std::sqrt(w.v_bound);
  for (int i = 0; i < N; ++i) r.push_back(wv * std::max(0.0, v[i] - lim.v_max));

  // w bound
  double ww = std::sqrt(w.w_bound);
  for (int i = 0; i < N; ++i) r.push_back(ww * std::max(0.0, std::abs(om[i]) - lim.w_max));

  // accel bounds
  double wa = std::sqrt(w.a_bound);
  double wal = std::sqrt(w.alpha_bound);
  for (int i = 0; i < N-1; ++i) {
    double dt_avg = 0.5 * (dt[i] + dt[i+1]);
    dt_avg = std::max(1e-9, dt_avg);
    double a_i = (v[i+1] - v[i]) / dt_avg;
    double al_i = (om[i+1] - om[i]) / dt_avg;
    r.push_back(wa * std::max(0.0, std::abs(a_i) - lim.a_max));
    r.push_back(wal * std::max(0.0, std::abs(al_i) - lim.alpha_max));
  }

  // obstacle clearance
  double wo = std::sqrt(w.obst);
  for (int i = 0; i < N+1; ++i) {
    double dist = dist_to_nearest_obstacle(x[i], y[i], obstacles);
    double clearance = dist - robot_radius;
    r.push_back(wo * std::max(0.0, d_min - clearance));
  }

  // alignment proxy
  double wa2 = std::sqrt(w.align);
  for (int i = 0; i < N; ++i) {
    double dx = x[i+1] - x[i];
    double dy = y[i+1] - y[i];
    double phi = std::atan2(dy, dx);
    r.push_back(wa2 * std::sin(wrap_angle(th[i] - phi)));
  }

  Eigen::VectorXd rv((int)r.size());
  for (int i = 0; i < (int)r.size(); ++i) rv[i] = r[i];
  return rv;
}

static Eigen::MatrixXd numerical_jacobian(
  const std::function& fun,
  const Eigen::VectorXd& z,
  double eps = 1e-6
) {
  Eigen::VectorXd r0 = fun(z);
  int m = (int)r0.size();
  int n = (int)z.size();
  Eigen::MatrixXd J(m, n);
  for (int j = 0; j < n; ++j) {
    Eigen::VectorXd zp = z;
    zp[j] += eps;
    Eigen::VectorXd rp = fun(zp);
    J.col(j) = (rp - r0) / eps;
  }
  return J;
}

static Eigen::VectorXd solve_lm(
  const std::function& fun,
  const Eigen::VectorXd& z0,
  int max_iters,
  double lam0
) {
  Eigen::VectorXd z = z0;
  double lam = lam0;

  for (int it = 0; it < max_iters; ++it) {
    Eigen::VectorXd r = fun(z);
    double cost = 0.5 * r.squaredNorm();
    std::cout << "iter=" << it << " cost=" << cost << " lam=" << lam << "\n";

    Eigen::MatrixXd J = numerical_jacobian(fun, z);
    Eigen::MatrixXd A = J.transpose() * J + lam * Eigen::MatrixXd::Identity(z.size(), z.size());
    Eigen::VectorXd b = -J.transpose() * r;

    Eigen::VectorXd d = A.ldlt().solve(b);
    Eigen::VectorXd z_new = z + d;
    Eigen::VectorXd r_new = fun(z_new);
    double cost_new = 0.5 * r_new.squaredNorm();

    if (cost_new < cost) {
      z = z_new;
      lam = std::max(lam / 2.0, 1e-9);
    } else {
      lam = std::min(lam * 5.0, 1e6);
    }

    if (d.norm() < 1e-6) break;
  }

  return z;
}

int main() {
  // Coarse global path waypoints
  std::vector> wp = {
    {0.0, 0.0},
    {0.7, 0.1},
    {1.5, 0.2},
    {2.2, 0.1},
    {3.0, 0.0},
    {3.8, 0.1},
    {4.6, 0.0},
    {5.2, 0.0},
  };
  int N = (int)wp.size() - 1;

  // initialize x,y,theta,dt
  Eigen::VectorXd x(N+1), y(N+1), th(N+1), dt(N);
  for (int i = 0; i < N+1; ++i) { x[i] = wp[i].first; y[i] = wp[i].second; }
  for (int i = 0; i < N; ++i) th[i] = std::atan2(y[i+1] - y[i], x[i+1] - x[i]);
  th[N] = th[N-1];
  dt.setConstant(0.25);

  // pack z
  Eigen::VectorXd z0(3*(N+1) + N);
  z0 << x, y, th, dt;

  std::vector obstacles = {
    {2.2, 0.15, 0.35},
    {3.9, -0.10, 0.35},
  };

  double robot_radius = 0.22;
  double d_min = 0.15;
  Limits lim;
  Weights w;

  auto fun = [&](const Eigen::VectorXd& z) {
    return residuals(z, N, obstacles, robot_radius, d_min, lim, w);
  };

  Eigen::VectorXd z_opt = solve_lm(fun, z0, 18, 1e-2);

  Eigen::VectorXd xo, yo, tho, dto;
  unpack(z_opt, N, xo, yo, tho, dto);

  std::cout << "\nOptimized dt sum: " << dto.sum()
            << "  min: " << dto.minCoeff()
            << "  max: " << dto.maxCoeff() << "\n";

  // approximate first cmd
  double dx0 = xo[1] - xo[0];
  double dy0 = yo[1] - yo[0];
  double dth0 = wrap_angle(tho[1] - tho[0]);
  double seg0 = std::hypot(dx0, dy0);
  double v0 = seg0 / std::max(1e-9, dto[0]);
  double w0 = dth0 / std::max(1e-9, dto[0]);
  std::cout << "Approx first cmd (v,w): " << v0 << " " << w0 << "\n";

  return 0;
}
      

6.3 Java Implementation (EJML)

File: Chapter15_Lesson4.java

// Chapter15_Lesson4.java
// Timed-Elastic-Band (TEB) local planning — educational implementation (2D, SE(2))
// Dense numerical Jacobian + Levenberg–Marquardt using EJML.
//
// Dependencies (Gradle/Maven): org.ejml:ejml-simple
//
// Run (conceptual):
//   javac -cp ejml-simple.jar Chapter15_Lesson4.java
//   java  -cp .:ejml-simple.jar Chapter15_Lesson4
//
// NOTE: Teaching implementation; not a production ROS TEB planner.

import org.ejml.simple.SimpleMatrix;

import java.util.ArrayList;
import java.util.List;

public class Chapter15_Lesson4 {

    static class CircleObstacle {
        double cx, cy, r;
        CircleObstacle(double cx, double cy, double r) { this.cx=cx; this.cy=cy; this.r=r; }
    }

    static double wrapAngle(double a) {
        double twoPi = 2.0*Math.PI;
        a = (a + Math.PI) % twoPi;
        if (a < 0) a += twoPi;
        return a - Math.PI;
    }

    static class Limits {
        double vMax=0.8, wMax=1.4, aMax=1.0, alphaMax=2.5, dtMin=0.05, dtMax=0.60;
    }

    static class Weights {
        double smooth=2.0, time=0.4, obst=6.0, vBound=3.0, wBound=2.0, aBound=1.0, alphaBound=0.8, align=0.6, dtBound=2.0;
    }

    // z packs: [x0..xN, y0..yN, th0..thN, dt0..dt_{N-1}]
    static double[] residuals(double[] z, int N, List obstacles, double robotRadius, double dMin, Limits lim, Weights w) {
        int nPose = N+1;
        double[] x = new double[nPose];
        double[] y = new double[nPose];
        double[] th = new double[nPose];
        double[] dt = new double[N];

        System.arraycopy(z, 0, x, 0, nPose);
        System.arraycopy(z, nPose, y, 0, nPose);
        System.arraycopy(z, 2*nPose, th, 0, nPose);
        System.arraycopy(z, 3*nPose, dt, 0, N);

        // clip dt
        for (int i=0;i r = new ArrayList<>();

        // smoothness
        double ws = Math.sqrt(w.smooth);
        for (int i=1;i obstacles){
        double dmin = Double.POSITIVE_INFINITY;
        for (CircleObstacle ob: obstacles){
            double d = Math.hypot(px-ob.cx, py-ob.cy) - ob.r;
            if (d < dmin) dmin = d;
        }
        return dmin;
    }

    static SimpleMatrix numericalJacobian(java.util.function.Function fun, double[] z, double eps){
        double[] r0 = fun.apply(z);
        int m = r0.length;
        int n = z.length;
        SimpleMatrix J = new SimpleMatrix(m, n);

        for (int j=0;j fun, double[] z0, int maxIters, double lam0){
        double[] z = z0.clone();
        double lam = lam0;

        for(int it=0; it obstacles = new ArrayList<>();
        obstacles.add(new CircleObstacle(2.2, 0.15, 0.35));
        obstacles.add(new CircleObstacle(3.9,-0.10, 0.35));

        double robotRadius=0.22, dMin=0.15;
        Limits lim = new Limits();
        Weights w = new Weights();

        java.util.function.Function fun = (zz) -> residuals(zz, N, obstacles, robotRadius, dMin, lim, w);

        double[] zOpt = solveLM(fun, z0, 18, 1e-2);

        // compute first cmd
        double x0 = zOpt[0], y0p = zOpt[nPose];
        double x1 = zOpt[1], y1p = zOpt[nPose+1];
        double th0 = zOpt[2*nPose], th1 = zOpt[2*nPose+1];
        double dt0 = zOpt[3*nPose];

        double seg0 = Math.hypot(x1-x0, y1p-y0p);
        double v0 = seg0/Math.max(1e-9, dt0);
        double w0 = wrapAngle(th1-th0)/Math.max(1e-9, dt0);
        System.out.println("Approx first cmd (v,w): " + v0 + " " + w0);
    }
}
      

6.4 MATLAB Implementation

File: Chapter15_Lesson4.m

% Chapter15_Lesson4.m
% Timed-Elastic-Band (TEB) local planning — educational implementation (2D, SE(2))
% Dense numerical Jacobian + damped Gauss-Newton (LM-style).
%
% Run:
%   Chapter15_Lesson4
%
% NOTE: Teaching implementation; not a production ROS TEB planner.

function Chapter15_Lesson4()
    waypoints = [ ...
        0.0 0.0; ...
        0.7 0.1; ...
        1.5 0.2; ...
        2.2 0.1; ...
        3.0 0.0; ...
        3.8 0.1; ...
        4.6 0.0; ...
        5.2 0.0 ];

    N = size(waypoints,1) - 1;
    x = waypoints(:,1); y = waypoints(:,2);
    th = zeros(N+1,1);
    for i=1:N
        th(i) = atan2(y(i+1)-y(i), x(i+1)-x(i));
    end
    th(N+1) = th(N);
    dt = 0.25*ones(N,1);

    obstacles = [2.2 0.15 0.35;
                3.9 -0.10 0.35];

    robot_radius = 0.22;
    d_min = 0.15;

    limits.v_max = 0.8;
    limits.w_max = 1.4;
    limits.a_max = 1.0;
    limits.alpha_max = 2.5;
    limits.dt_min = 0.05;
    limits.dt_max = 0.60;

    weights.smooth = 2.0;
    weights.time = 0.4;
    weights.obst = 6.0;
    weights.v_bound = 3.0;
    weights.w_bound = 2.0;
    weights.a_bound = 1.0;
    weights.alpha_bound = 0.8;
    weights.align = 0.6;
    weights.dt_bound = 2.0;

    z0 = pack_z(x,y,th,dt);

    fun = @(z) residuals(z,N,obstacles,robot_radius,d_min,limits,weights);

    [z_opt, hist] = solve_lm(fun, z0, 18, 1e-2);

    [xo,yo,tho,dto] = unpack_z(z_opt,N);
    fprintf('Optimized dt sum=%.4f min=%.4f max=%.4f\n', sum(dto), min(dto), max(dto));

    % First cmd approx
    dx0 = xo(2)-xo(1); dy0 = yo(2)-yo(1);
    dth0 = wrap_angle(tho(2)-tho(1));
    seg0 = hypot(dx0, dy0);
    v0 = seg0 / max(1e-9, dto(1));
    w0 = dth0 / max(1e-9, dto(1));
    fprintf('Approx first cmd (v,w) = (%.4f, %.4f)\n', v0, w0);

    % Plot
    figure; hold on; grid on; axis equal;
    plot(x,y,'o--','DisplayName','init band');
    plot(xo,yo,'o-','DisplayName','optimized band');

    thv = linspace(0, 2*pi, 200);
    for k = 1:size(obstacles,1)
        cx = obstacles(k,1); cy = obstacles(k,2); rr = obstacles(k,3);
        plot(cx + rr*cos(thv), cy + rr*sin(thv), '--');
    end

    legend; title('TEB demo (educational)');
end

function a = wrap_angle(a)
    a = mod(a + pi, 2*pi) - pi;
end

function z = pack_z(x,y,th,dt)
    z = [x; y; th; dt];
end

function [x,y,th,dt] = unpack_z(z,N)
    x = z(1:N+1);
    y = z(N+2:2*(N+1));
    th = z(2*(N+1)+1:3*(N+1));
    dt = z(3*(N+1)+1:3*(N+1)+N);
end

function dmin = dist_to_nearest(px,py,obstacles)
    dmin = inf;
    for k=1:size(obstacles,1)
        cx=obstacles(k,1); cy=obstacles(k,2); r=obstacles(k,3);
        d = hypot(px-cx, py-cy) - r;
        dmin = min(dmin, d);
    end
end

function r = residuals(z,N,obstacles,robot_radius,d_min,limits,weights)
    [x,y,th,dt] = unpack_z(z,N);

    % clip dt for stability
    dt = max(dt, 1e-3);
    dt = min(dt, 10.0);

    r = [];

    % smoothness
    ws = sqrt(weights.smooth);
    for i=2:N
        ddx = x(i+1) - 2*x(i) + x(i-1);
        ddy = y(i+1) - 2*y(i) + y(i-1);
        r(end+1,1) = ws*ddx;
        r(end+1,1) = ws*ddy;
    end

    % time
    wt = sqrt(weights.time);
    for i=1:N
        r(end+1,1) = wt*dt(i);
    end

    % dt bounds
    wdt = sqrt(weights.dt_bound);
    for i=1:N
        r(end+1,1) = wdt*max(0, limits.dt_min - dt(i));
        r(end+1,1) = wdt*max(0, dt(i) - limits.dt_max);
    end

    % v,w
    v = zeros(N,1); om = zeros(N,1);
    for i=1:N
        dx = x(i+1)-x(i);
        dy = y(i+1)-y(i);
        dth = wrap_angle(th(i+1)-th(i));
        seg = hypot(dx,dy);
        v(i) = seg / max(1e-9, dt(i));
        om(i) = dth / max(1e-9, dt(i));
    end

    % v bound
    wv = sqrt(weights.v_bound);
    for i=1:N
        r(end+1,1) = wv*max(0, v(i) - limits.v_max);
    end

    % w bound
    ww = sqrt(weights.w_bound);
    for i=1:N
        r(end+1,1) = ww*max(0, abs(om(i)) - limits.w_max);
    end

    % accel bounds
    wa = sqrt(weights.a_bound);
    wal = sqrt(weights.alpha_bound);
    for i=1:N-1
        dtavg = 0.5*(dt(i)+dt(i+1));
        dtavg = max(1e-9, dtavg);
        a = (v(i+1)-v(i))/dtavg;
        alpha = (om(i+1)-om(i))/dtavg;
        r(end+1,1) = wa*max(0, abs(a) - limits.a_max);
        r(end+1,1) = wal*max(0, abs(alpha) - limits.alpha_max);
    end

    % obstacle clearance
    wo = sqrt(weights.obst);
    for i=1:N+1
        dist = dist_to_nearest(x(i),y(i),obstacles);
        clearance = dist - robot_radius;
        r(end+1,1) = wo*max(0, d_min - clearance);
    end

    % alignment
    wa2 = sqrt(weights.align);
    for i=1:N
        dx = x(i+1)-x(i);
        dy = y(i+1)-y(i);
        phi = atan2(dy,dx);
        r(end+1,1) = wa2*sin(wrap_angle(th(i) - phi));
    end
end

function J = numerical_jacobian(fun, z, eps)
    if nargin < 3, eps = 1e-6; end
    r0 = fun(z);
    m = length(r0);
    n = length(z);
    J = zeros(m,n);
    for j=1:n
        zp = z; zp(j) = zp(j) + eps;
        rp = fun(zp);
        J(:,j) = (rp - r0)/eps;
    end
end

function [z, hist] = solve_lm(fun, z0, max_iters, lam0)
    z = z0;
    lam = lam0;
    hist.cost = zeros(max_iters,1);
    hist.lam  = zeros(max_iters,1);

    for it=1:max_iters
        r = fun(z);
        cost = 0.5*(r'*r);
        hist.cost(it) = cost;
        hist.lam(it) = lam;
        fprintf('iter=%02d cost=%.6f lam=%.3e\n', it-1, cost, lam);

        J = numerical_jacobian(fun, z, 1e-6);
        A = J'*J + lam*eye(length(z));
        b = -J'*r;

        d = A \ b;
        z_new = z + d;
        r_new = fun(z_new);
        cost_new = 0.5*(r_new'*r_new);

        if cost_new < cost
            z = z_new;
            lam = max(lam/2, 1e-9);
        else
            lam = min(lam*5, 1e6);
        end

        if norm(d) < 1e-6
            break;
        end
    end
end
      

6.5 Wolfram Mathematica Implementation

File: Chapter15_Lesson4.nb

(* Chapter15_Lesson4.nb
   Timed-Elastic-Band (TEB) local planning — educational implementation (2D, SE(2))
   Nonlinear least squares with numerical Jacobian + LM damping (dense, for teaching).
*)

ClearAll["Global`*"];

wrapAngle[a_] := Module[{x = Mod[a + Pi, 2 Pi] - Pi}, x];

(* Pack/unpack z = [x0..xN, y0..yN, th0..thN, dt0..dt_{N-1}] *)
packZ[x_, y_, th_, dt_] := Join[x, y, th, dt];

unpackZ[z_, N_] := Module[{nPose = N + 1},
  {
    z[[1 ;; nPose]],
    z[[nPose + 1 ;; 2 nPose]],
    z[[2 nPose + 1 ;; 3 nPose]],
    z[[3 nPose + 1 ;; 3 nPose + N]]
  }
];

distToNearest[px_, py_, obstacles_] := Module[{dmin = Infinity},
  Do[
    With[{cx = obstacles[[k, 1]], cy = obstacles[[k, 2]], r = obstacles[[k, 3]]},
      dmin = Min[dmin, Sqrt[(px - cx)^2 + (py - cy)^2] - r]
    ],
    {k, 1, Length[obstacles]}
  ];
  dmin
];

residuals[z_, N_, obstacles_, robotRadius_, dMin_, limits_, weights_] := Module[
  {x, y, th, dt, ws, wt, wdt, wv, ww, wa, wal, wo, wa2, r = {}, v, om, nPose = N + 1},
  {x, y, th, dt} = unpackZ[z, N];

  dt = Clip[dt, {10^-3, 10}];

  (* smoothness *)
  ws = Sqrt[weights["smooth"]];
  Do[
    AppendTo[r, ws*(x[[i + 1]] - 2 x[[i]] + x[[i - 1]])];
    AppendTo[r, ws*(y[[i + 1]] - 2 y[[i]] + y[[i - 1]])];
    ,
    {i, 2, N}
  ];

  (* time *)
  wt = Sqrt[weights["time"]];
  Do[AppendTo[r, wt*dt[[i]]], {i, 1, N}];

  (* dt bounds *)
  wdt = Sqrt[weights["dtBound"]];
  Do[
    AppendTo[r, wdt*Max[0, limits["dtMin"] - dt[[i]]]];
    AppendTo[r, wdt*Max[0, dt[[i]] - limits["dtMax"]]];
    ,
    {i, 1, N}
  ];

  (* v, w *)
  v = Table[
    With[{dx = x[[i + 1]] - x[[i]], dy = y[[i + 1]] - y[[i]]},
      Sqrt[dx^2 + dy^2]/Max[10^-9, dt[[i]]]
    ],
    {i, 1, N}
  ];
  om = Table[
    wrapAngle[th[[i + 1]] - th[[i]]]/Max[10^-9, dt[[i]]],
    {i, 1, N}
  ];

  (* bounds *)
  wv = Sqrt[weights["vBound"]];
  Do[AppendTo[r, wv*Max[0, v[[i]] - limits["vMax"]]], {i, 1, N}];

  ww = Sqrt[weights["wBound"]];
  Do[AppendTo[r, ww*Max[0, Abs[om[[i]]] - limits["wMax"]]], {i, 1, N}];

  wa = Sqrt[weights["aBound"]];
  wal = Sqrt[weights["alphaBound"]];
  Do[
    With[{dtavg = Max[10^-9, 0.5 (dt[[i]] + dt[[i + 1]])],
          a = (v[[i + 1]] - v[[i]])/Max[10^-9, 0.5 (dt[[i]] + dt[[i + 1]])],
          al = (om[[i + 1]] - om[[i]])/Max[10^-9, 0.5 (dt[[i]] + dt[[i + 1]])]
         },
      AppendTo[r, wa*Max[0, Abs[a] - limits["aMax"]]];
      AppendTo[r, wal*Max[0, Abs[al] - limits["alphaMax"]]];
    ],
    {i, 1, N - 1}
  ];

  (* obstacle clearance *)
  wo = Sqrt[weights["obst"]];
  Do[
    With[{dist = distToNearest[x[[i]], y[[i]], obstacles], clearance = dist - robotRadius},
      AppendTo[r, wo*Max[0, dMin - clearance]];
    ],
    {i, 1, nPose}
  ];

  (* alignment *)
  wa2 = Sqrt[weights["align"]];
  Do[
    With[{dx = x[[i + 1]] - x[[i]], dy = y[[i + 1]] - y[[i]], phi = ArcTan[dx, dy]},
      AppendTo[r, wa2*Sin[wrapAngle[th[[i]] - phi]]];
    ],
    {i, 1, N}
  ];

  r
];

numericalJacobian[fun_, z_, eps_: 10^-6] := Module[{r0 = fun[z], m, n, J, zp, rp},
  m = Length[r0]; n = Length[z];
  J = ConstantArray[0., {m, n}];
  Do[
    zp = z; zp[[j]] += eps;
    rp = fun[zp];
    J[[All, j]] = (rp - r0)/eps;
    ,
    {j, 1, n}
  ];
  J
];

solveLM[fun_, z0_, maxIters_: 18, lam0_: 10^-2] := Module[
  {z = z0, lam = lam0, it, r, cost, J, A, b, d, znew, costNew},
  For[it = 0, it < maxIters, it++,
    r = fun[z];
    cost = 0.5 (r.r);
    Print["iter=", it, " cost=", NumberForm[cost, {8, 6}], " lam=", ScientificForm[lam]];

    J = numericalJacobian[fun, z, 10^-6];
    A = Transpose[J].J + lam IdentityMatrix[Length[z]];
    b = -Transpose[J].r;

    d = LinearSolve[A, b];
    znew = z + d;
    costNew = 0.5 (fun[znew].fun[znew]);

    If[costNew < cost,
      z = znew; lam = Max[lam/2, 10^-9],
      lam = Min[lam*5, 10^6]
    ];

    If[Norm[d] < 10^-6, Break[]];
  ];
  z
];

(* Demo *)
waypoints = { {0, 0}, {0.7, 0.1}, {1.5, 0.2}, {2.2, 0.1}, {3.0, 0.0}, {3.8, 0.1}, {4.6, 0.0}, {5.2, 0.0} };
N = Length[waypoints] - 1;

x = waypoints[[All, 1]];
y = waypoints[[All, 2]];
th = Table[ArcTan[waypoints[[i + 1, 1]] - waypoints[[i, 1]], waypoints[[i + 1, 2]] - waypoints[[i, 2]]], {i, 1, N}];
th = Append[th, th[[-1]]];
dt = ConstantArray[0.25, N];

obstacles = { {2.2, 0.15, 0.35}, {3.9, -0.10, 0.35} };
robotRadius = 0.22; dMin = 0.15;

limits = <|"vMax" -> 0.8, "wMax" -> 1.4, "aMax" -> 1.0, "alphaMax" -> 2.5, "dtMin" -> 0.05, "dtMax" -> 0.60|>;
weights = <|"smooth" -> 2.0, "time" -> 0.4, "obst" -> 6.0, "vBound" -> 3.0, "wBound" -> 2.0, "aBound" -> 1.0,
            "alphaBound" -> 0.8, "align" -> 0.6, "dtBound" -> 2.0|>;

z0 = packZ[x, y, th, dt];
fun = Function[zz, residuals[zz, N, obstacles, robotRadius, dMin, limits, weights]];

zOpt = solveLM[fun, z0, 18, 10^-2];
{xOpt, yOpt, thOpt, dtOpt} = unpackZ[zOpt, N];

Print["Optimized dt sum=", Total[dtOpt], " min=", Min[dtOpt], " max=", Max[dtOpt]];

Show[
  ListLinePlot[{Transpose[{x, y}], Transpose[{xOpt, yOpt}]}, PlotMarkers -> Automatic, PlotLegends -> {"init", "opt"}],
  Graphics@Table[
    With[{cx = obstacles[[k, 1]], cy = obstacles[[k, 2]], rr = obstacles[[k, 3]]},
      Circle[{cx, cy}, rr]
    ],
    {k, 1, Length[obstacles]}
  ],
  AspectRatio -> Automatic, GridLines -> Automatic, PlotLabel -> "TEB demo (educational)"
]
      

Problem 1 (Discrete kinematic proxies): For a pose sequence \( \{(x_i,y_i,\theta_i)\}_{i=0}^N \) and time gaps \( \{\Delta t_i\}_{i=0}^{N-1} \), derive the expressions for \( v_i \) and \( \omega_i \) used in TEB and show that if the segment length is fixed, decreasing \( \Delta t_i \) increases both \( v_i \) and the penalty of any bound \( v_i < v_{\max} \).

Solution: By definition (Section 2),

\[ v_i = \frac{\sqrt{(x_{i+1}-x_i)^2 + (y_{i+1}-y_i)^2} }{\Delta t_i}, \qquad \omega_i = \frac{\mathrm{wrap}(\theta_{i+1}-\theta_i)}{\Delta t_i}. \]

For a fixed segment length \( s_i = \sqrt{(\Delta x_i)^2+(\Delta y_i)^2} \), we have \( v_i = s_i/\Delta t_i \), hence \( \tfrac{\partial v_i}{\partial \Delta t_i} = -s_i/\Delta t_i^2 < 0 \). Therefore, decreasing \( \Delta t_i \) increases \( v_i \), making a hinge penalty \( \max(0, v_i - v_{\max}) \) more likely to activate.

Problem 2 (Elasticity term as a discrete second derivative): Consider the continuous smoothness energy \( E = \int_0^1 \|\tfrac{d^2 \mathbf{p} }{ds^2}\|^2 ds \). Show that on a uniform grid, a consistent discrete approximation is proportional to \( \sum_i \|\mathbf{p}_{i+1} - 2\mathbf{p}_i + \mathbf{p}_{i-1}\|^2 \).

Solution: Let \( s_i = i\,h \) with step \( h \). The standard centered finite difference gives

\[ \frac{d^2 \mathbf{p} }{ds^2}(s_i) \approx \frac{\mathbf{p}(s_{i+1}) - 2\mathbf{p}(s_i) + \mathbf{p}(s_{i-1})}{h^2} = \frac{\mathbf{p}_{i+1} - 2\mathbf{p}_i + \mathbf{p}_{i-1} }{h^2}. \]

Substituting into \( E \) and approximating the integral by a Riemann sum, \( \int_0^1 f(s) ds \approx \sum_i f(s_i) h \), yields

\[ E \approx \sum_i \left\|\frac{\mathbf{p}_{i+1} - 2\mathbf{p}_i + \mathbf{p}_{i-1} }{h^2}\right\|^2 h = \frac{1}{h^3} \sum_i \|\mathbf{p}_{i+1} - 2\mathbf{p}_i + \mathbf{p}_{i-1}\|^2, \]

which is proportional (up to the constant \( 1/h^3 \)) to the discrete second-difference penalty used in TEB. This is the formal link between “band elasticity” and curvature/smoothness regularization.

Problem 3 (Normal equations in Gauss-Newton): Starting from the linearized residual \( \mathbf{r}(\mathbf{z}+\delta) \approx \mathbf{r} + \mathbf{J}\delta \), derive the optimality condition for \( \delta^* \) and state when the solution is unique.

Solution: From Section 4, the condition is

\[ \mathbf{J}^\top\mathbf{J}\,\delta^* = -\mathbf{J}^\top\mathbf{r}. \]

The solution is unique if \( \mathbf{J}^\top\mathbf{J} \) is positive definite, i.e., if \( \mathbf{J} \) has full column rank. In practice, damping \( \lambda\mathbf{I} \) ensures invertibility even when the rank is deficient.

Problem 4 (Obstacle hinge gradient intuition): For a single obstacle, suppose the clearance residual is \( r(\mathbf{p})=\sqrt{w_o}\max(0, d_{\min}-\bar{d}(\mathbf{p})) \). Explain why the gradient is zero when \( \bar{d}(\mathbf{p}) > d_{\min} \).

Solution: If \( \bar{d}(\mathbf{p}) > d_{\min} \), then the hinge is inactive and \( \max(0, d_{\min}-\bar{d})=0 \), so the residual is constant zero in a neighborhood, hence its gradient is also zero. When \( \bar{d}(\mathbf{p}) < d_{\min} \), the hinge is active and the gradient follows the gradient of the distance field, pushing the trajectory away from the obstacle.

We formulated TEB local planning as a sparse nonlinear least-squares problem over discrete poses and time gaps. By constructing residuals for smoothness, time optimality, obstacle clearance, and kinodynamic feasibility (velocity and acceleration bounds), we obtained a principled optimization view that contrasts with sampling-based local planners. The key computational step is Gauss-Newton/Levenberg–Marquardt on the linearized residuals, exploiting sparsity for real-time performance.