The core task is: given a robot with planar pose \( \mathbf{x} = [x, y, \theta]^\top \), a goal position \( \mathbf{g} \in \mathbb{R}^2 \), and a set of moving humans modeled as discs, compute control inputs \( u(t) = [v(t), \omega(t)]^\top \) such that the robot reaches the goal while maintaining a required clearance and exhibiting socially acceptable motion (no aggressive cutting, minimal discomfort).

Robot model (unicycle). We assume a kinematic model (from Chapters 2–3):

\[ \dot x = v \cos\theta,\qquad \dot y = v \sin\theta,\qquad \dot\theta = \omega. \]

Human model (dynamic discs). Each human \( j \) has state \( (\mathbf{p}_j(t), \mathbf{v}_j(t)) \) and radius \( r_j \), with a robot radius \( r_r \). The safety distance for pair (robot, human j) is \( R_j = r_r + r_j \).

Experimental contract. Your implementation is considered correct if it satisfies:

• Safety: for all humans \( j \) and all times, \( \|\mathbf{p}_r(t) - \mathbf{p}_j(t)\| > R_j \).
• Goal reach: robot reaches \( \|\mathbf{p}_r - \mathbf{g}\| < \varepsilon_g \) within a time budget.
• Comfort: trajectories minimize a discomfort functional (Section 4).
• Reproducibility: you log trajectories and metrics for repeated seeds.

flowchart TD
  S["Scenario generator (crowd + goal)"] --> P["Perception (tracks + velocities)"]
  P --> PR["Prediction (short-horizon)"]
  PR --> L["Local planner (crowd-aware sampling)"]
  L --> U["Control u(t) = [v, w]"]
  U --> M["Robot motion model"]
  M --> F["Safety & comfort monitors"]
  F -->|log| E["Evaluation metrics + report"]
        

In this lab we use a conservative, short-horizon predictor suitable for a local planner: a constant-velocity model with optional process noise. For each human \( j \) at time \( t \):

\[ \mathbf{p}_j(t+\Delta t) = \mathbf{p}_j(t) + \mathbf{v}_j(t)\,\Delta t, \qquad \mathbf{v}_j(t+\Delta t) = \mathbf{v}_j(t). \]

Uncertainty growth (prediction-aware navigation). If the velocity estimate is noisy, model position as Gaussian: \( \mathbf{p}_j(t) \sim \mathcal{N}(\bar{\mathbf{p}}_j(t), \Sigma_j(t)) \) with \( \Sigma_j(t+\Delta t) = \Sigma_j(t) + Q\,\Delta t \). A standard chance constraint for collision avoidance over a single time step is:

\[ \mathbb{P}\big( \|\mathbf{p}_r - \mathbf{p}_j\| > R_j \big) \ge 1-\delta. \]

In practice we use a conservative deterministic surrogate by inflating the safety radius: \( R_j^{\text{eff}}(t) = R_j + k_\delta\,\sigma_d(t) \), where \( \sigma_d(t) \) is an approximate standard deviation of the relative distance and \( k_\delta \) is a normal quantile (or a tuned constant).

For a single human \( j \), define relative position and relative velocity: \( \mathbf{p}_{rel} = \mathbf{p}_r - \mathbf{p}_j \), \( \mathbf{v}_{rel} = \mathbf{v}_r - \mathbf{v}_j \). Under constant velocities, a collision occurs when: \( \|\mathbf{p}_{rel} + t\,\mathbf{v}_{rel}\| = R_j \) for some \( t \ge 0 \).

Time-to-collision (TTC). Squaring yields a quadratic in \( t \):

\[ \|\mathbf{v}_{rel}\|^2 t^2 + 2(\mathbf{p}_{rel}^\top \mathbf{v}_{rel}) t + (\|\mathbf{p}_{rel}\|^2 - R_j^2) = 0. \]

The smallest nonnegative root is the TTC (or \( +\infty \) if no real nonnegative root exists). This TTC is used in the lab as a dynamic safety margin: large TTC means “safe for now”; small TTC means imminent collision unless the control changes.

Velocity-obstacle (VO) statement (Lesson 2 connection). Define the VO set (for horizon \( T \)) as relative velocities that will cause a collision within time:

\[ \mathrm{VO}_j^T(\mathbf{p}_{rel}) = \left\{ \mathbf{v}_{rel} : \exists t \in [0,T],\; \|\mathbf{p}_{rel} + t\,\mathbf{v}_{rel}\| \le R_j \right\}. \]

Selecting \( \mathbf{v}_r \) such that \( \mathbf{v}_r - \mathbf{v}_j \notin \mathrm{VO}_j^T \) is equivalent to enforcing \( \mathrm{TTC}_j > T \) under constant velocity prediction.

The lab uses a practical local-planning design pattern: sample feasible controls over a short horizon, predict trajectories, enforce safety constraints, and minimize a multi-objective cost combining task progress and social comfort. This approach is compatible with the sampling mindset of Chapter 15 (DWA), extended to dynamic obstacles (Chapter 16).

Candidate controls. At each control cycle, sample \( u = [v, \omega]^\top \) within a dynamic window defined by acceleration limits: \( v \in [v_{min}, v_{max}] \), \( \omega \in [\omega_{min}, \omega_{max}] \).

Trajectory rollout. With step size \( \Delta t \) and horizon \( T = N\Delta t \), define predicted robot positions: \( \mathbf{p}_r^{(k)} \approx \mathbf{p}_r(t + k\Delta t) \).

Safety constraints. We use a strict collision rejection: \( \min_{j,k} \big( \|\mathbf{p}_r^{(k)} - \mathbf{p}_j^{(k)}\| - R_j \big) > 0 \), and a soft safety cost via TTC: \( J_{ttc}(u) = \sum_j \frac{1}{\mathrm{TTC}_j(u) + \epsilon} \).

Social discomfort field. Model each human as generating an anisotropic potential centered at their position. Let the human heading define a local frame (forward axis aligned with \( \mathbf{v}_j \)). For robot position \( \mathbf{p} \) expressed in that frame as \( (d_x, d_y) \):

\[ \phi_j(\mathbf{p}) = \exp\!\left( -\tfrac{1}{2}\left( \frac{d_x^2}{\sigma_x^2} + \frac{d_y^2}{\sigma_y^2} \right) \right), \qquad \sigma_x = \begin{cases} \sigma_{front} & \text{if } d_x \ge 0 \\ \sigma_{back} & \text{if } d_x < 0 \end{cases} \]

The discomfort cost along a predicted trajectory is approximated by a discrete sum: \( J_{soc}(u) = \frac{1}{N}\sum_{k=1}^N \sum_j \phi_j(\mathbf{p}_r^{(k)}) \).

Total cost. A typical crowd-aware sampling objective is:

\[ J(u) = w_g\,\|\mathbf{p}_r^{(N)} - \mathbf{g}\| + w_c\,\frac{1}{c_{min}(u)+\epsilon} + w_t\,\frac{1}{\mathrm{TTC}_{min}(u)+\epsilon} + \\ w_s\,J_{soc}(u) + w_v\,(v_{pref}-v)^2 + w_\omega\,\omega^2 + w_\Delta\,\|u-u_{prev}\|^2. \]

flowchart TD
  A["Sample (v,w) in dynamic window"] --> B["Roll out N steps"]
  B --> C["Predict humans (const-vel)"]
  C --> D["Compute clearance + TTC + discomfort"]
  D --> E["Reject if clearance le 0"]
  E --> F["Select argmin cost J(u)"]
  F --> G["Apply (v,w) for one dt"]
        

Why the hard rejection works (local guarantee). If humans follow the constant-velocity predictor over the horizon and a candidate control yields \( \|\mathbf{p}_r^{(k)} - \mathbf{p}_j^{(k)}\| > R_j \) for all \( k=1,\dots,N \) and all humans \( j \), then the discretized rollout is collision-free at all sampled instants. By choosing \( \Delta t \) small enough (Chapter 15 clearance reasoning), you can make this a tight approximation to continuous-time safety.

Run the planner across multiple crowd scenarios and random seeds. Your deliverable is a short report with plots and aggregated metrics.

5.1 Scenarios

• Crossing flows: two perpendicular human streams crossing near the origin.
• Corridor bidirectional: humans move in opposite directions through a narrow corridor.
• Goal behind crowd: robot must pass through a cluster that drifts slowly.

5.2 Metrics

Log robot and human trajectories and compute:

• Success rate (reach goal without collision).
• Minimum clearance: \( c_{min} = \min_{t,j} \|\mathbf{p}_r(t)-\mathbf{p}_j(t)\| - R_j \).
• Minimum TTC over time: \( \mathrm{TTC}_{min} = \min_{t,j} \mathrm{TTC}_j(t) \).
• Path efficiency (time, path length).
• Smoothness (finite differences in commanded \( v,\omega \)).
• Discomfort integral: \( \sum_k \sum_j \phi_j(\mathbf{p}_r^{(k)}) \).

