We assume a planar wheeled AMR with a unicycle kinematic model and a disc footprint in a 2D world containing static circular obstacles. The global reference is a smooth corridor centerline. Your task is to command admissible controls to reach the goal while maintaining clearance.

\( \mathbf{x}(t) \): robot state \( \mathbf{x}(t) = [x(t),\,y(t),\,\theta(t)]^\top \) and \( \mathbf{u}(t) \): control \( \mathbf{u}(t) = [v(t),\,\omega(t)]^\top \). The unicycle kinematics are

\[ \dot x(t)=v(t)\cos\theta(t),\quad \dot y(t)=v(t)\sin\theta(t),\quad \dot\theta(t)=\omega(t). \]

Obstacles are circles \( \mathcal{O}_i \): \( \|(x,y)-(x_i,y_i)\| \le r_i \). The robot is a disc of radius \( R \). A configuration \( (x,y) \) is collision-free if

\[ \forall i:\quad \|(x,y)-(x_i,y_i)\| - r_i - R \; > \; 0. \]

We will use the signed clearance function \( c(x,y) \): \( c(x,y)=\min_i \big(\|(x,y)-(x_i,y_i)\| - r_i - R\big) \). Safety requires \( c(x,y) > 0 \) along the executed trajectory.

Lessons 3–4 introduced the algorithmic families. In this lab we treat each local planner as a black-box policy: given \( \mathbf{x}_k \), current \( v_k,\omega_k \), a short horizon \( T \), the local map/cost field, and a reference path, it returns one command \( (v_{k+1},\omega_{k+1}) \).

Velocity-space sampling (DWA-style). Sample candidate pairs \( (v,\omega) \) in a dynamic window defined by actuator limits over one step \( \Delta t \):

\[ v \in \Big[\max(0, v_k - a_{\max}\Delta t),\; \min(v_{\max}, v_k + a_{\max}\Delta t)\Big], \\ \omega \in \Big[\max(-\omega_{\max}, \omega_k - \alpha_{\max}\Delta t),\; \min(\omega_{\max}, \omega_k + \alpha_{\max}\Delta t)\Big]. \]

Each candidate is forward-simulated for horizon \( T \), collision-checked using clearance \( c(x,y) \), then scored via a weighted objective.

Trajectory-band optimization (TEB-style). Maintain a band of poses \( \mathbf{p}_0,\dots,\mathbf{p}_N \) near the path and optimize an energy that trades off smoothness, obstacle clearance, and time/feasibility constraints. The command is derived from the first segment direction (and a heading controller). In this lab we use a simplified educational proxy that still expresses the core optimization structure.

flowchart TD
  S["State x_k and current (v_k,w_k)"] --> I["Local map + reference path"]
  I --> DWA["Velocity-space sampling over (v,w)"]
  I --> TEB["Trajectory-band optimization over points"]
  DWA --> R1["Rollout + collision check"]
  R1 --> J1["Score candidates; pick best (v,w)"]
  TEB --> R2["Optimize band; take first segment"]
  R2 --> J2["Compute (v,w) via heading + limits"]
  J1 --> U["Apply command and step dynamics"]
  J2 --> U
        

To compare planners meaningfully, we define metrics that capture: (i) task completion, (ii) efficiency, (iii) safety margin, and (iv) control effort/smoothness. For a discrete rollout at times \( t_k = k\Delta t \):

• Success indicator \( \mathbb{I}_\text{succ} \): \( \mathbb{I}_\text{succ}=1 \) if \( \|\mathbf{p}_K-\mathbf{p}_g\| \le \varepsilon_g \) with no collisions.
• Time-to-goal \( T_g \): \( T_g = K\Delta t \) on success.
• Path length \( L \): \( L = \sum_{k=0}^{K-1} \|\mathbf{p}_{k+1}-\mathbf{p}_k\| \).
• Minimum clearance \( c_{\min} \): \( c_{\min} = \min_k c(\mathbf{p}_k) \).
• Angular effort proxy \( E_\omega \): \( E_\omega = \sum_{k=0}^{K-1} \omega_k^2\Delta t \) (penalizes aggressive turning).

Why discretized collision checking can be justified. Define the obstacle distance-to-set function (without robot radius) \( d(\mathbf{p}) \): \( d(\mathbf{p})=\min_i \big(\|\mathbf{p}-\mathbf{o}_i\|-r_i\big) \). A classical property is that \( d \) is 1-Lipschitz:

\[ |d(\mathbf{p})-d(\mathbf{q})| \; \le \; \|\mathbf{p}-\mathbf{q}\|,\quad \forall\mathbf{p},\mathbf{q}\in\mathbb{R}^2. \]

Proof sketch. For any obstacle set \( \mathcal{O} \), the distance to a closed set satisfies the triangle inequality bound \( d(\mathbf{p}) \le \|\mathbf{p}-\mathbf{q}\| + d(\mathbf{q}) \). Swapping \( \mathbf{p} \) and \( \mathbf{q} \) gives the claim. Therefore if the robot translation per step is bounded as \( \|\mathbf{p}_{k+1}-\mathbf{p}_k\| \le v_{\max}\Delta t \), then the clearance can drop by at most that amount between checks. A conservative discrete-time safety condition is:

\[ c(\mathbf{p}_k) \; > \; v_{\max}\Delta t \quad \Longrightarrow \quad c(\mathbf{p}(t)) \; > \; 0 \; \text{for all}\; t\in[t_k,t_{k+1}], \]

assuming the motion between samples is continuous and the obstacle set is static. This lemma motivates choosing \( \Delta t \) small enough when benchmarking dense obstacles.

We compare planners using the same seeds and world generator. Each trial generates a corridor-like environment by placing circular obstacles near (but not inside) the corridor, then runs the planner until success, collision, or a time limit. We record all metrics and aggregate across trials.

flowchart TD
  A["Set robot + sim params"] --> B["For each seed: generate dense world"]
  B --> C["Initialize state at start"]
  C --> D["Loop: planner -> command (v,w)"]
  D --> E["Roll forward dynamics dt"]
  E --> F["Compute metrics: clearance, length, effort"]
  F --> G["Stop if goal reached or collision or timeout"]
  G --> H["Aggregate across trials: mean + rates"]
  H --> I["Interpret trade-offs and tune weights"]
        

Fairness controls. (1) identical robot limits \( v_{\max},\omega_{\max},a_{\max},\alpha_{\max} \), (2) identical collision model (disc vs circles), (3) identical time step \( \Delta t \), and (4) identical reference path and termination criteria.

The Python script builds the full benchmark harness, including environment generation, DWA rollout scoring, a simplified TEB-like band optimizer, and comparative summaries.

Chapter15_Lesson5.py

# Chapter15_Lesson5.py
# Lab: Compare Local Planners in Dense Obstacles (DWA vs simplified TEB-proxy)
# Requirements: Python 3.9+, numpy, matplotlib (optional for plotting)

import math
import numpy as np

# ---------- World / geometry ----------
def corridor_y(x: float) -> float:
    return 0.9 * math.sin(0.6 * x)

def reference_path(n=120, x0=0.5, x1=11.5):
    xs = np.linspace(x0, x1, n)
    ys = np.array([corridor_y(x) for x in xs])
    return np.c_[xs, ys]