5.3 Parameter sweep

Perform a controlled sweep over \( w_s \) (social weight) and \( w_t \) (TTC weight), and report the Pareto-like tradeoff between speed and comfort.

The following implementations provide a consistent baseline across languages. They implement the same crowd scenario (crossing flows) and the same crowd-aware sampling planner. You are expected to modify parameters, add scenarios, and compute metrics.

6.1 Python

Chapter16_Lesson5.py

import math
import random
from dataclasses import dataclass
from typing import List, Tuple, Dict

# ============================
# Chapter16_Lesson5.py
# Lab: Navigate Through Moving Crowds (baseline multi-objective sampler)
# ============================

@dataclass
class UnicycleState:
    x: float
    y: float
    theta: float

@dataclass
class Human:
    x: float
    y: float
    vx: float
    vy: float
    r: float

@dataclass
class Params:
    dt: float = 0.1
    horizon: float = 2.0
    robot_radius: float = 0.30
    # dynamic window bounds (kinematic; can be extended to include accel constraints)
    v_min: float = 0.0
    v_max: float = 1.2
    w_min: float = -1.5
    w_max: float = 1.5
    # sampling
    n_v: int = 11
    n_w: int = 21
    # costs
    w_goal: float = 1.0
    w_clear: float = 0.8
    w_ttc: float = 0.6
    w_social: float = 0.7
    w_speed: float = 0.15
    w_omega: float = 0.05
    w_delta_u: float = 0.1
    v_pref: float = 0.8
    eps: float = 1e-6
    # social field parameters (anisotropic)
    sigma_front: float = 1.2
    sigma_back: float = 0.6
    sigma_y: float = 0.8

def wrap_angle(a: float) -> float:
    while a > math.pi:
        a -= 2.0 * math.pi
    while a < -math.pi:
        a += 2.0 * math.pi
    return a

def step_unicycle(s: UnicycleState, v: float, w: float, dt: float) -> UnicycleState:
    # forward Euler
    nx = s.x + v * math.cos(s.theta) * dt
    ny = s.y + v * math.sin(s.theta) * dt
    nth = wrap_angle(s.theta + w * dt)
    return UnicycleState(nx, ny, nth)

def predict_humans_const_vel(humans: List[Human], dt: float) -> List[Human]:
    nxt = []
    for h in humans:
        nxt.append(Human(h.x + h.vx * dt, h.y + h.vy * dt, h.vx, h.vy, h.r))
    return nxt

def dist(a: Tuple[float,float], b: Tuple[float,float]) -> float:
    return math.hypot(a[0]-b[0], a[1]-b[1])

def ttc_disc_rel(prel: Tuple[float,float], vrel: Tuple[float,float], R: float, eps: float=1e-9) -> float:
    # Solve ||p + t v||^2 = R^2 for smallest t >= 0
    px, py = prel
    vx, vy = vrel
    A = vx*vx + vy*vy
    B = 2.0*(px*vx + py*vy)
    C = px*px + py*py - R*R
    if A < eps:
        # relative speed ~ 0: either already colliding or never colliding
        return 0.0 if C <= 0.0 else float("inf")
    disc = B*B - 4.0*A*C
    if disc < 0.0:
        return float("inf")
    sqrt_disc = math.sqrt(disc)
    t1 = (-B - sqrt_disc) / (2.0*A)
    t2 = (-B + sqrt_disc) / (2.0*A)
    ts = [t for t in (t1, t2) if t >= 0.0]
    return min(ts) if ts else float("inf")

def human_frame_components(px: float, py: float, hvx: float, hvy: float) -> Tuple[float,float]:
    # define human forward axis aligned with velocity; if near zero, use x-axis
    sp = math.hypot(hvx, hvy)
    if sp < 1e-9:
        fx, fy = 1.0, 0.0
    else:
        fx, fy = hvx/sp, hvy/sp
    # lateral axis (rotate +90)
    lx, ly = -fy, fx
    dx = px*fx + py*fy
    dy = px*lx + py*ly
    return dx, dy

def social_phi(robot_pos: Tuple[float,float], h: Human, P: Params) -> float:
    # anisotropic gaussian-like discomfort around human
    rx, ry = robot_pos
    relx = rx - h.x
    rely = ry - h.y
    dx, dy = human_frame_components(relx, rely, h.vx, h.vy)
    sigma_x = P.sigma_front if dx >= 0.0 else P.sigma_back
    # potential in [0,1]
    val = math.exp(-0.5*((dx*dx)/(sigma_x*sigma_x) + (dy*dy)/(P.sigma_y*P.sigma_y)))
    return val

def rollout_and_score(
    s0: UnicycleState,
    humans0: List[Human],
    goal: Tuple[float,float],
    u: Tuple[float,float],
    u_prev: Tuple[float,float],
    P: Params
) -> Tuple[float, Dict[str,float]]:
    v, w = u
    dt = P.dt
    N = max(1, int(P.horizon / dt))
    s = s0
    humans = humans0

    min_clear = float("inf")
    min_ttc = float("inf")
    soc_sum = 0.0

    for k in range(1, N+1):
        # propagate robot
        s = step_unicycle(s, v, w, dt)
        # propagate humans
        humans = predict_humans_const_vel(humans, dt)

        pr = (s.x, s.y)
        vr = (v*math.cos(s.theta), v*math.sin(s.theta))

        # evaluate against each human
        for h in humans:
            ph = (h.x, h.y)
            R = P.robot_radius + h.r
            d = dist(pr, ph)
            clear = d - R
            if clear < min_clear:
                min_clear = clear

            # TTC w.r.t. human (in world frame)
            prel = (pr[0]-ph[0], pr[1]-ph[1])
            vrel = (vr[0]-h.vx, vr[1]-h.vy)
            ttc = ttc_disc_rel(prel, vrel, R)
            if ttc < min_ttc:
                min_ttc = ttc

            # social potential
            soc_sum += social_phi(pr, h, P)

    # hard rejection handled outside; score assumes min_clear > 0
    goal_dist = dist((s.x, s.y), goal)
    # soft penalties
    J_clear = 1.0 / (min_clear + P.eps)
    J_ttc = 1.0 / (min_ttc + P.eps)
    J_soc = soc_sum / float(N)

    J = (
        P.w_goal*goal_dist
        + P.w_clear*J_clear
        + P.w_ttc*J_ttc
        + P.w_social*J_soc
        + P.w_speed*((P.v_pref - v)**2)
        + P.w_omega*(w*w)
        + P.w_delta_u*((v-u_prev[0])**2 + (w-u_prev[1])**2)
    )

    info = {
        "goal_dist": goal_dist,
        "min_clear": min_clear,
        "min_ttc": min_ttc,
        "J_soc": J_soc,
        "J_total": J
    }
    return J, info

def sample_controls(P: Params) -> List[Tuple[float,float]]:
    controls = []
    if P.n_v <= 1:
        v_list = [0.5*(P.v_min+P.v_max)]
    else:
        v_list = [P.v_min + i*(P.v_max-P.v_min)/(P.n_v-1) for i in range(P.n_v)]
    if P.n_w <= 1:
        w_list = [0.0]
    else:
        w_list = [P.w_min + j*(P.w_max-P.w_min)/(P.n_w-1) for j in range(P.n_w)]
    for v in v_list:
        for w in w_list:
            controls.append((v,w))
    return controls

def make_crossing_flow_scene(seed: int = 0) -> Tuple[UnicycleState, Tuple[float,float], List[Human]]:
    random.seed(seed)
    # robot start and goal
    s0 = UnicycleState(-4.0, -4.0, math.radians(45.0))
    goal = (4.0, 4.0)

    humans: List[Human] = []
    # flow 1: left to right near y=0
    for i in range(10):
        y = random.uniform(-1.2, 1.2)
        x = random.uniform(-4.0, 4.0)
        vx = random.uniform(0.4, 0.9)
        humans.append(Human(x, y, vx, 0.0, 0.30))

    # flow 2: bottom to top near x=0
    for i in range(10):
        x = random.uniform(-1.2, 1.2)
        y = random.uniform(-4.0, 4.0)
        vy = random.uniform(0.4, 0.9)
        humans.append(Human(x, y, 0.0, vy, 0.30))

    return s0, goal, humans