def sample_dense_world(n_obs=45, seed=0):
    rng = np.random.default_rng(seed)
    obs = []
    xmin, xmax, ymin, ymax = 0.0, 12.0, -3.0, 3.0
    tries = 0
    while len(obs) < n_obs and tries < 20000:
        tries += 1
        x = rng.uniform(xmin + 0.5, xmax - 0.5)
        y = rng.uniform(ymin + 0.5, ymax - 0.5)
        r = rng.uniform(0.12, 0.32)
        if abs(y - corridor_y(x)) < 0.55 + r:
            continue
        ok = True
        for (ox, oy, orad) in obs:
            if (x - ox) ** 2 + (y - oy) ** 2 < (r + orad + 0.05) ** 2:
                ok = False
                break
        if ok:
            obs.append((x, y, r))
    return np.array(obs, dtype=float)

def clearance(p, obs, R):
    d = np.inf
    for (ox, oy, r) in obs:
        d = min(d, math.hypot(p[0] - ox, p[1] - oy) - r - R)
    return d

def wrap(a):
    return (a + math.pi) % (2 * math.pi) - math.pi

# ---------- Unicycle ----------
def step_unicycle(x, v, w, dt):
    return np.array([x[0] + v * math.cos(x[2]) * dt,
                     x[1] + v * math.sin(x[2]) * dt,
                     wrap(x[2] + w * dt)], float)

# ---------- DWA (short) ----------
def rollout_min_clear(x, v, w, obs, R, dt, T):
    steps = max(1, int(round(T / dt)))
    xc = x.copy()
    mc = np.inf
    for _ in range(steps):
        xc = step_unicycle(xc, v, w, dt)
        mc = min(mc, clearance(xc[:2], obs, R))
        if mc <= 0:
            break
    return xc, mc

def dwa_command(x, v0, w0, path, obs, prm):
    dt, T = prm["dt"], prm["T"]
    vmin = max(0.0, v0 - prm["a"] * dt); vmax = min(prm["vmax"], v0 + prm["a"] * dt)
    wmin = max(-prm["wmax"], w0 - prm["alpha"] * dt); wmax = min(prm["wmax"], w0 + prm["alpha"] * dt)

    best = (-1e18, 0.0, 0.0)
    goal = path[-1]

    for v in np.linspace(vmin, vmax, 9):
        for w in np.linspace(wmin, wmax, 17):
            xf, mc = rollout_min_clear(x, v, w, obs, prm["R"], dt, T)
            if mc <= prm["R"] + 0.05:
                continue
            goal_dist = np.linalg.norm(xf[:2] - goal)
            J = -0.6 * goal_dist + 1.8 * mc + 0.4 * (v / (prm["vmax"] + 1e-9)) - 0.12 * (w * w)
            if J > best[0]:
                best = (J, float(v), float(w))
    return best[1], best[2]

# ---------- TEB-proxy (short band optimization) ----------
def optimize_band(x, path, obs, prm, N=12, band_len=2.6, iters=20, step=0.12):
    # sample N points ahead along x (good enough for corridor reference)
    x0 = x[0]
    xs = np.linspace(x0, x0 + band_len, N)
    pts = np.c_[xs, np.array([corridor_y(xx) for xx in xs])]
    pts[0] = x[:2]

    for _ in range(iters):
        g = np.zeros_like(pts)

        # smoothness (2nd diff)
        g[1:-1] += 0.35 * (2 * pts[1:-1] - pts[:-2] - pts[2:])

        # obstacle repulsion (nearest obstacle)
        for i in range(1, N):
            best_d = np.inf; best_u = np.zeros(2)
            for (ox, oy, r) in obs:
                dvec = pts[i] - np.array([ox, oy])
                n = np.linalg.norm(dvec) + 1e-12
                d = n - r - prm["R"]
                if d < best_d:
                    best_d = d; best_u = dvec / n
            if best_d < 0.9:
                phi = math.exp(-4.5 * (best_d - 0.9))
                g[i] += 1.8 * (-4.5 * phi) * best_u

        # time preference (shorten segments)
        g[1:] += 0.25 * (pts[1:] - pts[:-1])

        pts[1:] -= step * g[1:]
        pts[0] = x[:2]
    return pts

def teb_command(x, v0, w0, path, obs, prm):
    pts = optimize_band(x, path, obs, prm)
    d = pts[1] - pts[0]
    heading = math.atan2(d[1], d[0])
    herr = wrap(heading - x[2])
    w = max(-prm["wmax"], min(prm["wmax"], 2.2 * herr))
    v = max(0.0, min(prm["vmax"], 0.8 * (np.linalg.norm(d) / prm["dt"])))
    v *= 1.0 / (1.0 + 1.2 * abs(w))
    # accel limits
    v = max(max(0.0, v0 - prm["a"] * prm["dt"]), min(prm["vmax"], v0 + prm["a"] * prm["dt"], v))
    w = max(w0 - prm["alpha"] * prm["dt"], min(w0 + prm["alpha"] * prm["dt"], w))
    return v, w

# ---------- Benchmark ----------
def run_one(planner, seed, prm):
    obs = sample_dense_world(prm["n_obs"], seed)
    path = reference_path()
    x = np.array([0.6, 0.0, 0.0], float)
    v = 0.0; w = 0.0

    L = 0.0; cmin = np.inf; w2 = 0.0
    goal = path[-1]

    for k in range(prm["max_steps"]):
        c = clearance(x[:2], obs, prm["R"])
        cmin = min(cmin, c)
        if c <= 0:
            return dict(success=False, collision=True, time=k * prm["dt"], L=L, cmin=cmin, w2=w2)
        if np.linalg.norm(x[:2] - goal) <= prm["goal_tol"]:
            return dict(success=True, collision=False, time=k * prm["dt"], L=L, cmin=cmin, w2=w2)

        if planner == "teb":
            vcmd, wcmd = teb_command(x, v, w, path, obs, prm)
        else:
            vcmd, wcmd = dwa_command(x, v, w, path, obs, prm)

        x2 = step_unicycle(x, vcmd, wcmd, prm["dt"])
        L += float(np.linalg.norm(x2[:2] - x[:2]))
        w2 += float((wcmd ** 2) * prm["dt"])
        x, v, w = x2, vcmd, wcmd

    return dict(success=False, collision=False, time=prm["max_steps"] * prm["dt"], L=L, cmin=cmin, w2=w2)

def summarize(results):
    n = len(results)
    succ = sum(r["success"] for r in results)
    col  = sum(r["collision"] for r in results)
    def mean(key): return float(np.mean([r[key] for r in results]))
    return dict(trials=n, success_rate=succ / n, collision_rate=col / n,
                time_mean=mean("time"), L_mean=mean("L"), cmin_mean=mean("cmin"), w2_mean=mean("w2"))

def compare(trials=30, seed0=0):
    prm = dict(R=0.25, vmax=0.9, wmax=1.6, a=1.2, alpha=2.5,
               dt=0.1, T=2.0, goal_tol=0.25, max_steps=800, n_obs=45)
    for name in ["dwa", "teb"]:
        res = [run_one(name, seed0 + i, prm) for i in range(trials)]
        print(name.upper(), summarize(res))

if __name__ == "__main__":
    compare(30, 0)
      

Chapter15_Lesson5.cpp

// Chapter15_Lesson5.cpp
// Lab: Compare Local Planners in Dense Obstacles (DWA vs simplified TEB-proxy)
// Build: g++ -O2 -std=c++17 Chapter15_Lesson5.cpp -o lab
// Run:   ./lab 30 0
#include <algorithm>
#include <array>
#include <cmath>
#include <iostream>
#include <random>
#include <vector>