def run_episode(seed: int = 0, P: Params = Params()) -> Dict[str, float]:
    s, goal, humans = make_crossing_flow_scene(seed)
    u_prev = (0.0, 0.0)

    T_total = 35.0
    steps = int(T_total / P.dt)
    min_clear_all = float("inf")
    min_ttc_all = float("inf")
    soc_integral = 0.0

    for t in range(steps):
        # stop if reached
        if dist((s.x, s.y), goal) < 0.35:
            return {
                "success": 1.0,
                "time": t*P.dt,
                "min_clear": min_clear_all,
                "min_ttc": min_ttc_all,
                "social": soc_integral
            }

        best_u = None
        best_J = float("inf")
        best_info = None

        # evaluate all controls
        for u in sample_controls(P):
            J, info = rollout_and_score(s, humans, goal, u, u_prev, P)
            # hard safety rejection
            if info["min_clear"] <= 0.0:
                continue
            if J < best_J:
                best_J = J
                best_u = u
                best_info = info

        # if no safe control found, freeze (or rotate in place as a recovery)
        if best_u is None:
            v, w = 0.0, 0.0
            best_info = {"min_clear": -1.0, "min_ttc": 0.0, "J_soc": 0.0}
        else:
            v, w = best_u

        # apply for one step
        s = step_unicycle(s, v, w, P.dt)
        humans = predict_humans_const_vel(humans, P.dt)
        u_prev = (v, w)

        # accumulate episode metrics
        min_clear_all = min(min_clear_all, best_info["min_clear"])
        min_ttc_all = min(min_ttc_all, best_info["min_ttc"])
        soc_integral += best_info.get("J_soc", 0.0)

        # hard collision check (simulation truth under the same human model)
        for h in humans:
            if dist((s.x, s.y), (h.x, h.y)) <= P.robot_radius + h.r:
                return {
                    "success": 0.0,
                    "time": t*P.dt,
                    "min_clear": min_clear_all,
                    "min_ttc": min_ttc_all,
                    "social": soc_integral
                }

    return {
        "success": 0.0,
        "time": T_total,
        "min_clear": min_clear_all,
        "min_ttc": min_ttc_all,
        "social": soc_integral
    }

def sweep_weights(seeds: List[int], w_social_list: List[float], w_ttc_list: List[float]) -> List[Dict[str, float]]:
    P = Params()
    results = []
    for ws in w_social_list:
        for wt in w_ttc_list:
            P.w_social = ws
            P.w_ttc = wt
            succ = 0.0
            times = []
            clears = []
            ttcs = []
            socs = []
            for sd in seeds:
                out = run_episode(sd, P)
                succ += out["success"]
                times.append(out["time"])
                clears.append(out["min_clear"])
                ttcs.append(out["min_ttc"])
                socs.append(out["social"])
            n = float(len(seeds))
            results.append({
                "w_social": ws,
                "w_ttc": wt,
                "success_rate": succ/n,
                "time_avg": sum(times)/n,
                "min_clear_avg": sum(clears)/n,
                "min_ttc_avg": sum(ttcs)/n,
                "social_avg": sum(socs)/n
            })
    return results

if __name__ == "__main__":
    # quick demo
    P = Params()
    out = run_episode(seed=0, P=P)
    print("Episode metrics:", out)

    # small sweep
    seeds = list(range(5))
    res = sweep_weights(seeds, w_social_list=[0.2, 0.7, 1.2], w_ttc_list=[0.2, 0.6, 1.0])
    for r in res:
        print(r)
      

6.2 C++

Chapter16_Lesson5.cpp

#include <cmath>
#include <iostream>
#include <limits>
#include <random>
#include <tuple>
#include <vector>

// ============================
// Chapter16_Lesson5.cpp
// Lab: Navigate Through Moving Crowds (baseline multi-objective sampler)
// ============================

struct UnicycleState {
  double x{0.0}, y{0.0}, theta{0.0};
};

struct Human {
  double x{0.0}, y{0.0}, vx{0.0}, vy{0.0}, r{0.30};
};

struct Params {
  double dt = 0.1;
  double horizon = 2.0;
  double robot_radius = 0.30;

  double v_min = 0.0, v_max = 1.2;
  double w_min = -1.5, w_max = 1.5;

  int n_v = 11;
  int n_w = 21;

  double w_goal = 1.0;
  double w_clear = 0.8;
  double w_ttc = 0.6;
  double w_social = 0.7;
  double w_speed = 0.15;
  double w_omega = 0.05;
  double w_delta_u = 0.1;
  double v_pref = 0.8;
  double eps = 1e-6;

  double sigma_front = 1.2;
  double sigma_back = 0.6;
  double sigma_y = 0.8;
};

static double wrap_angle(double a) {
  while (a > M_PI) a -= 2.0 * M_PI;
  while (a < -M_PI) a += 2.0 * M_PI;
  return a;
}

static UnicycleState step_unicycle(const UnicycleState& s, double v, double w, double dt) {
  UnicycleState ns;
  ns.x = s.x + v * std::cos(s.theta) * dt;
  ns.y = s.y + v * std::sin(s.theta) * dt;
  ns.theta = wrap_angle(s.theta + w * dt);
  return ns;
}

static std::vector<Human> predict_humans_const_vel(const std::vector<Human>& humans, double dt) {
  std::vector<Human> nxt;
  nxt.reserve(humans.size());
  for (const auto& h : humans) {
    nxt.push_back(Human{h.x + h.vx * dt, h.y + h.vy * dt, h.vx, h.vy, h.r});
  }
  return nxt;
}

static double dist2(double ax, double ay, double bx, double by) {
  double dx = ax - bx, dy = ay - by;
  return dx * dx + dy * dy;
}

static double dist(double ax, double ay, double bx, double by) {
  return std::sqrt(dist2(ax, ay, bx, by));
}

static double ttc_disc_rel(double px, double py, double vx, double vy, double R) {
  // Solve ||p + t v||^2 = R^2 for smallest t >= 0
  double A = vx * vx + vy * vy;
  double B = 2.0 * (px * vx + py * vy);
  double C = px * px + py * py - R * R;

  const double eps = 1e-12;
  if (A < eps) {
    return (C <= 0.0) ? 0.0 : std::numeric_limits<double>::infinity();
  }

  double disc = B * B - 4.0 * A * C;
  if (disc < 0.0) return std::numeric_limits<double>::infinity();

  double sd = std::sqrt(disc);
  double t1 = (-B - sd) / (2.0 * A);
  double t2 = (-B + sd) / (2.0 * A);

  double best = std::numeric_limits<double>::infinity();
  if (t1 >= 0.0) best = std::min(best, t1);
  if (t2 >= 0.0) best = std::min(best, t2);
  return best;
}

static std::pair<double, double> human_frame_components(double relx, double rely, double hvx, double hvy) {
  double sp = std::hypot(hvx, hvy);
  double fx = 1.0, fy = 0.0;
  if (sp > 1e-12) {
    fx = hvx / sp;
    fy = hvy / sp;
  }
  // lateral axis = rotate +90
  double lx = -fy, ly = fx;
  double dx = relx * fx + rely * fy;
  double dy = relx * lx + rely * ly;
  return {dx, dy};
}

static double social_phi(double rx, double ry, const Human& h, const Params& P) {
  double relx = rx - h.x;
  double rely = ry - h.y;
  auto [dx, dy] = human_frame_components(relx, rely, h.vx, h.vy);
  double sigma_x = (dx >= 0.0) ? P.sigma_front : P.sigma_back;
  double val = std::exp(-0.5 * ((dx * dx) / (sigma_x * sigma_x) + (dy * dy) / (P.sigma_y * P.sigma_y)));
  return val;
}

struct RolloutInfo {
  double goal_dist{0.0};
  double min_clear{0.0};
  double min_ttc{0.0};
  double J_soc{0.0};
  double J_total{0.0};
};

static std::pair<double, RolloutInfo> rollout_and_score(
  const UnicycleState& s0,
  const std::vector<Human>& humans0,
  std::pair<double, double> goal,
  std::pair<double, double> u,
  std::pair<double, double> u_prev,
  const Params& P
) {
  double v = u.first, w = u.second;
  int N = std::max(1, static_cast<int>(P.horizon / P.dt));

  UnicycleState s = s0;
  std::vector<Human> humans = humans0;

  double min_clear = std::numeric_limits<double>::infinity();
  double min_ttc = std::numeric_limits<double>::infinity();
  double soc_sum = 0.0;

  for (int k = 1; k <= N; ++k) {
    s = step_unicycle(s, v, w, P.dt);
    humans = predict_humans_const_vel(humans, P.dt);

    double vrx = v * std::cos(s.theta);
    double vry = v * std::sin(s.theta);

    for (const auto& h : humans) {
      double R = P.robot_radius + h.r;
      double d = dist(s.x, s.y, h.x, h.y);
      double clear = d - R;
      min_clear = std::min(min_clear, clear);

      // TTC
      double prel_x = s.x - h.x;
      double prel_y = s.y - h.y;
      double vrel_x = vrx - h.vx;
      double vrel_y = vry - h.vy;
      double ttc = ttc_disc_rel(prel_x, prel_y, vrel_x, vrel_y, R);
      min_ttc = std::min(min_ttc, ttc);

      soc_sum += social_phi(s.x, s.y, h, P);
    }
  }

  double goal_dist = dist(s.x, s.y, goal.first, goal.second);
  double J_clear = 1.0 / (min_clear + P.eps);
  double J_ttc = 1.0 / (min_ttc + P.eps);
  double J_soc = soc_sum / static_cast<double>(N);

  double J =
    P.w_goal * goal_dist +
    P.w_clear * J_clear +
    P.w_ttc * J_ttc +
    P.w_social * J_soc +
    P.w_speed * std::pow(P.v_pref - v, 2) +
    P.w_omega * (w * w) +
    P.w_delta_u * (std::pow(v - u_prev.first, 2) + std::pow(w - u_prev.second, 2));

  RolloutInfo info;
  info.goal_dist = goal_dist;
  info.min_clear = min_clear;
  info.min_ttc = min_ttc;
  info.J_soc = J_soc;
  info.J_total = J;

  return {J, info};
}