struct Ob { double x,y,r; };
struct Res { bool success=false, collision=false; double time=0, L=0, cmin=1e9, w2=0; };

static inline double wrap(double a){ a = std::fmod(a + M_PI, 2*M_PI); if(a<0) a+=2*M_PI; return a - M_PI; }
static inline double hypot2(double x,double y){ return std::sqrt(x*x+y*y); }
static inline double corridor_y(double x){ return 0.9*std::sin(0.6*x); }

std::vector<std::array<double,2>> reference_path(int n=120){
  std::vector<std::array<double,2>> p; p.reserve(n);
  double x0=0.5,x1=11.5;
  for(int i=0;i<n;i++){
    double x = x0 + (x1-x0)*double(i)/(n-1);
    p.push_back({x, corridor_y(x)});
  }
  return p;
}

std::vector<Ob> sample_dense_world(int nObs, int seed){
  std::mt19937 rng(seed);
  std::uniform_real_distribution<double> Ux(0.5, 11.5), Uy(-2.5, 2.5), Ur(0.12, 0.32);
  std::vector<Ob> obs; obs.reserve(nObs);
  int tries=0;
  while((int)obs.size()<nObs && tries<20000){
    tries++;
    double x=Ux(rng), y=Uy(rng), r=Ur(rng);
    if(std::abs(y - corridor_y(x)) < 0.55 + r) continue;
    bool ok=true;
    for(auto &o: obs){
      double dx=x-o.x, dy=y-o.y;
      double rr=r+o.r+0.05;
      if(dx*dx+dy*dy < rr*rr){ ok=false; break; }
    }
    if(ok) obs.push_back({x,y,r});
  }
  return obs;
}

double clearance(double px,double py, const std::vector<Ob>& obs, double R){
  double d=1e18;
  for(auto &o: obs){
    d = std::min(d, hypot2(px-o.x, py-o.y) - o.r - R);
  }
  return d;
}

std::array<double,3> step_unicycle(const std::array<double,3>& x, double v,double w,double dt){
  return { x[0] + v*std::cos(x[2])*dt,
           x[1] + v*std::sin(x[2])*dt,
           wrap(x[2] + w*dt) };
}

std::pair<std::array<double,3>, double> rollout_min_clear(std::array<double,3> x, double v,double w,
                                                          const std::vector<Ob>& obs, double R, double dt, double T){
  int steps = std::max(1, (int)std::lround(T/dt));
  double mc=1e18;
  for(int i=0;i<steps;i++){
    x = step_unicycle(x,v,w,dt);
    mc = std::min(mc, clearance(x[0],x[1],obs,R));
    if(mc<=0) break;
  }
  return {x, mc};
}

struct Params{
  double R=0.25, vmax=0.9, wmax=1.6, a=1.2, alpha=2.5;
  double dt=0.1, T=2.0, goal_tol=0.25;
  int max_steps=800, n_obs=45;
};

std::pair<double,double> dwa_command(const std::array<double,3>& x, double v0,double w0,
                                     const std::vector<std::array<double,2>>& path,
                                     const std::vector<Ob>& obs, const Params& p){
  double vmin=std::max(0.0, v0 - p.a*p.dt), vmax=std::min(p.vmax, v0 + p.a*p.dt);
  double wmin=std::max(-p.wmax, w0 - p.alpha*p.dt), wmax=std::min(p.wmax, w0 + p.alpha*p.dt);

  double bestJ=-1e18, bestV=0, bestW=0;
  auto goal = path.back();

  for(int i=0;i<9;i++){
    double v = vmin + (vmax-vmin)*double(i)/8.0;
    for(int j=0;j<17;j++){
      double w = wmin + (wmax-wmin)*double(j)/16.0;
      auto rr = rollout_min_clear(x,v,w,obs,p.R,p.dt,p.T);
      double mc = rr.second;
      if(mc <= p.R + 0.05) continue;
      double gx = rr.first[0]-goal[0], gy = rr.first[1]-goal[1];
      double goalDist = hypot2(gx,gy);
      double J = -0.6*goalDist + 1.8*mc + 0.4*(v/(p.vmax+1e-9)) - 0.12*(w*w);
      if(J>bestJ){ bestJ=J; bestV=v; bestW=w; }
    }
  }
  return {bestV, bestW};
}

std::vector<std::array<double,2>> optimize_band(const std::array<double,3>& x,
                                                const std::vector<Ob>& obs, const Params& p,
                                                int N=12, double bandLen=2.6, int iters=20, double step=0.12){
  std::vector<std::array<double,2>> pts(N);
  for(int i=0;i<N;i++){
    double xx = x[0] + bandLen*double(i)/(N-1);
    pts[i] = {xx, corridor_y(xx)};
  }
  pts[0] = {x[0], x[1]};

  for(int it=0; it<iters; ++it){
    std::vector<std::array<double,2>> g(N, {0,0});

    // smoothness
    for(int i=1;i<N-1;i++){
      g[i][0] += 0.35*(2*pts[i][0] - pts[i-1][0] - pts[i+1][0]);
      g[i][1] += 0.35*(2*pts[i][1] - pts[i-1][1] - pts[i+1][1]);
    }

    // obstacle repulsion (nearest obstacle)
    for(int i=1;i<N;i++){
      double bestD=1e18; std::array<double,2> bestU{0,0};
      for(auto &o: obs){
        double vx=pts[i][0]-o.x, vy=pts[i][1]-o.y;
        double n = hypot2(vx,vy) + 1e-12;
        double d = n - o.r - p.R;
        if(d<bestD){ bestD=d; bestU={vx/n, vy/n}; }
      }
      if(bestD < 0.9){
        double phi = std::exp(-4.5*(bestD - 0.9));
        g[i][0] += 1.8*(-4.5*phi)*bestU[0];
        g[i][1] += 1.8*(-4.5*phi)*bestU[1];
      }
    }

    // time preference
    for(int i=1;i<N;i++){
      g[i][0] += 0.25*(pts[i][0]-pts[i-1][0]);
      g[i][1] += 0.25*(pts[i][1]-pts[i-1][1]);
    }

    for(int i=1;i<N;i++){
      pts[i][0] -= step*g[i][0];
      pts[i][1] -= step*g[i][1];
    }
    pts[0] = {x[0], x[1]};
  }
  return pts;
}

std::pair<double,double> teb_command(const std::array<double,3>& x, double v0,double w0,
                                     const std::vector<std::array<double,2>>& path,
                                     const std::vector<Ob>& obs, const Params& p){
  (void)path; // not needed in this proxy
  auto pts = optimize_band(x, obs, p);
  double dx=pts[1][0]-pts[0][0], dy=pts[1][1]-pts[0][1];
  double heading = std::atan2(dy,dx);
  double herr = wrap(heading - x[2]);

  double w = std::clamp(2.2*herr, -p.wmax, p.wmax);
  double v = std::clamp(0.8*(hypot2(dx,dy)/p.dt), 0.0, p.vmax);
  v *= 1.0/(1.0 + 1.2*std::abs(w));

  // accel limits
  v = std::clamp(v, std::max(0.0, v0 - p.a*p.dt), std::min(p.vmax, v0 + p.a*p.dt));
  w = std::clamp(w, w0 - p.alpha*p.dt, w0 + p.alpha*p.dt);
  return {v,w};
}

Res run_one(const std::string& planner, int seed, const Params& p){
  auto obs  = sample_dense_world(p.n_obs, seed);
  auto path = reference_path();
  auto goal = path.back();

  std::array<double,3> x{0.6,0.0,0.0};
  double v=0,w=0;
  Res r; r.time = p.max_steps*p.dt;

  for(int k=0;k<p.max_steps;k++){
    double c = clearance(x[0],x[1],obs,p.R);
    r.cmin = std::min(r.cmin, c);
    if(c<=0){ r.collision=true; r.time=k*p.dt; return r; }

    double gdist = hypot2(x[0]-goal[0], x[1]-goal[1]);
    if(gdist <= p.goal_tol){ r.success=true; r.time=k*p.dt; return r; }

    double vcmd, wcmd;
    if(planner=="teb"){ auto u = teb_command(x,v,w,path,obs,p); vcmd=u.first; wcmd=u.second; }
    else             { auto u = dwa_command(x,v,w,path,obs,p); vcmd=u.first; wcmd=u.second; }

    auto x2 = step_unicycle(x, vcmd, wcmd, p.dt);
    r.L  += hypot2(x2[0]-x[0], x2[1]-x[1]);
    r.w2 += (wcmd*wcmd)*p.dt;
    x=x2; v=vcmd; w=wcmd;
  }
  return r;
}

struct Summary{ int n=0; double succ=0,col=0, t=0,L=0,cmin=0,w2=0; };

Summary summarize(const std::vector<Res>& R){
  Summary s; s.n = (int)R.size();
  for(auto &r: R){
    s.succ += r.success?1:0;
    s.col  += r.collision?1:0;
    s.t    += r.time; s.L += r.L; s.cmin += r.cmin; s.w2 += r.w2;
  }
  s.t/=s.n; s.L/=s.n; s.cmin/=s.n; s.w2/=s.n;
  return s;
}

int main(int argc, char** argv){
  int trials = (argc>1)? std::atoi(argv[1]) : 30;
  int seed0  = (argc>2)? std::atoi(argv[2]) : 0;
  Params p;

  for(auto name: {std::string("dwa"), std::string("teb")}){
    std::vector<Res> R; R.reserve(trials);
    for(int i=0;i<trials;i++) R.push_back(run_one(name, seed0+i, p));
    auto s = summarize(R);
    std::cout << "\n" << (name=="dwa"?"DWA":"TEB") << " trials="<<s.n
              << " success_rate="<< (s.succ/s.n)
              << " collision_rate="<< (s.col/s.n)
              << " time_mean="<< s.t
              << " L_mean="<< s.L
              << " cmin_mean="<< s.cmin
              << " w2_mean="<< s.w2
              << "\n";
  }
  return 0;
}
      

Chapter15_Lesson5.java

// Chapter15_Lesson5.java
// Lab: Compare Local Planners in Dense Obstacles (DWA vs simplified TEB-proxy)
// Build: javac Chapter15_Lesson5.java
// Run:   java Chapter15_Lesson5 30 0
import java.util.*;

public class Chapter15_Lesson5 {

  static class Ob { double x,y,r; Ob(double x,double y,double r){this.x=x;this.y=y;this.r=r;} }
  static class Res { boolean success=false, collision=false; double time=0,L=0,cmin=1e9,w2=0; }
  static class Params{
    double R=0.25,vmax=0.9,wmax=1.6,a=1.2,alpha=2.5,dt=0.1,T=2.0,goalTol=0.25;
    int maxSteps=800,nObs=45;
  }

  static double wrap(double a){
    a = (a + Math.PI) % (2*Math.PI);
    if(a < 0) a += 2*Math.PI;
    return a - Math.PI;
  }
  static double hypot(double x,double y){ return Math.hypot(x,y); }
  static double corridorY(double x){ return 0.9*Math.sin(0.6*x); }

  static double[][] referencePath(int n){
    double x0=0.5,x1=11.5;
    double[][] p = new double[n][2];
    for(int i=0;i<n;i++){
      double x = x0 + (x1-x0)*((double)i/(n-1));
      p[i][0]=x; p[i][1]=corridorY(x);
    }
    return p;
  }

  static ArrayList<Ob> sampleDenseWorld(int nObs, int seed){
    Random rng = new Random(seed);
    ArrayList<Ob> obs = new ArrayList<>(nObs);
    int tries=0;
    while(obs.size()<nObs && tries<20000){
      tries++;
      double x = 0.5 + 11.0*rng.nextDouble();
      double y = -2.5 + 5.0*rng.nextDouble();
      double r = 0.12 + 0.20*rng.nextDouble();
      if(Math.abs(y - corridorY(x)) < 0.55 + r) continue;

      boolean ok=true;
      for(Ob o: obs){
        double dx=x-o.x, dy=y-o.y;
        double rr=r+o.r+0.05;
        if(dx*dx+dy*dy < rr*rr){ ok=false; break; }
      }
      if(ok) obs.add(new Ob(x,y,r));
    }
    return obs;
  }

  static double clearance(double px,double py, ArrayList<Ob> obs, double R){
    double d=1e18;
    for(Ob o: obs){
      d = Math.min(d, hypot(px-o.x, py-o.y) - o.r - R);
    }
    return d;
  }

  static double[] stepUnicycle(double[] x, double v,double w,double dt){
    return new double[]{ x[0] + v*Math.cos(x[2])*dt,
                         x[1] + v*Math.sin(x[2])*dt,
                         wrap(x[2] + w*dt) };
  }

  static class Rollout { double[] xf; double minClear; Rollout(double[] xf,double mc){this.xf=xf; this.minClear=mc;} }

  static Rollout rolloutMinClear(double[] x0, double v,double w, ArrayList<Ob> obs, Params p){
    int steps = Math.max(1, (int)Math.round(p.T/p.dt));
    double[] x = x0.clone();
    double mc = 1e18;
    for(int i=0;i<steps;i++){
      x = stepUnicycle(x,v,w,p.dt);
      mc = Math.min(mc, clearance(x[0],x[1],obs,p.R));
      if(mc<=0) break;
    }
    return new Rollout(x,mc);
  }

  static double[] dwaCommand(double[] x, double v0,double w0, double[][] path, ArrayList<Ob> obs, Params p){
    double vmin=Math.max(0.0, v0 - p.a*p.dt), vmax=Math.min(p.vmax, v0 + p.a*p.dt);
    double wmin=Math.max(-p.wmax, w0 - p.alpha*p.dt), wmax=Math.min(p.wmax, w0 + p.alpha*p.dt);
    double bestJ=-1e18, bestV=0, bestW=0;
    double[] goal = path[path.length-1];

    for(int i=0;i<9;i++){
      double v = vmin + (vmax-vmin)*(i/8.0);
      for(int j=0;j<17;j++){
        double w = wmin + (wmax-wmin)*(j/16.0);
        Rollout rr = rolloutMinClear(x,v,w,obs,p);
        double mc = rr.minClear;
        if(mc <= p.R + 0.05) continue;
        double gx=rr.xf[0]-goal[0], gy=rr.xf[1]-goal[1];
        double goalDist = hypot(gx,gy);
        double J = -0.6*goalDist + 1.8*mc + 0.4*(v/(p.vmax+1e-9)) - 0.12*(w*w);
        if(J>bestJ){ bestJ=J; bestV=v; bestW=w; }
      }
    }
    return new double[]{bestV,bestW};
  }