static std::vector<std::pair<double, double>> sample_controls(const Params& P) {
  std::vector<std::pair<double, double>> controls;
  std::vector<double> v_list, w_list;

  if (P.n_v <= 1) v_list = {0.5 * (P.v_min + P.v_max)};
  else {
    v_list.reserve(P.n_v);
    for (int i = 0; i < P.n_v; ++i)
      v_list.push_back(P.v_min + i * (P.v_max - P.v_min) / (P.n_v - 1));
  }

  if (P.n_w <= 1) w_list = {0.0};
  else {
    w_list.reserve(P.n_w);
    for (int j = 0; j < P.n_w; ++j)
      w_list.push_back(P.w_min + j * (P.w_max - P.w_min) / (P.n_w - 1));
  }

  controls.reserve(v_list.size() * w_list.size());
  for (double v : v_list) for (double w : w_list) controls.push_back({v, w});
  return controls;
}

static std::tuple<UnicycleState, std::pair<double, double>, std::vector<Human>>
make_crossing_flow_scene(int seed) {
  std::mt19937 rng(seed);
  auto unif = [&](double a, double b) {
    std::uniform_real_distribution<double> d(a, b);
    return d(rng);
  };

  UnicycleState s0{-4.0, -4.0, 45.0 * M_PI / 180.0};
  std::pair<double, double> goal{4.0, 4.0};

  std::vector<Human> humans;
  humans.reserve(20);

  for (int i = 0; i < 10; ++i) {
    double y = unif(-1.2, 1.2);
    double x = unif(-4.0, 4.0);
    double vx = unif(0.4, 0.9);
    humans.push_back(Human{x, y, vx, 0.0, 0.30});
  }

  for (int i = 0; i < 10; ++i) {
    double x = unif(-1.2, 1.2);
    double y = unif(-4.0, 4.0);
    double vy = unif(0.4, 0.9);
    humans.push_back(Human{x, y, 0.0, vy, 0.30});
  }

  return {s0, goal, humans};
}

struct EpisodeOut {
  double success{0.0};
  double time{0.0};
  double min_clear{0.0};
  double min_ttc{0.0};
  double social{0.0};
};

static EpisodeOut run_episode(int seed, const Params& P) {
  auto [s0, goal, humans0] = make_crossing_flow_scene(seed);
  UnicycleState s = s0;
  std::pair<double, double> g = goal;
  std::vector<Human> humans = humans0;

  std::pair<double, double> u_prev{0.0, 0.0};

  double T_total = 35.0;
  int steps = static_cast<int>(T_total / P.dt);

  double min_clear_all = std::numeric_limits<double>::infinity();
  double min_ttc_all = std::numeric_limits<double>::infinity();
  double soc_integral = 0.0;

  for (int t = 0; t < steps; ++t) {
    if (dist(s.x, s.y, g.first, g.second) < 0.35) {
      return EpisodeOut{1.0, t * P.dt, min_clear_all, min_ttc_all, soc_integral};
    }

    std::pair<double, double> best_u{0.0, 0.0};
    double best_J = std::numeric_limits<double>::infinity();
    RolloutInfo best_info;
    bool found = false;

    for (auto u : sample_controls(P)) {
      auto [J, info] = rollout_and_score(s, humans, g, u, u_prev, P);
      if (info.min_clear <= 0.0) continue; // hard reject
      if (J < best_J) {
        best_J = J;
        best_u = u;
        best_info = info;
        found = true;
      }
    }

    double v = 0.0, w = 0.0;
    if (!found) {
      // freeze (or implement recovery rotation)
      v = 0.0; w = 0.0;
      best_info.min_clear = -1.0;
      best_info.min_ttc = 0.0;
      best_info.J_soc = 0.0;
    } else {
      v = best_u.first; w = best_u.second;
    }

    s = step_unicycle(s, v, w, P.dt);
    humans = predict_humans_const_vel(humans, P.dt);
    u_prev = {v, w};

    min_clear_all = std::min(min_clear_all, best_info.min_clear);
    min_ttc_all = std::min(min_ttc_all, best_info.min_ttc);
    soc_integral += best_info.J_soc;

    for (const auto& h : humans) {
      if (dist(s.x, s.y, h.x, h.y) <= P.robot_radius + h.r) {
        return EpisodeOut{0.0, t * P.dt, min_clear_all, min_ttc_all, soc_integral};
      }
    }
  }

  return EpisodeOut{0.0, T_total, min_clear_all, min_ttc_all, soc_integral};
}

int main() {
  Params P;
  EpisodeOut out = run_episode(0, P);
  std::cout << "Episode metrics: "
            << "{success=" << out.success
            << ", time=" << out.time
            << ", min_clear=" << out.min_clear
            << ", min_ttc=" << out.min_ttc
            << ", social=" << out.social << "}" << std::endl;

  // tiny sweep example
  std::vector<double> wS = {0.2, 0.7, 1.2};
  std::vector<double> wT = {0.2, 0.6, 1.0};
  for (double ws : wS) {
    for (double wt : wT) {
      Params Q = P;
      Q.w_social = ws;
      Q.w_ttc = wt;
      double succ = 0.0;
      double time_avg = 0.0;
      double clr_avg = 0.0;
      double ttc_avg = 0.0;
      double soc_avg = 0.0;
      int N = 5;
      for (int sd = 0; sd < N; ++sd) {
        EpisodeOut e = run_episode(sd, Q);
        succ += e.success;
        time_avg += e.time;
        clr_avg += e.min_clear;
        ttc_avg += e.min_ttc;
        soc_avg += e.social;
      }
      std::cout << "w_social=" << ws << ", w_ttc=" << wt
                << ", success_rate=" << (succ / N)
                << ", time_avg=" << (time_avg / N)
                << ", min_clear_avg=" << (clr_avg / N)
                << ", min_ttc_avg=" << (ttc_avg / N)
                << ", social_avg=" << (soc_avg / N)
                << std::endl;
    }
  }
  return 0;
}
      

6.3 Java

Chapter16_Lesson5.java

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

// ============================
// Chapter16_Lesson5.java
// Lab: Navigate Through Moving Crowds (baseline multi-objective sampler)
// ============================

public class Chapter16_Lesson5 {

  static class UnicycleState {
    double x, y, theta;
    UnicycleState(double x, double y, double theta) { this.x = x; this.y = y; this.theta = theta; }
  }

  static class Human {
    double x, y, vx, vy, r;
    Human(double x, double y, double vx, double vy, double r) { this.x = x; this.y = y; this.vx = vx; this.vy = vy; this.r = r; }
  }

  static class Params {
    double dt = 0.1;
    double horizon = 2.0;
    double robotRadius = 0.30;

    double vMin = 0.0, vMax = 1.2;
    double wMin = -1.5, wMax = 1.5;

    int nV = 11;
    int nW = 21;

    double wGoal = 1.0;
    double wClear = 0.8;
    double wTtc = 0.6;
    double wSocial = 0.7;
    double wSpeed = 0.15;
    double wOmega = 0.05;
    double wDeltaU = 0.1;
    double vPref = 0.8;
    double eps = 1e-6;

    double sigmaFront = 1.2;
    double sigmaBack = 0.6;
    double sigmaY = 0.8;
  }

  static double wrapAngle(double a) {
    while (a > Math.PI) a -= 2.0 * Math.PI;
    while (a < -Math.PI) a += 2.0 * Math.PI;
    return a;
  }

  static UnicycleState stepUnicycle(UnicycleState s, double v, double w, double dt) {
    double nx = s.x + v * Math.cos(s.theta) * dt;
    double ny = s.y + v * Math.sin(s.theta) * dt;
    double nth = wrapAngle(s.theta + w * dt);
    return new UnicycleState(nx, ny, nth);
  }

  static List<Human> predictHumansConstVel(List<Human> humans, double dt) {
    List<Human> nxt = new ArrayList<>(humans.size());
    for (Human h : humans) {
      nxt.add(new Human(h.x + h.vx * dt, h.y + h.vy * dt, h.vx, h.vy, h.r));
    }
    return nxt;
  }

  static double dist(double ax, double ay, double bx, double by) {
    double dx = ax - bx, dy = ay - by;
    return Math.hypot(dx, dy);
  }