  static double[][] optimizeBand(double[] x, ArrayList<Ob> obs, Params p, int N, double bandLen, int iters, double step){
    double[][] pts = new double[N][2];
    for(int i=0;i<N;i++){
      double xx = x[0] + bandLen*(i/(double)(N-1));
      pts[i][0]=xx; pts[i][1]=corridorY(xx);
    }
    pts[0][0]=x[0]; pts[0][1]=x[1];

    for(int it=0; it<iters; it++){
      double[][] g = new double[N][2];

      for(int i=1;i<N-1;i++){
        g[i][0] += 0.35*(2*pts[i][0] - pts[i-1][0] - pts[i+1][0]);
        g[i][1] += 0.35*(2*pts[i][1] - pts[i-1][1] - pts[i+1][1]);
      }

      for(int i=1;i<N;i++){
        double bestD=1e18, ux=0, uy=0;
        for(Ob o: obs){
          double vx=pts[i][0]-o.x, vy=pts[i][1]-o.y;
          double n = hypot(vx,vy) + 1e-12;
          double d = n - o.r - p.R;
          if(d<bestD){ bestD=d; ux=vx/n; uy=vy/n; }
        }
        if(bestD < 0.9){
          double phi = Math.exp(-4.5*(bestD - 0.9));
          g[i][0] += 1.8*(-4.5*phi)*ux;
          g[i][1] += 1.8*(-4.5*phi)*uy;
        }
      }

      for(int i=1;i<N;i++){
        g[i][0] += 0.25*(pts[i][0]-pts[i-1][0]);
        g[i][1] += 0.25*(pts[i][1]-pts[i-1][1]);
      }

      for(int i=1;i<N;i++){
        pts[i][0] -= step*g[i][0];
        pts[i][1] -= step*g[i][1];
      }
      pts[0][0]=x[0]; pts[0][1]=x[1];
    }
    return pts;
  }

  static double[] tebCommand(double[] x, double v0,double w0, double[][] path, ArrayList<Ob> obs, Params p){
    double[][] pts = optimizeBand(x, obs, p, 12, 2.6, 20, 0.12);
    double dx=pts[1][0]-pts[0][0], dy=pts[1][1]-pts[0][1];
    double heading = Math.atan2(dy,dx);
    double herr = wrap(heading - x[2]);

    double w = Math.max(-p.wmax, Math.min(p.wmax, 2.2*herr));
    double v = Math.max(0.0, Math.min(p.vmax, 0.8*(hypot(dx,dy)/p.dt)));
    v *= 1.0/(1.0 + 1.2*Math.abs(w));

    v = Math.max(Math.max(0.0, v0 - p.a*p.dt), Math.min(Math.min(p.vmax, v0 + p.a*p.dt), v));
    w = Math.max(w0 - p.alpha*p.dt, Math.min(w0 + p.alpha*p.dt, w));
    return new double[]{v,w};
  }

  static Res runOne(String planner, int seed, Params p){
    ArrayList<Ob> obs = sampleDenseWorld(p.nObs, seed);
    double[][] path = referencePath(120);
    double[] goal = path[path.length-1];

    double[] x = new double[]{0.6,0.0,0.0};
    double v=0,w=0;
    Res r = new Res();
    r.time = p.maxSteps*p.dt;

    for(int k=0;k<p.maxSteps;k++){
      double c = clearance(x[0],x[1],obs,p.R);
      r.cmin = Math.min(r.cmin, c);
      if(c<=0){ r.collision=true; r.time=k*p.dt; return r; }
      if(hypot(x[0]-goal[0], x[1]-goal[1]) <= p.goalTol){ r.success=true; r.time=k*p.dt; return r; }

      double[] u = planner.equals("teb") ? tebCommand(x,v,w,path,obs,p) : dwaCommand(x,v,w,path,obs,p);
      double vcmd=u[0], wcmd=u[1];

      double[] x2 = stepUnicycle(x, vcmd, wcmd, p.dt);
      r.L += hypot(x2[0]-x[0], x2[1]-x[1]);
      r.w2 += (wcmd*wcmd)*p.dt;
      x=x2; v=vcmd; w=wcmd;
    }
    return r;
  }

  static Map<String,Double> summarize(ArrayList<Res> R){
    int n=R.size();
    double succ=0,col=0,t=0,L=0,cmin=0,w2=0;
    for(Res r: R){
      succ += r.success?1:0; col += r.collision?1:0;
      t += r.time; L += r.L; cmin += r.cmin; w2 += r.w2;
    }
    LinkedHashMap<String,Double> s = new LinkedHashMap<>();
    s.put("trials", (double)n);
    s.put("success_rate", succ/n);
    s.put("collision_rate", col/n);
    s.put("time_mean", t/n);
    s.put("L_mean", L/n);
    s.put("cmin_mean", cmin/n);
    s.put("w2_mean", w2/n);
    return s;
  }

  public static void main(String[] args){
    int trials = (args.length>0) ? Integer.parseInt(args[0]) : 30;
    int seed0  = (args.length>1) ? Integer.parseInt(args[1]) : 0;
    Params p = new Params();

    for(String name: new String[]{"dwa","teb"}){
      ArrayList<Res> R = new ArrayList<>(trials);
      for(int i=0;i<trials;i++) R.add(runOne(name, seed0+i, p));
      System.out.println("\n" + name.toUpperCase() + " " + summarize(R));
    }
  }
}
      

Chapter15_Lesson5.m

% Chapter15_Lesson5.m
% Lab: Compare Local Planners in Dense Obstacles (DWA vs simplified TEB-proxy)
% Run:
%   ComparePlanners(30, 0);

function ComparePlanners(trials, seed0)
  if nargin < 1, trials = 30; end
  if nargin < 2, seed0 = 0; end

  prm.R=0.25; prm.vmax=0.9; prm.wmax=1.6; prm.a=1.2; prm.alpha=2.5;
  prm.dt=0.1; prm.T=2.0; prm.goal_tol=0.25; prm.max_steps=800; prm.n_obs=45;

  fprintf('\nDWA: '); disp(Summarize(RunMany('dwa', trials, seed0, prm)));
  fprintf('\nTEB: '); disp(Summarize(RunMany('teb', trials, seed0, prm)));
end

function R = RunMany(name, trials, seed0, prm)
  R = repmat(struct('success',false,'collision',false,'time',0,'L',0,'cmin',inf,'w2',0), trials, 1);
  for i=1:trials
    R(i) = RunOne(name, seed0 + (i-1), prm);
  end
end

function S = Summarize(R)
  n = numel(R);
  succ = sum([R.success]); col = sum([R.collision]);
  S.trials = n;
  S.success_rate = succ/n;
  S.collision_rate = col/n;
  S.time_mean = mean([R.time]);
  S.L_mean = mean([R.L]);
  S.cmin_mean = mean([R.cmin]);
  S.w2_mean = mean([R.w2]);
end

% ---------- World / geometry ----------
function y = corridor_y(x), y = 0.9*sin(0.6*x); end

function path = reference_path()
  xs = linspace(0.5, 11.5, 120)';
  ys = arrayfun(@corridor_y, xs);
  path = [xs, ys];
end