  static double ttcDiscRel(double px, double py, double vx, double vy, double R) {
    // Solve ||p + t v||^2 = R^2 for smallest t >= 0
    double A = vx * vx + vy * vy;
    double B = 2.0 * (px * vx + py * vy);
    double C = px * px + py * py - R * R;

    double eps = 1e-12;
    if (A < eps) {
      return (C <= 0.0) ? 0.0 : Double.POSITIVE_INFINITY;
    }

    double disc = B * B - 4.0 * A * C;
    if (disc < 0.0) return Double.POSITIVE_INFINITY;

    double sd = Math.sqrt(disc);
    double t1 = (-B - sd) / (2.0 * A);
    double t2 = (-B + sd) / (2.0 * A);

    double best = Double.POSITIVE_INFINITY;
    if (t1 >= 0.0) best = Math.min(best, t1);
    if (t2 >= 0.0) best = Math.min(best, t2);
    return best;
  }

  static double[] humanFrameComponents(double relx, double rely, double hvx, double hvy) {
    double sp = Math.hypot(hvx, hvy);
    double fx = 1.0, fy = 0.0;
    if (sp > 1e-12) {
      fx = hvx / sp;
      fy = hvy / sp;
    }
    double lx = -fy, ly = fx;
    double dx = relx * fx + rely * fy;
    double dy = relx * lx + rely * ly;
    return new double[]{dx, dy};
  }

  static double socialPhi(double rx, double ry, Human h, Params P) {
    double relx = rx - h.x;
    double rely = ry - h.y;
    double[] comps = humanFrameComponents(relx, rely, h.vx, h.vy);
    double dx = comps[0], dy = comps[1];
    double sigmaX = (dx >= 0.0) ? P.sigmaFront : P.sigmaBack;
    return Math.exp(-0.5 * ((dx * dx) / (sigmaX * sigmaX) + (dy * dy) / (P.sigmaY * P.sigmaY)));
  }

  static class RolloutInfo {
    double goalDist;
    double minClear;
    double minTtc;
    double jSoc;
    double jTotal;
  }

  static class Score {
    double J;
    RolloutInfo info;
    Score(double J, RolloutInfo info) { this.J = J; this.info = info; }
  }

  static Score rolloutAndScore(UnicycleState s0, List<Human> humans0, double gx, double gy,
                               double v, double w, double vPrev, double wPrev, Params P) {
    int N = Math.max(1, (int)Math.floor(P.horizon / P.dt));
    UnicycleState s = s0;
    List<Human> humans = humans0;

    double minClear = Double.POSITIVE_INFINITY;
    double minTtc = Double.POSITIVE_INFINITY;
    double socSum = 0.0;

    for (int k = 1; k <= N; k++) {
      s = stepUnicycle(s, v, w, P.dt);
      humans = predictHumansConstVel(humans, P.dt);

      double vrx = v * Math.cos(s.theta);
      double vry = v * Math.sin(s.theta);

      for (Human h : humans) {
        double R = P.robotRadius + h.r;
        double d = dist(s.x, s.y, h.x, h.y);
        double clear = d - R;
        if (clear < minClear) minClear = clear;

        double prelX = s.x - h.x;
        double prelY = s.y - h.y;
        double vrelX = vrx - h.vx;
        double vrelY = vry - h.vy;
        double ttc = ttcDiscRel(prelX, prelY, vrelX, vrelY, R);
        if (ttc < minTtc) minTtc = ttc;

        socSum += socialPhi(s.x, s.y, h, P);
      }
    }

    double goalDist = dist(s.x, s.y, gx, gy);
    double jClear = 1.0 / (minClear + P.eps);
    double jTtc = 1.0 / (minTtc + P.eps);
    double jSoc = socSum / (double)N;

    double J =
      P.wGoal * goalDist +
      P.wClear * jClear +
      P.wTtc * jTtc +
      P.wSocial * jSoc +
      P.wSpeed * (P.vPref - v) * (P.vPref - v) +
      P.wOmega * (w * w) +
      P.wDeltaU * ((v - vPrev) * (v - vPrev) + (w - wPrev) * (w - wPrev));

    RolloutInfo info = new RolloutInfo();
    info.goalDist = goalDist;
    info.minClear = minClear;
    info.minTtc = minTtc;
    info.jSoc = jSoc;
    info.jTotal = J;

    return new Score(J, info);
  }

  static List<double[]> sampleControls(Params P) {
    List<double[]> controls = new ArrayList<>();
    double[] vList;
    double[] wList;

    if (P.nV <= 1) vList = new double[]{0.5 * (P.vMin + P.vMax)};
    else {
      vList = new double[P.nV];
      for (int i = 0; i < P.nV; i++) vList[i] = P.vMin + i * (P.vMax - P.vMin) / (P.nV - 1);
    }

    if (P.nW <= 1) wList = new double[]{0.0};
    else {
      wList = new double[P.nW];
      for (int j = 0; j < P.nW; j++) wList[j] = P.wMin + j * (P.wMax - P.wMin) / (P.nW - 1);
    }

    for (double v : vList) for (double w : wList) controls.add(new double[]{v, w});
    return controls;
  }

  static class Scene {
    UnicycleState s0;
    double gx, gy;
    List<Human> humans;
  }

  static Scene makeCrossingFlowScene(int seed) {
    Random rng = new Random(seed);
    Scene sc = new Scene();

    sc.s0 = new UnicycleState(-4.0, -4.0, Math.toRadians(45.0));
    sc.gx = 4.0; sc.gy = 4.0;

    sc.humans = new ArrayList<>();
    // flow 1: left to right near y=0
    for (int i = 0; i < 10; i++) {
      double y = -1.2 + 2.4 * rng.nextDouble();
      double x = -4.0 + 8.0 * rng.nextDouble();
      double vx = 0.4 + 0.5 * rng.nextDouble();
      sc.humans.add(new Human(x, y, vx, 0.0, 0.30));
    }
    // flow 2: bottom to top near x=0
    for (int i = 0; i < 10; i++) {
      double x = -1.2 + 2.4 * rng.nextDouble();
      double y = -4.0 + 8.0 * rng.nextDouble();
      double vy = 0.4 + 0.5 * rng.nextDouble();
      sc.humans.add(new Human(x, y, 0.0, vy, 0.30));
    }
    return sc;
  }

  static class EpisodeOut {
    double success;
    double time;
    double minClear;
    double minTtc;
    double social;
    EpisodeOut(double success, double time, double minClear, double minTtc, double social) {
      this.success = success; this.time = time; this.minClear = minClear; this.minTtc = minTtc; this.social = social;
    }
  }

  static EpisodeOut runEpisode(int seed, Params P) {
    Scene sc = makeCrossingFlowScene(seed);
    UnicycleState s = sc.s0;
    double gx = sc.gx, gy = sc.gy;
    List<Human> humans = sc.humans;

    double vPrev = 0.0, wPrev = 0.0;

    double Ttotal = 35.0;
    int steps = (int)Math.floor(Ttotal / P.dt);

    double minClearAll = Double.POSITIVE_INFINITY;
    double minTtcAll = Double.POSITIVE_INFINITY;
    double socIntegral = 0.0;

    for (int t = 0; t < steps; t++) {
      if (dist(s.x, s.y, gx, gy) < 0.35) {
        return new EpisodeOut(1.0, t * P.dt, minClearAll, minTtcAll, socIntegral);
      }

      double bestV = 0.0, bestW = 0.0;
      double bestJ = Double.POSITIVE_INFINITY;
      RolloutInfo bestInfo = new RolloutInfo();
      boolean found = false;

      for (double[] u : sampleControls(P)) {
        double v = u[0], w = u[1];
        Score scScore = rolloutAndScore(s, humans, gx, gy, v, w, vPrev, wPrev, P);
        if (scScore.info.minClear <= 0.0) continue;
        if (scScore.J < bestJ) {
          bestJ = scScore.J;
          bestV = v; bestW = w;
          bestInfo = scScore.info;
          found = true;
        }
      }

      double vCmd = 0.0, wCmd = 0.0;
      if (!found) {
        vCmd = 0.0; wCmd = 0.0;
        bestInfo.minClear = -1.0;
        bestInfo.minTtc = 0.0;
        bestInfo.jSoc = 0.0;
      } else {
        vCmd = bestV; wCmd = bestW;
      }

      s = stepUnicycle(s, vCmd, wCmd, P.dt);
      humans = predictHumansConstVel(humans, P.dt);
      vPrev = vCmd; wPrev = wCmd;

      minClearAll = Math.min(minClearAll, bestInfo.minClear);
      minTtcAll = Math.min(minTtcAll, bestInfo.minTtc);
      socIntegral += bestInfo.jSoc;

      for (Human h : humans) {
        if (dist(s.x, s.y, h.x, h.y) <= P.robotRadius + h.r) {
          return new EpisodeOut(0.0, t * P.dt, minClearAll, minTtcAll, socIntegral);
        }
      }
    }
    return new EpisodeOut(0.0, Ttotal, minClearAll, minTtcAll, socIntegral);
  }

  public static void main(String[] args) {
    Params P = new Params();
    EpisodeOut out = runEpisode(0, P);
    System.out.println("Episode metrics: {success=" + out.success +
        ", time=" + out.time +
        ", min_clear=" + out.minClear +
        ", min_ttc=" + out.minTtc +
        ", social=" + out.social + "}");

    int N = 5;
    double[] wS = new double[]{0.2, 0.7, 1.2};
    double[] wT = new double[]{0.2, 0.6, 1.0};
    for (double ws : wS) {
      for (double wt : wT) {
        Params Q = new Params();
        Q.wSocial = ws;
        Q.wTtc = wt;

        double succ = 0.0, timeAvg = 0.0, clrAvg = 0.0, ttcAvg = 0.0, socAvg = 0.0;
        for (int sd = 0; sd < N; sd++) {
          EpisodeOut e = runEpisode(sd, Q);
          succ += e.success;
          timeAvg += e.time;
          clrAvg += e.minClear;
          ttcAvg += e.minTtc;
          socAvg += e.social;
        }
        System.out.println("w_social=" + ws + ", w_ttc=" + wt +
            ", success_rate=" + (succ / N) +
            ", time_avg=" + (timeAvg / N) +
            ", min_clear_avg=" + (clrAvg / N) +
            ", min_ttc_avg=" + (ttcAvg / N) +
            ", social_avg=" + (socAvg / N));
      }
    }
  }
}
      

6.4 MATLAB / Simulink

Chapter16_Lesson5.m

function Chapter16_Lesson5()
% ============================
% Chapter16_Lesson5.m
% Lab: Navigate Through Moving Crowds (baseline multi-objective sampler)
% ============================

P = defaultParams();

out = runEpisode(0, P);
disp(out);

% small sweep
seeds = 0:4;
wS = [0.2, 0.7, 1.2];
wT = [0.2, 0.6, 1.0];

for i = 1:numel(wS)
  for j = 1:numel(wT)
    Q = P;
    Q.w_social = wS(i);
    Q.w_ttc = wT(j);

    succ = 0; timeAvg = 0; clrAvg = 0; ttcAvg = 0; socAvg = 0;
    for sd = seeds
      e = runEpisode(sd, Q);
      succ = succ + e.success;
      timeAvg = timeAvg + e.time;
      clrAvg = clrAvg + e.min_clear;
      ttcAvg = ttcAvg + e.min_ttc;
      socAvg = socAvg + e.social;
    end
    N = numel(seeds);
    fprintf('w_social=%.2f, w_ttc=%.2f, success_rate=%.2f, time_avg=%.2f, min_clear_avg=%.3f, min_ttc_avg=%.3f, social_avg=%.3f\n', ...
      Q.w_social, Q.w_ttc, succ/N, timeAvg/N, clrAvg/N, ttcAvg/N, socAvg/N);
  end
end

end

% ----------------------------
% Parameters
% ----------------------------
function P = defaultParams()
P.dt = 0.1;
P.horizon = 2.0;
P.robot_radius = 0.30;

P.v_min = 0.0; P.v_max = 1.2;
P.w_min = -1.5; P.w_max = 1.5;

P.n_v = 11;
P.n_w = 21;

P.w_goal = 1.0;
P.w_clear = 0.8;
P.w_ttc = 0.6;
P.w_social = 0.7;
P.w_speed = 0.15;
P.w_omega = 0.05;
P.w_delta_u = 0.1;
P.v_pref = 0.8;
P.eps = 1e-6;

P.sigma_front = 1.2;
P.sigma_back = 0.6;
P.sigma_y = 0.8;

P.goal_tol = 0.35;
P.T_total = 35.0;
end

% ----------------------------
% Core functions
% ----------------------------
function a = wrapAngle(a)
while a > pi, a = a - 2*pi; end
while a < -pi, a = a + 2*pi; end
end

function s = stepUnicycle(s, v, w, dt)
s.x = s.x + v*cos(s.theta)*dt;
s.y = s.y + v*sin(s.theta)*dt;
s.theta = wrapAngle(s.theta + w*dt);
end

function humans = predictHumansConstVel(humans, dt)
humans.x = humans.x + humans.vx*dt;
humans.y = humans.y + humans.vy*dt;
end

function d = dist(ax, ay, bx, by)
d = hypot(ax-bx, ay-by);
end

function ttc = ttcDiscRel(px, py, vx, vy, R)
A = vx*vx + vy*vy;
B = 2*(px*vx + py*vy);
C = px*px + py*py - R*R;
if A < 1e-12
  if C <= 0, ttc = 0; else, ttc = inf; end
  return;
end
disc = B*B - 4*A*C;
if disc < 0
  ttc = inf; return;
end
sd = sqrt(disc);
t1 = (-B - sd) / (2*A);
t2 = (-B + sd) / (2*A);
cand = [t1, t2];
cand = cand(cand >= 0);
if isempty(cand), ttc = inf; else, ttc = min(cand); end
end

function [dx, dy] = humanFrameComponents(relx, rely, hvx, hvy)
sp = hypot(hvx, hvy);
if sp < 1e-12
  fx = 1; fy = 0;
else
  fx = hvx/sp; fy = hvy/sp;
end
lx = -fy; ly = fx;
dx = relx*fx + rely*fy;
dy = relx*lx + rely*ly;
end

function val = socialPhi(rx, ry, h, P)
relx = rx - h.x;
rely = ry - h.y;
[dx, dy] = humanFrameComponents(relx, rely, h.vx, h.vy);
if dx >= 0, sigma_x = P.sigma_front; else, sigma_x = P.sigma_back; end
val = exp(-0.5*((dx*dx)/(sigma_x*sigma_x) + (dy*dy)/(P.sigma_y*P.sigma_y)));
end

function controls = sampleControls(P)
if P.n_v <= 1
  v_list = 0.5*(P.v_min+P.v_max);
else
  v_list = linspace(P.v_min, P.v_max, P.n_v);
end
if P.n_w <= 1
  w_list = 0;
else
  w_list = linspace(P.w_min, P.w_max, P.n_w);
end
[V, W] = meshgrid(v_list, w_list);
controls = [V(:), W(:)];
end

function [s0, goal, humans] = makeCrossingFlowScene(seed)
rng(seed);

s0.x = -4.0; s0.y = -4.0; s0.theta = deg2rad(45.0);
goal = [4.0, 4.0];

% flow1: left->right
n1 = 10;
x1 = -4 + 8*rand(n1,1);
y1 = -1.2 + 2.4*rand(n1,1);
vx1 = 0.4 + 0.5*rand(n1,1);
vy1 = zeros(n1,1);

% flow2: bottom->top
n2 = 10;
x2 = -1.2 + 2.4*rand(n2,1);
y2 = -4 + 8*rand(n2,1);
vx2 = zeros(n2,1);
vy2 = 0.4 + 0.5*rand(n2,1);

humans.x = [x1; x2];
humans.y = [y1; y2];
humans.vx = [vx1; vx2];
humans.vy = [vy1; vy2];
humans.r = 0.30*ones(n1+n2,1);
end

function [J, info] = rolloutAndScore(s0, humans0, goal, u, u_prev, P)
v = u(1); w = u(2);
dt = P.dt;
N = max(1, floor(P.horizon/dt));

s = s0;
humans = humans0;

min_clear = inf;
min_ttc = inf;
soc_sum = 0;

for k = 1:N
  s = stepUnicycle(s, v, w, dt);
  humans = predictHumansConstVel(humans, dt);

  vrx = v*cos(s.theta);
  vry = v*sin(s.theta);

  for i = 1:numel(humans.x)
    R = P.robot_radius + humans.r(i);
    d = dist(s.x, s.y, humans.x(i), humans.y(i));
    clear = d - R;
    min_clear = min(min_clear, clear);

    prel = [s.x - humans.x(i), s.y - humans.y(i)];
    vrel = [vrx - humans.vx(i), vry - humans.vy(i)];
    ttc = ttcDiscRel(prel(1), prel(2), vrel(1), vrel(2), R);
    min_ttc = min(min_ttc, ttc);

    h.x = humans.x(i); h.y = humans.y(i);
    h.vx = humans.vx(i); h.vy = humans.vy(i);
    soc_sum = soc_sum + socialPhi(s.x, s.y, h, P);
  end
end

goal_dist = dist(s.x, s.y, goal(1), goal(2));
J_clear = 1/(min_clear + P.eps);
J_ttc = 1/(min_ttc + P.eps);
J_soc = soc_sum / N;

J = P.w_goal*goal_dist + P.w_clear*J_clear + P.w_ttc*J_ttc + P.w_social*J_soc + ...
    P.w_speed*(P.v_pref - v)^2 + P.w_omega*(w^2) + P.w_delta_u*((v-u_prev(1))^2 + (w-u_prev(2))^2);