function obs = sample_dense_world(n_obs, seed)
  rng(seed);
  obs = zeros(0,3);
  tries = 0;
  while size(obs,1) < n_obs && tries < 20000
    tries = tries + 1;
    x = 0.5 + 11.0*rand();
    y = -2.5 + 5.0*rand();
    r = 0.12 + 0.20*rand();
    if abs(y - corridor_y(x)) < 0.55 + r, continue; end
    ok = true;
    for k=1:size(obs,1)
      dx = x-obs(k,1); dy = y-obs(k,2);
      rr = r + obs(k,3) + 0.05;
      if dx*dx + dy*dy < rr*rr, ok=false; break; end
    end
    if ok, obs(end+1,:) = [x,y,r]; end %#ok<AGROW>
  end
end

function c = clearance(p, obs, R)
  d = inf;
  for i=1:size(obs,1)
    d = min(d, hypot(p(1)-obs(i,1), p(2)-obs(i,2)) - obs(i,3) - R);
  end
  c = d;
end

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

% ---------- Unicycle ----------
function x2 = step_unicycle(x, v, w, dt)
  x2 = [x(1) + v*cos(x(3))*dt;
        x(2) + v*sin(x(3))*dt;
        wrap(x(3) + w*dt)];
end

% ---------- DWA ----------
function [xf, mc] = rollout_min_clear(x, v, w, obs, prm)
  steps = max(1, round(prm.T/prm.dt));
  mc = inf; xf = x;
  for k=1:steps
    xf = step_unicycle(xf, v, w, prm.dt);
    mc = min(mc, clearance(xf(1:2)', obs, prm.R));
    if mc <= 0, break; end
  end
end

function [vbest, wbest] = dwa_command(x, v0, w0, path, obs, prm)
  vmin = max(0, v0 - prm.a*prm.dt); vmax = min(prm.vmax, v0 + prm.a*prm.dt);
  wmin = max(-prm.wmax, w0 - prm.alpha*prm.dt); wmax = min(prm.wmax, w0 + prm.alpha*prm.dt);

  goal = path(end,:);
  bestJ = -1e18; vbest = 0; wbest = 0;

  for v = linspace(vmin, vmax, 9)
    for w = linspace(wmin, wmax, 17)
      [xf, mc] = rollout_min_clear(x, v, w, obs, prm);
      if mc <= prm.R + 0.05, continue; end
      goalDist = norm(xf(1:2)' - goal);
      J = -0.6*goalDist + 1.8*mc + 0.4*(v/(prm.vmax+1e-9)) - 0.12*w*w;
      if J > bestJ, bestJ = J; vbest = v; wbest = w; end
    end
  end
end

% ---------- TEB-proxy ----------
function pts = optimize_band(x, obs, prm, N, bandLen, iters, step)
  if nargin < 4, N=12; end
  if nargin < 5, bandLen=2.6; end
  if nargin < 6, iters=20; end
  if nargin < 7, step=0.12; end

  xs = linspace(x(1), x(1)+bandLen, N)';
  pts = [xs, arrayfun(@corridor_y, xs)];
  pts(1,:) = x(1:2)';

  for it=1:iters
    g = zeros(size(pts));

    g(2:end-1,:) = g(2:end-1,:) + 0.35*(2*pts(2:end-1,:) - pts(1:end-2,:) - pts(3:end,:));

    for i=2:N
      bestD = inf; bestU = [0,0];
      for k=1:size(obs,1)
        dvec = pts(i,:) - obs(k,1:2);
        n = norm(dvec) + 1e-12;
        d = n - obs(k,3) - prm.R;
        if d < bestD, bestD = d; bestU = dvec/n; end
      end
      if bestD < 0.9
        phi = exp(-4.5*(bestD - 0.9));
        g(i,:) = g(i,:) + 1.8*(-4.5*phi)*bestU;
      end
    end

    g(2:end,:) = g(2:end,:) + 0.25*(pts(2:end,:) - pts(1:end-1,:));

    pts(2:end,:) = pts(2:end,:) - step*g(2:end,:);
    pts(1,:) = x(1:2)';
  end
end

function [vcmd, wcmd] = teb_command(x, v0, w0, path, obs, prm) %#ok<INUSD>
  pts = optimize_band(x, obs, prm);
  d = pts(2,:) - pts(1,:);
  heading = atan2(d(2), d(1));
  herr = wrap(heading - x(3));

  wcmd = max(-prm.wmax, min(prm.wmax, 2.2*herr));
  vcmd = max(0, min(prm.vmax, 0.8*(norm(d)/prm.dt)));
  vcmd = vcmd / (1 + 1.2*abs(wcmd));

  vcmd = max(max(0, v0 - prm.a*prm.dt), min(min(prm.vmax, v0 + prm.a*prm.dt), vcmd));
  wcmd = max(w0 - prm.alpha*prm.dt, min(w0 + prm.alpha*prm.dt, wcmd));
end

% ---------- Simulation ----------
function r = RunOne(planner, seed, prm)
  obs = sample_dense_world(prm.n_obs, seed);
  path = reference_path();
  goal = path(end,:);

  x = [0.6;0.0;0.0]; v=0; w=0;
  L=0; cmin=inf; w2=0;

  for k=1:prm.max_steps
    c = clearance(x(1:2)', obs, prm.R);
    cmin = min(cmin, c);
    if c <= 0
      r = struct('success',false,'collision',true,'time',(k-1)*prm.dt,'L',L,'cmin',cmin,'w2',w2);
      return;
    end
    if norm(x(1:2)' - goal) <= prm.goal_tol
      r = struct('success',true,'collision',false,'time',(k-1)*prm.dt,'L',L,'cmin',cmin,'w2',w2);
      return;
    end

    if strcmpi(planner,'teb')
      [vcmd,wcmd] = teb_command(x,v,w,path,obs,prm);
    else
      [vcmd,wcmd] = dwa_command(x,v,w,path,obs,prm);
    end

    x2 = step_unicycle(x, vcmd, wcmd, prm.dt);
    L = L + norm(x2(1:2)-x(1:2));
    w2 = w2 + (wcmd*wcmd)*prm.dt;
    x = x2; v=vcmd; w=wcmd;
  end

  r = struct('success',false,'collision',false,'time',prm.max_steps*prm.dt,'L',L,'cmin',cmin,'w2',w2);
end

% ---------------- Optional: Simulink Harness Builder ----------------
% BuildSimulinkHarness('AMR_UnicycleHarness');

function BuildSimulinkHarness(modelName)
  if nargin < 1, modelName = 'AMR_UnicycleHarness'; end
  if bdIsLoaded(modelName), close_system(modelName,0); end
  new_system(modelName); open_system(modelName);
  set_param(modelName, 'StopTime', '20');

  add_block('simulink/Sources/From Workspace', [modelName '/Vin'], 'VariableName', 'Vin', 'Position', [30 60 140 90]);
  add_block('simulink/Sources/From Workspace', [modelName '/Win'], 'VariableName', 'Win', 'Position', [30 140 140 170]);

  add_block('simulink/Math Operations/Trigonometric Function', [modelName '/cos'], 'Operator', 'cos', 'Position', [220 40 270 80]);
  add_block('simulink/Math Operations/Trigonometric Function', [modelName '/sin'], 'Operator', 'sin', 'Position', [220 110 270 150]);
  add_block('simulink/Math Operations/Product', [modelName '/v*cos'], 'Position', [320 55 360 85]);
  add_block('simulink/Math Operations/Product', [modelName '/v*sin'], 'Position', [320 125 360 155]);

  add_block('simulink/Continuous/Integrator', [modelName '/x'], 'Position', [420 55 450 85]);
  add_block('simulink/Continuous/Integrator', [modelName '/y'], 'Position', [420 125 450 155]);
  add_block('simulink/Continuous/Integrator', [modelName '/theta'], 'Position', [220 200 250 230]);

  add_block('simulink/Sinks/To Workspace', [modelName '/x_out'], 'VariableName', 'x_out', 'SaveFormat', 'Array', 'Position', [520 55 600 85]);
  add_block('simulink/Sinks/To Workspace', [modelName '/y_out'], 'VariableName', 'y_out', 'SaveFormat', 'Array', 'Position', [520 125 600 155]);
  add_block('simulink/Sinks/To Workspace', [modelName '/theta_out'], 'VariableName', 'theta_out', 'SaveFormat', 'Array', 'Position', [320 200 410 230]);

  add_line(modelName, 'theta/1', 'cos/1'); add_line(modelName, 'theta/1', 'sin/1');
  add_line(modelName, 'Vin/1', 'v*cos/1'); add_line(modelName, 'cos/1', 'v*cos/2');
  add_line(modelName, 'Vin/1', 'v*sin/1'); add_line(modelName, 'sin/1', 'v*sin/2');
  add_line(modelName, 'v*cos/1', 'x/1'); add_line(modelName, 'v*sin/1', 'y/1');
  add_line(modelName, 'Win/1', 'theta/1');
  add_line(modelName, 'x/1', 'x_out/1'); add_line(modelName, 'y/1', 'y_out/1'); add_line(modelName, 'theta/1', 'theta_out/1');

  save_system(modelName);
end
      

Chapter15_Lesson5.nb

(* Chapter15_Lesson5.nb (Wolfram Language script) *)
(* Lab: Compare Local Planners in Dense Obstacles (DWA vs simplified TEB-proxy) *)

ClearAll["Global`*"];

corridorY[x_] := 0.9*Sin[0.6*x];
referencePath[n_:120] := Table[{x, corridorY[x]}, {x, 0.5, 11.5, (11.5-0.5)/(n-1)}];

sampleDenseWorld[nObs_:45, seed_:0] := Module[
  {rng = RandomGeneratorState[], obs = {}, tries = 0, x, y, r, ok},
  SeedRandom[seed];
  While[Length[obs] < nObs && tries < 20000,
    tries++;
    x = RandomReal[{0.5, 11.5}];
    y = RandomReal[{-2.5, 2.5}];
    r = RandomReal[{0.12, 0.32}];
    If[Abs[y - corridorY[x]] < 0.55 + r, Continue[]];
    ok = True;
    Do[
      If[(x - o[[1]])^2 + (y - o[[2]])^2 < (r + o[[3]] + 0.05)^2, ok = False; Break[]],
      {o, obs}
    ];
    If[ok, obs = Append[obs, {x, y, r}]];
  ];
  RandomGeneratorState[rng];
  obs
];

clearance[p_, obs_, R_] := Min@Table[Norm[p - o[[{1,2}]]] - o[[3]] - R, {o, obs}];

wrap[a_] := Mod[a + Pi, 2 Pi] - Pi;

stepUnicycle[x_, v_, w_, dt_] := {
  x[[1]] + v*Cos[x[[3]]]*dt,
  x[[2]] + v*Sin[x[[3]]]*dt,
  wrap[x[[3]] + w*dt]
};

rolloutMinClear[x0_, v_, w_, obs_, R_, dt_, T_] := Module[
  {steps = Max[1, Round[T/dt]], x = x0, mc = Infinity},
  Do[
    x = stepUnicycle[x, v, w, dt];
    mc = Min[mc, clearance[x[[{1,2}]], obs, R]];
    If[mc <= 0, Break[]],
    {steps}
  ];
  {x, mc}
];

dwaCommand[x_, v0_, w0_, path_, obs_, prm_] := Module[
  {dt = prm["dt"], T = prm["T"], vmax = prm["vmax"], wmax = prm["wmax"],
   a = prm["a"], alpha = prm["alpha"], R = prm["R"], goal = Last[path],
   vmin, vmax2, wmin, wmax2, best = {-10^18, 0., 0.}, v, w, rr, xf, mc, goalDist, J},
  vmin = Max[0., v0 - a*dt]; vmax2 = Min[vmax, v0 + a*dt];
  wmin = Max[-wmax, w0 - alpha*dt]; wmax2 = Min[wmax, w0 + alpha*dt];
  Do[
    Do[
      rr = rolloutMinClear[x, v, w, obs, R, dt, T];
      xf = rr[[1]]; mc = rr[[2]];
      If[mc <= R + 0.05, Continue[]];
      goalDist = Norm[xf[[{1,2}]] - goal];
      J = -0.6*goalDist + 1.8*mc + 0.4*(v/(vmax + 10^-9)) - 0.12*w^2;
      If[J > best[[1]], best = {J, v, w}],
      {w, Subdivide[wmin, wmax2, 16]}
    ],
    {v, Subdivide[vmin, vmax2, 8]}
  ];
  best[[{2,3}]]
];

optimizeBand[x_, obs_, prm_, N_:12, bandLen_:2.6, iters_:20, step_:0.12] := Module[
  {R = prm["R"], pts, g, bestD, bestU, dvec, n, d, phi},
  pts = Table[{x[[1]] + bandLen*(i-1)/(N-1), corridorY[x[[1]] + bandLen*(i-1)/(N-1)]}, {i, 1, N}];
  pts[[1]] = x[[{1,2}]];
  Do[
    g = ConstantArray[{0., 0.}, N];
    (* smoothness *)
    Do[g[[i]] += 0.35*(2 pts[[i]] - pts[[i-1]] - pts[[i+1]]), {i, 2, N-1}];
    (* obstacle repulsion *)
    Do[
      {bestD, bestU} = {Infinity, {0., 0.}};
      Do[
        dvec = pts[[i]] - o[[{1,2}]];
        n = Norm[dvec] + 10^-12;
        d = n - o[[3]] - R;
        If[d < bestD, bestD = d; bestU = dvec/n],
        {o, obs}
      ];
      If[bestD < 0.9,
        phi = Exp[-4.5*(bestD - 0.9)];
        g[[i]] += 1.8*(-4.5*phi)*bestU
      ],
      {i, 2, N}
    ];
    (* time preference *)
    Do[g[[i]] += 0.25*(pts[[i]] - pts[[i-1]]), {i, 2, N}];
    Do[pts[[i]] -= step*g[[i]], {i, 2, N}];
    pts[[1]] = x[[{1,2}]],
    {iters}
  ];
  pts
];

tebCommand[x_, v0_, w0_, path_, obs_, prm_] := Module[
  {pts, d, heading, herr, w, v, vmax = prm["vmax"], wmax = prm["wmax"], a = prm["a"], alpha = prm["alpha"], dt = prm["dt"]},
  pts = optimizeBand[x, obs, prm];
  d = pts[[2]] - pts[[1]];
  heading = ArcTan[d[[1]], d[[2]]];
  herr = wrap[heading - x[[3]]];
  w = Clip[2.2*herr, {-wmax, wmax}];
  v = Clip[0.8*(Norm[d]/dt), {0., vmax}];
  v = v/(1 + 1.2*Abs[w]);
  v = Clip[v, {Max[0., v0 - a*dt], Min[vmax, v0 + a*dt]}];
  w = Clip[w, {w0 - alpha*dt, w0 + alpha*dt}];
  {v, w}
];

runOne[planner_, seed_, prm_] := Module[
  {obs = sampleDenseWorld[prm["n_obs"], seed], path = referencePath[], goal, x = {0.6, 0.0, 0.0},
   v = 0., w = 0., L = 0., cmin = Infinity, w2 = 0., c, u, x2, k},
  goal = Last[path];
  For[k = 0, k < prm["max_steps"], k++,
    c = clearance[x[[{1,2}]], obs, prm["R"]];
    cmin = Min[cmin, c];
    If[c <= 0, Return[<|"success"->False,"collision"->True,"time"->k*prm["dt"],"L"->L,"cmin"->cmin,"w2"->w2|>]];
    If[Norm[x[[{1,2}]] - goal] <= prm["goal_tol"], Return[<|"success"->True,"collision"->False,"time"->k*prm["dt"],"L"->L,"cmin"->cmin,"w2"->w2|>]];
    u = If[planner=="teb", tebCommand[x, v, w, path, obs, prm], dwaCommand[x, v, w, path, obs, prm]];
    x2 = stepUnicycle[x, u[[1]], u[[2]], prm["dt"]];
    L += Norm[x2[[{1,2}]] - x[[{1,2}]]];
    w2 += u[[2]]^2*prm["dt"];
    x = x2; v = u[[1]]; w = u[[2]];
  ];
  <|"success"->False,"collision"->False,"time"->prm["max_steps"]*prm["dt"],"L"->L,"cmin"->cmin,"w2"->w2|>
];

summarize[R_] := Module[
  {n = Length[R]},
  <|
    "trials"->n,
    "success_rate"->Mean[Boole[R[[All,"success"]]]],
    "collision_rate"->Mean[Boole[R[[All,"collision"]]]],
    "time_mean"->Mean[R[[All,"time"]]],
    "L_mean"->Mean[R[[All,"L"]]],
    "cmin_mean"->Mean[R[[All,"cmin"]]],
    "w2_mean"->Mean[R[[All,"w2"]]]
  |>
];

compare[trials_:30, seed0_:0] := Module[
  {prm = <|"R"->0.25,"vmax"->0.9,"wmax"->1.6,"a"->1.2,"alpha"->2.5,"dt"->0.1,"T"->2.0,
                 "goal_tol"->0.25,"max_steps"->800,"n_obs"->45|>, resDWA, resTEB},
  resDWA = Table[runOne["dwa", seed0+i, prm], {i, 0, trials-1}];
  resTEB = Table[runOne["teb", seed0+i, prm], {i, 0, trials-1}];
  Print["DWA ", summarize[resDWA]];
  Print["TEB ", summarize[resTEB]];
];

compare[30, 0];
      

Problem 1 (Distance is 1-Lipschitz): Let \( d(\mathbf{p})=\inf_{\mathbf{z}\in\mathcal{O}}\|\mathbf{p}-\mathbf{z}\| \) be the distance from a point to a nonempty closed set \( \mathcal{O} \subset \mathbb{R}^2 \). Prove that \( |d(\mathbf{p})-d(\mathbf{q})| \le \|\mathbf{p}-\mathbf{q}\| \).

Solution: Fix arbitrary \( \mathbf{p},\mathbf{q} \). For any \( \mathbf{z}\in\mathcal{O} \), the triangle inequality gives \( \|\mathbf{p}-\mathbf{z}\| \le \|\mathbf{p}-\mathbf{q}\|+\|\mathbf{q}-\mathbf{z}\| \). Taking infimum over \( \mathbf{z} \) yields \( d(\mathbf{p}) \le \|\mathbf{p}-\mathbf{q}\| + d(\mathbf{q}) \). Swap \( \mathbf{p} \) and \( \mathbf{q} \) to get \( d(\mathbf{q}) \le \|\mathbf{p}-\mathbf{q}\| + d(\mathbf{p}) \). Combining implies \( |d(\mathbf{p})-d(\mathbf{q})| \le \|\mathbf{p}-\mathbf{q}\| \).

Problem 2 (Discrete clearance bound): Suppose \( \left\|\frac{d\mathbf{p}(t)}{dt}\right\| \le v_{\max} \) and the clearance function \( c(\mathbf{p}) = d(\mathbf{p}) - R \) uses distance \( d \) to obstacles. Show that if \( c(\mathbf{p}(t_k)) \; > \; v_{\max}\Delta t \), then \( c(\mathbf{p}(t)) \; > \; 0 \) for all \( t \in [t_k, t_{k+1}] \).

Solution: Since the distance-to-obstacle function \( d(\cdot) \) is 1-Lipschitz, for any \( t \in [t_k, t_{k+1}] \) we have

\[ d(\mathbf{p}(t)) \; \ge \; d(\mathbf{p}(t_k)) - \|\mathbf{p}(t) - \mathbf{p}(t_k)\|. \]

Also, by the fundamental theorem of calculus and the speed bound,

\[ \|\mathbf{p}(t) - \mathbf{p}(t_k)\| \; \le \; \int_{t_k}^{t} \left\|\frac{d\mathbf{p}(\tau)}{d\tau}\right\| d\tau \; \le \; \int_{t_k}^{t} v_{\max}\, d\tau \; = \; v_{\max}(t - t_k) \; \le \; v_{\max}\Delta t. \]

Combining the two inequalities yields

\[ d(\mathbf{p}(t)) \; \ge \; d(\mathbf{p}(t_k)) - v_{\max}\Delta t. \]

Therefore, using \( c(\mathbf{p}) = d(\mathbf{p}) - R \),

\[ c(\mathbf{p}(t)) \; = \; d(\mathbf{p}(t)) - R \; \ge \; d(\mathbf{p}(t_k)) - v_{\max}\Delta t - R \; = \; c(\mathbf{p}(t_k)) - v_{\max}\Delta t \; > \; 0, \]

which proves that \( c(\mathbf{p}(t)) \; > \; 0 \) for all \( t \in [t_k, t_{k+1}] \).

Problem 3 (Dynamic window derivation): Assume discrete-time acceleration bounds \( |v_{k+1}-v_k| \le a_{\max}\Delta t \) and \( |\omega_{k+1}-\omega_k| \le \alpha_{\max}\Delta t \) plus saturation bounds \( 0 \le v \le v_{\max} \), \( |\omega| \le \omega_{\max} \). Derive the feasible intervals for \( v_{k+1} \) and \( \omega_{k+1} \).

Solution: From acceleration bounds: \( v_{k+1} \in [v_k-a_{\max}\Delta t,\; v_k+a_{\max}\Delta t] \), \( \omega_{k+1} \in [\omega_k-\alpha_{\max}\Delta t,\; \omega_k+\alpha_{\max}\Delta t] \). Intersect with saturation constraints to obtain:

\[ v_{k+1} \in \Big[\max(0, v_k-a_{\max}\Delta t),\; \min(v_{\max}, v_k+a_{\max}\Delta t)\Big], \\ \omega_{k+1} \in \Big[\max(-\omega_{\max}, \omega_k-\alpha_{\max}\Delta t),\; \min(\omega_{\max}, \omega_k+\alpha_{\max}\Delta t)\Big]. \]

You built a controlled benchmark to compare two mobile-specific local planning families in dense obstacles. With identical limits, maps, and termination criteria, you can attribute differences in success/collision rate, time, path length, and minimum clearance to the algorithmic family: velocity-space sampling (DWA-style) versus trajectory-band optimization (TEB-style).