info.goal_dist = goal_dist;
info.min_clear = min_clear;
info.min_ttc = min_ttc;
info.J_soc = J_soc;
info.J_total = J;
end

function out = runEpisode(seed, P)
[s, goal, humans] = makeCrossingFlowScene(seed);
u_prev = [0, 0];

steps = floor(P.T_total / P.dt);

min_clear_all = inf;
min_ttc_all = inf;
soc_integral = 0;

for t = 1:steps
  if dist(s.x, s.y, goal(1), goal(2)) < P.goal_tol
    out.success = 1;
    out.time = (t-1)*P.dt;
    out.min_clear = min_clear_all;
    out.min_ttc = min_ttc_all;
    out.social = soc_integral;
    return;
  end

  bestJ = inf;
  bestU = [];
  bestInfo = [];

  controls = sampleControls(P);
  for k = 1:size(controls,1)
    u = controls(k,:);
    [J, info] = rolloutAndScore(s, humans, goal, u, u_prev, P);
    if info.min_clear <= 0, continue; end
    if J < bestJ
      bestJ = J;
      bestU = u;
      bestInfo = info;
    end
  end

  if isempty(bestU)
    v = 0; w = 0;
    bestInfo.min_clear = -1;
    bestInfo.min_ttc = 0;
    bestInfo.J_soc = 0;
  else
    v = bestU(1); w = bestU(2);
  end

  s = stepUnicycle(s, v, w, P.dt);
  humans = predictHumansConstVel(humans, P.dt);
  u_prev = [v, w];

  min_clear_all = min(min_clear_all, bestInfo.min_clear);
  min_ttc_all = min(min_ttc_all, bestInfo.min_ttc);
  soc_integral = soc_integral + bestInfo.J_soc;

  % collision check
  for i = 1:numel(humans.x)
    if dist(s.x, s.y, humans.x(i), humans.y(i)) <= (P.robot_radius + humans.r(i))
      out.success = 0;
      out.time = (t-1)*P.dt;
      out.min_clear = min_clear_all;
      out.min_ttc = min_ttc_all;
      out.social = soc_integral;
      return;
    end
  end
end

out.success = 0;
out.time = P.T_total;
out.min_clear = min_clear_all;
out.min_ttc = min_ttc_all;
out.social = soc_integral;
end
      

6.5 Wolfram Mathematica

Chapter16_Lesson5.nb

(* ============================ *)
(* Chapter16_Lesson5.nb        *)
(* Lab: Navigate Through Moving Crowds (baseline multi-objective sampler) *)
(* ============================ *)

ClearAll["Global`*"];

(* Parameters *)
P = <|
   "dt" -> 0.1,
   "horizon" -> 2.0,
   "robotRadius" -> 0.30,
   "vMin" -> 0.0, "vMax" -> 1.2,
   "wMin" -> -1.5, "wMax" -> 1.5,
   "nV" -> 11, "nW" -> 21,
   "wGoal" -> 1.0,
   "wClear" -> 0.8,
   "wTtc" -> 0.6,
   "wSocial" -> 0.7,
   "wSpeed" -> 0.15,
   "wOmega" -> 0.05,
   "wDeltaU" -> 0.1,
   "vPref" -> 0.8,
   "eps" -> 10^-6,
   "sigmaFront" -> 1.2,
   "sigmaBack" -> 0.6,
   "sigmaY" -> 0.8,
   "goalTol" -> 0.35,
   "Ttotal" -> 35.0
   |>;

WrapAngle[a_] := Module[{x = a},
  While[x > Pi, x -= 2 Pi];
  While[x < -Pi, x += 2 Pi];
  x
];

StepUnicycle[s_, v_, w_, dt_] := Module[{x, y, th},
  {x, y, th} = s;
  {x + v Cos[th] dt, y + v Sin[th] dt, WrapAngle[th + w dt]}
];

PredictHumansConstVel[humans_, dt_] :=
  humans /. {x_, y_, vx_, vy_, r_} :> {x + vx dt, y + vy dt, vx, vy, r};

Dist[a_, b_] := Norm[a - b];

TTCDiscRel[p_, v_, R_] := Module[{A, B, C, disc, t1, t2, cand},
  A = v.v;
  B = 2 (p.v);
  C = p.p - R^2;
  If[A < 10^-12, Return[If[C <= 0, 0, Infinity]]];
  disc = B^2 - 4 A C;
  If[disc < 0, Return[Infinity]];
  t1 = (-B - Sqrt[disc])/(2 A);
  t2 = (-B + Sqrt[disc])/(2 A);
  cand = Select[{t1, t2}, # >= 0 && NumericQ[#] && # < Infinity &];
  If[cand === {}, Infinity, Min[cand]]
];

HumanFrameComponents[rel_, hv_] := Module[{sp, f, l, dx, dy},
  sp = Norm[hv];
  f = If[sp < 10^-12, {1, 0}, hv/sp];
  l = {-f[[2]], f[[1]]};
  dx = rel.f;
  dy = rel.l;
  {dx, dy}
];

SocialPhi[robotPos_, h_, P_] := Module[{rel, hv, dx, dy, sigmaX, val},
  rel = robotPos - h[[{1, 2}]];
  hv = h[[{3, 4}]];
  {dx, dy} = HumanFrameComponents[rel, hv];
  sigmaX = If[dx >= 0, P["sigmaFront"], P["sigmaBack"]];
  val = Exp[-1/2 ((dx^2)/(sigmaX^2) + (dy^2)/(P["sigmaY"]^2))];
  val
];

SampleControls[P_] := Module[{vList, wList},
  vList = If[P["nV"] <= 1, {0.5 (P["vMin"] + P["vMax"])},
    Table[P["vMin"] + i (P["vMax"] - P["vMin"])/(P["nV"] - 1), {i, 0, P["nV"] - 1}]
  ];
  wList = If[P["nW"] <= 1, {0},
    Table[P["wMin"] + j (P["wMax"] - P["wMin"])/(P["nW"] - 1), {j, 0, P["nW"] - 1}]
  ];
  Flatten[Table[{v, w}, {v, vList}, {w, wList}], 1]
];

MakeCrossingFlowScene[seed_] := Module[{rng, s0, goal, humans, n1 = 10, n2 = 10,
    unif, flow1, flow2},
  rng = RandomGeneratorState[seed];
  SeedRandom[seed];
  s0 = {-4.0, -4.0, 45 Degree};
  goal = {4.0, 4.0};
  unif[a_, b_] := a + (b - a) RandomReal[];

  flow1 = Table[{unif[-4, 4], unif[-1.2, 1.2], unif[0.4, 0.9], 0.0, 0.30}, {n1}];
  flow2 = Table[{unif[-1.2, 1.2], unif[-4, 4], 0.0, unif[0.4, 0.9], 0.30}, {n2}];

  humans = Join[flow1, flow2];
  {s0, goal, humans}
];

RolloutAndScore[s0_, humans0_, goal_, u_, uPrev_, P_] :=
 Module[{v = u[[1]], w = u[[2]], dt = P["dt"], N, s = s0, humans = humans0,
   minClear = Infinity, minTtc = Infinity, socSum = 0.0, k, robotPos, vr,
   h, R, d, clear, prel, vrel, ttc, goalDist, Jclear, Jttc, Jsoc, J},
  N = Max[1, Floor[P["horizon"]/dt]];
  For[k = 1, k <= N, k++,
   s = StepUnicycle[s, v, w, dt];
   humans = PredictHumansConstVel[humans, dt];
   robotPos = s[[{1, 2}]];
   vr = v {Cos[s[[3]]], Sin[s[[3]]]};
   Do[
    h = humans[[i]];
    R = P["robotRadius"] + h[[5]];
    d = Dist[robotPos, h[[{1, 2}]]];
    clear = d - R;
    minClear = Min[minClear, clear];
    prel = robotPos - h[[{1, 2}]];
    vrel = vr - h[[{3, 4}]];
    ttc = TTCDiscRel[prel, vrel, R];
    minTtc = Min[minTtc, ttc];
    socSum += SocialPhi[robotPos, h, P];
    , {i, Length[humans]}];
   ];
  goalDist = Dist[s[[{1, 2}]], goal];
  Jclear = 1/(minClear + P["eps"]);
  Jttc = 1/(minTtc + P["eps"]);
  Jsoc = socSum/N;
  J = P["wGoal"] goalDist + P["wClear"] Jclear + P["wTtc"] Jttc + P["wSocial"] Jsoc +
     P["wSpeed"] (P["vPref"] - v)^2 + P["wOmega"] w^2 + P["wDeltaU"] ((v - uPrev[[1]])^2 + (w - uPrev[[2]])^2);
  {J, <|"goalDist" -> goalDist, "minClear" -> minClear, "minTtc" -> minTtc, "Jsoc" -> Jsoc, "J" -> J|>}
];

RunEpisode[seed_, P_] := Module[{s0, goal, humans, s, uPrev = {0, 0}, steps, t,
   minClearAll = Infinity, minTtcAll = Infinity, socIntegral = 0.0,
   controls, bestJ, bestU, bestInfo, found, v, w},
  {s0, goal, humans} = MakeCrossingFlowScene[seed];
  s = s0;
  steps = Floor[P["Ttotal"]/P["dt"]];
  For[t = 1, t <= steps, t++,
   If[Dist[s[[{1, 2}]], goal] < P["goalTol"],
    Return[<|"success" -> 1, "time" -> (t - 1) P["dt"], "minClear" -> minClearAll, "minTtc" -> minTtcAll, "social" -> socIntegral|>]
   ];
   controls = SampleControls[P];
   bestJ = Infinity; found = False;
   Do[
    Module[{J, info},
     {J, info} = RolloutAndScore[s, humans, goal, u, uPrev, P];
     If[info["minClear"] <= 0, Continue[]];
     If[J < bestJ, bestJ = J; bestU = u; bestInfo = info; found = True];
     ],
    {u, controls}];
   If[!found,
    v = 0; w = 0; bestInfo = <|"minClear" -> -1, "minTtc" -> 0, "Jsoc" -> 0|>,
    v = bestU[[1]]; w = bestU[[2]];
   ];
   s = StepUnicycle[s, v, w, P["dt"]];
   humans = PredictHumansConstVel[humans, P["dt"]];
   uPrev = {v, w};
   minClearAll = Min[minClearAll, bestInfo["minClear"]];
   minTtcAll = Min[minTtcAll, bestInfo["minTtc"]];
   socIntegral += bestInfo["Jsoc"];
   (* collision check *)
   If[Min[Table[Dist[s[[{1, 2}]], humans[[i, {1, 2}]]] - (P["robotRadius"] + humans[[i, 5]]), {i, Length[humans]}]] <= 0,
    Return[<|"success" -> 0, "time" -> (t - 1) P["dt"], "minClear" -> minClearAll, "minTtc" -> minTtcAll, "social" -> socIntegral|>]
   ];
  ];
  <|"success" -> 0, "time" -> P["Ttotal"], "minClear" -> minClearAll, "minTtc" -> minTtcAll, "social" -> socIntegral|>
];

(* Demo *)
out = RunEpisode[0, P];
Print["Episode metrics: ", out];

(* Tiny sweep *)
seeds = Range[0, 4];
wS = {0.2, 0.7, 1.2};
wT = {0.2, 0.6, 1.0};

results = Flatten[
  Table[
    Module[{Q = P, succ = 0.0, timeSum = 0.0, clrSum = 0.0, ttcSum = 0.0, socSum = 0.0, e, Nn},
      Q["wSocial"] = ws; Q["wTtc"] = wt;
      Do[
        e = RunEpisode[sd, Q];
        succ += e["success"];
        timeSum += e["time"];
        clrSum += e["minClear"];
        ttcSum += e["minTtc"];
        socSum += e["social"];
        , {sd, seeds}];
      Nn = Length[seeds];
      <|"wSocial" -> ws, "wTtc" -> wt, "successRate" -> succ/Nn,
        "timeAvg" -> timeSum/Nn, "minClearAvg" -> clrSum/Nn,
        "minTtcAvg" -> ttcSum/Nn, "socialAvg" -> socSum/Nn|>
    ],
    {ws, wS}, {wt, wT}],
  1
];

Print /@ results;
      

ROS/Navigation-stack integration note (optional extension). In ROS2 Nav2, the local planner/controller is typically implemented in C++ as a plugin. As an extension, wrap the sampling planner inside a controller plugin that consumes tracked people (e.g., from a perception stack) and publishes \( v,\omega \) commands. This extension is not required for the multi-language baseline but is strongly recommended if you plan to deploy on a real platform.

Problem 1 (TTC derivation for discs): Let \( \mathbf{p}_{rel} \) and \( \mathbf{v}_{rel} \) be constant. Show that collision between two discs of combined radius \( R \) occurs iff the quadratic \( \|\mathbf{v}_{rel}\|^2 t^2 + 2(\mathbf{p}_{rel}^\top \mathbf{v}_{rel}) t + (\|\mathbf{p}_{rel}\|^2 - R^2)=0 \) has a real root \( t \ge 0 \), and derive the smallest such root.

Solution: Starting from \( \|\mathbf{p}_{rel} + t\mathbf{v}_{rel}\|^2 = R^2 \), expand: \( (\mathbf{p}_{rel} + t\mathbf{v}_{rel})^\top(\mathbf{p}_{rel} + t\mathbf{v}_{rel})=R^2 \), giving: \( \|\mathbf{v}_{rel}\|^2 t^2 + 2(\mathbf{p}_{rel}^\top \mathbf{v}_{rel}) t + (\|\mathbf{p}_{rel}\|^2 - R^2)=0 \). If the discriminant \( \Delta = 4(\mathbf{p}_{rel}^\top \mathbf{v}_{rel})^2 - 4\|\mathbf{v}_{rel}\|^2(\|\mathbf{p}_{rel}\|^2 - R^2) \) is negative, there is no real collision time. Otherwise the roots are:

\[ t_{1,2} = \frac{-2\,\mathbf{p}_{rel}^\top \mathbf{v}_{rel} \pm \sqrt{\Delta}}{2\|\mathbf{v}_{rel}\|^2} = \frac{-\mathbf{p}_{rel}^\top \mathbf{v}_{rel} \pm \sqrt{(\mathbf{p}_{rel}^\top \mathbf{v}_{rel})^2 - \|\mathbf{v}_{rel}\|^2(\|\mathbf{p}_{rel}\|^2 - R^2)}}{\|\mathbf{v}_{rel}\|^2}. \]

The TTC is \( \min\{t_1,t_2\} \) among nonnegative roots; if both are negative, TTC is \( +\infty \).

Problem 2 (VO–TTC equivalence for constant velocity): Prove that for fixed horizon \( T \) and constant \( \mathbf{v}_j \), the condition \( \mathbf{v}_r - \mathbf{v}_j \notin \mathrm{VO}_j^T(\mathbf{p}_{rel}) \) holds iff \( \mathrm{TTC}_j > T \) (or \( \mathrm{TTC}_j = +\infty \)).

Solution: By definition, \( \mathbf{v}_{rel} \in \mathrm{VO}_j^T \) iff \( \exists t\in[0,T] \) such that \( \|\mathbf{p}_{rel} + t\mathbf{v}_{rel}\| \le R_j \). The earliest such time is exactly the TTC. Therefore \( \mathbf{v}_{rel} \notin \mathrm{VO}_j^T \) iff no collision occurs in \( [0,T] \), i.e., TTC exceeds \( T \) (or is infinite).

Problem 3 (Discomfort anisotropy): The discomfort field uses different spreads in front of and behind a human. Show that \( \phi_j(\mathbf{p}) \) is continuous but generally not differentiable at the line \( d_x=0 \) when \( \sigma_{front} \neq \sigma_{back} \).

Solution: For \( d_x \ge 0 \), \( \phi = \exp(-\tfrac12(d_x^2/\sigma_{front}^2 + d_y^2/\sigma_y^2)) \). For \( d_x < 0 \), replace \( \sigma_{front} \) by \( \sigma_{back} \). At \( d_x=0 \), both expressions evaluate to \( \exp(-\tfrac12 d_y^2/\sigma_y^2) \), hence continuity. The derivative with respect to \( d_x \) depends on \( 1/\sigma_x^2 \); if spreads differ, left/right derivatives do not match.

Problem 4 (Discrete-time safety sampling): Assume the distance function to a moving disc boundary is 1-Lipschitz in time with constant \( L \) over a step \( \Delta t \), i.e., \( |c(t) - c(t_k)| \le L|t-t_k| \). Show that if \( c(t_k) > L\Delta t \) then \( c(t) > 0 \) for all \( t\in[t_k,t_{k+1}] \).

Solution: For any \( t\in[t_k,t_{k+1}] \), \( c(t) \ge c(t_k) - L|t-t_k| \ge c(t_k) - L\Delta t > 0 \). This justifies using sufficiently small \( \Delta t \) for discrete collision checks.

Problem 5 (Weight tuning and failure modes): Explain qualitatively why too large \( w_s \) can cause “freezing” (robot stops), while too small \( w_s \) can cause socially unacceptable near-cutting.

Solution: With large \( w_s \), the optimizer strongly penalizes proximity to humans, so the minimum-cost action may be to stop and wait for the field to dissipate (especially if all forward motions incur high social penalty). With small \( w_s \), progress-to-goal dominates and the planner chooses aggressive gap-cutting trajectories that are safe geometrically but violate comfort norms.

You implemented a crowd-aware local planner by extending sampling-based local motion generation to dynamic obstacles using TTC/VO reasoning and a socially aware discomfort field. You designed evaluation metrics, ran repeated-seed experiments, and learned how weight tuning trades off speed, safety margin, and human comfort in dense crowds.