In Lesson 2, we framed dynamic obstacle avoidance via velocity-space constraints (VO / reciprocal methods). Socially aware navigation adds human-centered objectives: safety (no collision), comfort (respect personal space), and predictability/legibility (motions that people can easily interpret).

Let the robot state be \( x(t) \) and control be \( u(t) \). Let the set of humans be \( \mathcal{H}(t)=\{h_i(t)\}_{i=1}^N \). A canonical continuous-time formulation is:

\[ \min_{u(\cdot)} \int_{0}^{T} \Big( w_g \|p(t)-p_g\|^2 + w_u \|u(t)\|^2 + w_s \, C_{\text{social} }(p(t),\mathcal{H}(t)) \Big)\, dt \]

subject to robot dynamics and hard safety constraints (collision avoidance). If we discretize at time step \( k \) with step \( \Delta t \) and a simple kinematic integrator, we write: \( p_{k+1} \; ← \; p_k + \Delta t\, v_k \) (a local planner typically chooses \( v_k \) directly).

Socially aware navigation is therefore naturally implemented inside a local planner that already trades off tracking and obstacle costs (Chapter 15), by adding a social cost term \( C_{\text{social} } \), and/or by modifying collision constraints to include “comfort margins”.

flowchart TD
  A["Perception: detect humans (p_i, v_i)"] --> B["Estimate human heading + comfort parameters"]
  B --> C["Build social cost field (anisotropic personal space)"]
  C --> D["Local planner: sample/optimize candidate v(t)"]
  D --> E["Hard safety: reject candidates that collide within horizon"]
  E --> F["Score: tracking + obstacle + social + smoothness"]
  F --> G["Send command velocity to controller"]
        

Humans maintain “spaces” (intimate/personal/social/public). In robotics, we represent these as soft constraints—penalties that grow as the robot approaches a person, typically stronger in front of a person than behind (because approaching from behind is often less disturbing than cutting in front).

Model each human \( i \) by position \( p_i \in \mathbb{R}^2 \), velocity \( v_i \in \mathbb{R}^2 \), and an orientation \( \theta_i = \operatorname{atan2}(v_{i,y}, v_{i,x}) \) when moving. Define a rotation matrix \( R(\theta_i) \) from human frame to world frame. An anisotropic Gaussian comfort field is:

\[ C_i(p) = \exp\!\Big( -\tfrac{1}{2}\,(p-p_i)^\top \Sigma_i^{-1} (p-p_i) \Big), \quad \Sigma_i = R(\theta_i)\, \begin{bmatrix} \sigma_{x,i}^2 & 0 \\ 0 & \sigma_{y,i}^2 \end{bmatrix} R(\theta_i)^\top \]

Typically choose \( \sigma_{x,i} \) larger in the “front” half-space and smaller behind. The total social cost can be an additive field: \( C_{\text{social} }(p,\mathcal{H}) = \sum_{i=1}^{N} C_i(p) \), or a shaped penalty like \( \sum_i \phi(C_i) \) (e.g., saturating).

Convex surrogate (useful for proofs and optimization). The Gaussian itself is not globally convex in \( p \), but the negative log is quadratic: \( \ell_i(p) = -\log C_i(p) \). Then:

\[ \ell_i(p) = \tfrac{1}{2}\,(p-p_i)^\top \Sigma_i^{-1} (p-p_i) \]

Proposition: If \( \Sigma_i \) is symmetric positive definite, then \( \ell_i(p) \) is convex in \( p \). Proof: The Hessian is constant: \( \nabla^2 \ell_i(p) = \Sigma_i^{-1} \), and since \( \Sigma_i \succ 0 \Rightarrow \Sigma_i^{-1} \succ 0 \), the Hessian is positive definite, hence \( \ell_i \) is strictly convex. ■

This matters because many local planners minimize sums of quadratic-like penalties. If we approximate social discomfort using \( \ell_i \), we obtain convex contributions that behave well under gradient-based solvers (e.g., TEB-style trajectory optimization).

Lesson 2 introduced velocity obstacles: a set of robot velocities that lead to collision within a time horizon \( \tau \). Let relative position and velocity to human \( i \) be: \( p_{ri} = p_i - p_r \) and \( v_{ri} = v_r - v_i \). A collision occurs if there exists \( t \) with \( 0 < t \le \tau \) such that:

\[ \|p_{ri} - t\, v_{ri}\| \le R \quad \text{for some } 0 < t \le \tau \]

where \( R \) is the combined safety radius (robot + human). Social navigation modifies this in two common ways:

• Comfort inflation: replace \( R \) by a direction-dependent \( R_i(\phi) \) (bigger in front of a person, smaller behind).
• Soft VO penalty: keep safety as hard, but add a penalty that grows as \( v_r \) approaches the boundary of forbidden velocities (encourages early yielding).

ORCA-style half-plane constraint (key geometric fact). In reciprocal methods, collision avoidance can be expressed as a linear constraint in velocity space: a half-plane of permitted velocities for each neighbor agent. The overall feasible set is the intersection of half-planes, which is convex, enabling efficient projection/QP.

A standard local step chooses \( v \) by projecting a desired velocity \( v_{\text{des} } \) onto the feasible set:

\[ v^\star = \arg\min_{v} \|v - v_{\text{des} }\|^2 \quad \text{s.t.}\; A v \le b \]

Social awareness is inserted by modifying \( v_{\text{des} } \) (e.g., “pass on the right” bias), and/or adding a differentiable penalty term:

\[ v^\star = \arg\min_{v} \|v - v_{\text{des} }\|^2 + \lambda\, \widetilde{C}_{\text{social} }(p_r + \Delta t\, v) \quad \text{s.t.}\; A v \le b \]

where \( \widetilde{C}_{\text{social} } \) is evaluated at a short lookahead point \( p_r + \Delta t v \). If \( \widetilde{C}_{\text{social} } \) is approximated by a convex quadratic (negative-log Gaussian), the objective stays convex; with linear constraints, this becomes a convex QP.

Two integration patterns dominate AMR stacks:

(A) Sampling-based (DWA-like): sample candidate velocities \( v \), reject those that violate safety within horizon \( \tau \), and score the rest with a weighted sum. A typical score is:

\[ J(v) = w_t \|v - v_{\text{pref} }\|^2 + w_o \, C_{\text{obst} }(p_r + \Delta t v) + w_s \, C_{\text{social} }(p_r + \Delta t v) + w_c \sum_{i=1}^{N}\frac{1}{\|p_r+\Delta t v - p_i\|+\epsilon} \]

where \( v_{\text{pref} } \) is a goal-directed velocity (Chapter 15 tracking idea), and the last term encourages clearance (smoothly).

(B) Optimization-based (TEB-like / local trajopt): optimize a short horizon trajectory \( \{p_k\}_{k=0}^{K} \) or control sequence. Social terms appear as additive penalties:

\[ J(\{p_k\}) = \sum_{k=0}^{K} \Big( w_p \|p_k - \bar{p}_k\|^2 + w_s \sum_{i=1}^{N} \ell_i(p_k) \Big) \quad \text{with}\;\; \ell_i(p)=\tfrac{1}{2}(p-p_i)^\top \Sigma_i^{-1}(p-p_i) \]

Lemma (closure under weighted sums). If each \( f_j \) is convex and weights satisfy \( w_j \ge 0 \), then \( \sum_j w_j f_j \) is convex. This justifies “add a social quadratic term” without breaking convexity of a convex baseline objective. ■

flowchart TD
  S["Candidates: sample v around v_pref"] --> K["Kinematic rollout: p_next = p + dt*v"]
  K --> H["Hard safety test: collision within tau?"]
  H -->|reject| R["discard v"]
  H -->|keep| C["Compute costs: tracking + obstacles + social + clearance"]
  C --> M["Pick v with minimum total score"]
  M --> U["Command v to robot"]
        

The following implementations demonstrate a sampling-based socially aware local planner: compute a goal-directed \( v_{\text{pref} } \), sample candidate velocities, reject those with predicted collision within \( \tau \), and minimize a score that includes an anisotropic proxemic cost.

Code (Chapter16_Lesson3.py):

# Chapter16_Lesson3.py
# Socially Aware Navigation (sampling-based local velocity selection)
# Dependencies: numpy, matplotlib

import numpy as np
import matplotlib.pyplot as plt

def rot2(th):
    c, s = np.cos(th), np.sin(th)
    return np.array([[c, -s],[s, c]])

def anisotropic_gaussian_cost(x, humans, sigma_front=1.2, sigma_side=0.6, sigma_back=0.8):
    """
    Social discomfort field around each human i:
      C_i(x) = exp(-0.5 * (x-p_i)^T Sigma_i^{-1} (x-p_i))
    with anisotropy aligned with the human heading (estimated from velocity).
    """
    cost = 0.0
    for h in humans:
        p = h["p"]
        v = h["v"]
        spd = np.linalg.norm(v)
        th = np.arctan2(v[1], v[0]) if spd > 1e-6 else 0.0
        R = rot2(th)

        # front/back asymmetry: choose sigma_x depending on whether x is in front of the human
        r = x - p
        r_h = R.T @ r
        sx = sigma_front if r_h[0] >= 0.0 else sigma_back
        Sy = np.diag([sx**2, sigma_side**2])
        # Quadratic form
        q = r_h.T @ np.linalg.inv(Sy) @ r_h
        cost += np.exp(-0.5 * q)
    return cost

def collision_within_tau(v_robot, p_robot, humans, R=0.55, tau=3.0):
    """
    Check if robot velocity v_robot would collide with any human within horizon tau,
    using closest-approach analysis on relative motion (constant velocity model).
    """
    for h in humans:
        p_h, v_h = h["p"], h["v"]
        p = p_h - p_robot
        v_rel = v_robot - v_h
        vv = float(v_rel @ v_rel)
        if vv < 1e-10:
            # relative speed ~0 => collision if already too close
            if np.linalg.norm(p) < R:
                return True
            continue
        t_star = - float(p @ v_rel) / vv
        t_star = max(0.0, min(tau, t_star))
        d_min = np.linalg.norm(p + t_star * (-v_rel))
        if d_min < R:
            return True
    return False

def sample_velocities(v_pref, v_max=1.2, n_speed=9, n_angle=21, spread=np.deg2rad(80)):
    """
    Sample candidate velocities around v_pref direction (speed grid + angle offsets).
    """
    spref = np.linalg.norm(v_pref)
    th0 = np.arctan2(v_pref[1], v_pref[0]) if spref > 1e-8 else 0.0
    speeds = np.linspace(0.0, v_max, n_speed)
    angles = np.linspace(-spread, spread, n_angle)
    V = []
    for s in speeds:
        for a in angles:
            th = th0 + a
            V.append(np.array([s*np.cos(th), s*np.sin(th)]))
    return V

def choose_velocity(p_robot, v_pref, humans,
                    v_max=1.2, w_track=1.0, w_social=1.5, w_clear=2.0,
                    R=0.55, tau=3.0):
    """
    Sampling-based socially aware velocity selection:
      minimize  w_track * ||v - v_pref||^2 + w_social * C_social(p_next) + w_clear * sum 1/(d+eps)
      s.t. no predicted collision within tau.
    """
    best_v = np.zeros(2)
    best_J = float("inf")
    eps = 1e-3

    for v in sample_velocities(v_pref, v_max=v_max):
        if collision_within_tau(v, p_robot, humans, R=R, tau=tau):
            continue

        # one-step lookahead position for evaluating comfort
        dt_eval = 0.6
        p_next = p_robot + dt_eval * v

        # social field and clearance shaping
        C = anisotropic_gaussian_cost(p_next, humans)
        clear = 0.0
        for h in humans:
            d = np.linalg.norm(p_next - h["p"])
            clear += 1.0/(d + eps)

        J = w_track*np.sum((v - v_pref)**2) + w_social*C + w_clear*clear
        if J < best_J:
            best_J = J
            best_v = v

    return best_v, best_J

def simulate(seed=2):
    np.random.seed(seed)
    dt = 0.1
    T = 26.0
    steps = int(T/dt)

    p = np.array([0.0, 0.0])
    goal = np.array([10.0, 0.0])
    v_max = 1.2

    # moving humans
    humans = [
        {"p": np.array([4.5,  1.2]), "v": np.array([ 0.35, -0.05])},
        {"p": np.array([4.0, -1.4]), "v": np.array([ 0.35,  0.08])},
        {"p": np.array([7.0,  0.3]), "v": np.array([-0.25,  0.02])},
    ]

    traj = [p.copy()]
    traj_h = [[h["p"].copy()] for h in humans]

    for k in range(steps):
        # update humans
        for i, h in enumerate(humans):
            h["p"] = h["p"] + dt*h["v"]
            traj_h[i].append(h["p"].copy())

        # preferred velocity to goal
        e = goal - p
        dist = np.linalg.norm(e)
        if dist < 0.15:
            break
        v_pref = (e / (dist + 1e-9)) * min(v_max, 0.8*dist)

        v_cmd, _ = choose_velocity(p, v_pref, humans, v_max=v_max, tau=3.0)
        p = p + dt*v_cmd
        traj.append(p.copy())

    traj = np.array(traj)
    plt.figure(figsize=(8,5))
    plt.plot(traj[:,0], traj[:,1], label="robot")
    for i, th in enumerate(traj_h):
        th = np.array(th)
        plt.plot(th[:,0], th[:,1], "--", label=f"human {i+1}")
        plt.scatter(th[0,0], th[0,1], s=30)

    plt.scatter(goal[0], goal[1], marker="*", s=120)
    plt.axis("equal"); plt.grid(True); plt.legend()
    plt.title("Socially aware navigation: sampling-based local velocity selection")
    plt.show()

if __name__ == "__main__":
    simulate()

Exercise Code (Chapter16_Lesson3_Ex1.py):

# Chapter16_Lesson3_Ex1.py
# Exercise: compare isotropic vs anisotropic personal-space models

import numpy as np
import matplotlib.pyplot as plt

def rot2(th):
    c, s = np.cos(th), np.sin(th)
    return np.array([[c, -s],[s, c]])

def field(X, human_p, human_v, anisotropic=True):
    spd = np.linalg.norm(human_v)
    th = np.arctan2(human_v[1], human_v[0]) if spd > 1e-6 else 0.0
    R = rot2(th)
    Z = np.zeros(X.shape[:2])
    for i in range(X.shape[0]):
        for j in range(X.shape[1]):
            x = X[i,j]
            r = x - human_p
            r_h = R.T @ r
            if anisotropic:
                sx = 1.4 if r_h[0] >= 0.0 else 0.9
                sy = 0.6
            else:
                sx = sy = 0.9
            q = (r_h[0]**2)/(sx**2) + (r_h[1]**2)/(sy**2)
            Z[i,j] = np.exp(-0.5*q)
    return Z

def main():
    human_p = np.array([0.0, 0.0])
    human_v = np.array([1.0, 0.0])

    xs = np.linspace(-3,3,200)
    ys = np.linspace(-3,3,200)
    X, Y = np.meshgrid(xs, ys)
    P = np.stack([X, Y], axis=-1)

    Z_iso = field(P, human_p, human_v, anisotropic=False)
    Z_an  = field(P, human_p, human_v, anisotropic=True)

    plt.figure(figsize=(12,5))
    plt.subplot(1,2,1); plt.contourf(X,Y,Z_iso, levels=20); plt.axis("equal"); plt.title("Isotropic comfort field")
    plt.subplot(1,2,2); plt.contourf(X,Y,Z_an,  levels=20); plt.axis("equal"); plt.title("Anisotropic comfort field")
    plt.show()

if __name__ == "__main__":
    main()

Code (Chapter16_Lesson3.cpp):

// Chapter16_Lesson3.cpp
// Socially aware local velocity selection (sampling-based) for a point robot.
// Output: CSV trajectories for robot and humans.
// Build (example): g++ -O2 -std=c++17 Chapter16_Lesson3.cpp -o run
// Run: ./run  (produces out_robot.csv and out_humans.csv)

#include <cmath>
#include <fstream>
#include <iostream>
#include <vector>
#include <array>
#include <limits>

struct Vec2 {
    double x{0}, y{0};
    Vec2() = default;
    Vec2(double x_, double y_) : x(x_), y(y_) {}
    Vec2 operator+(const Vec2& o) const { return {x+o.x, y+o.y}; }
    Vec2 operator-(const Vec2& o) const { return {x-o.x, y-o.y}; }
    Vec2 operator*(double s) const { return {x*s, y*s}; }
};

static inline double dot(const Vec2& a, const Vec2& b){ return a.x*b.x + a.y*b.y; }
static inline double norm2(const Vec2& a){ return dot(a,a); }
static inline double norm(const Vec2& a){ return std::sqrt(norm2(a)); }

struct Human {
    Vec2 p;
    Vec2 v;
};

static inline Vec2 rotT(const Vec2& r, double th){
    // R^T * r with R(th) rotation
    double c = std::cos(th), s = std::sin(th);
    return { c*r.x + s*r.y, -s*r.x + c*r.y };
}

double anisotropic_gaussian_cost(const Vec2& x, const std::vector<Human>& humans,
                                 double sigma_front=1.2, double sigma_side=0.6, double sigma_back=0.8){
    double cost = 0.0;
    for(const auto& h: humans){
        double spd = norm(h.v);
        double th = (spd > 1e-6) ? std::atan2(h.v.y, h.v.x) : 0.0;
        Vec2 r = x - h.p;
        Vec2 rh = rotT(r, th);
        double sx = (rh.x >= 0.0) ? sigma_front : sigma_back;
        double sy = sigma_side;
        double q = (rh.x*rh.x)/(sx*sx) + (rh.y*rh.y)/(sy*sy);
        cost += std::exp(-0.5*q);
    }
    return cost;
}

bool collision_within_tau(const Vec2& v_robot, const Vec2& p_robot, const std::vector<Human>& humans,
                          double R=0.55, double tau=3.0){
    for(const auto& h: humans){
        Vec2 p = h.p - p_robot;
        Vec2 vrel = v_robot - h.v;
        double vv = norm2(vrel);
        if(vv < 1e-12){
            if(norm(p) < R) return true;
            continue;
        }
        double tstar = -dot(p, vrel)/vv;
        if(tstar < 0.0) tstar = 0.0;
        if(tstar > tau) tstar = tau;
        Vec2 closest = p - vrel*tstar; // p + (-vrel)*t
        if(norm(closest) < R) return true;
    }
    return false;
}

std::vector<Vec2> sample_velocities(const Vec2& v_pref, double v_max=1.2, int n_speed=9, int n_angle=21, double spread_deg=80.0){
    double spref = norm(v_pref);
    double th0 = (spref > 1e-8) ? std::atan2(v_pref.y, v_pref.x) : 0.0;
    double spread = spread_deg * M_PI / 180.0;

    std::vector<Vec2> V;
    for(int i=0;i<n_speed;i++){
        double s = (double)i/(n_speed-1) * v_max;
        for(int j=0;j<n_angle;j++){
            double a = -spread + 2.0*spread*(double)j/(n_angle-1);
            double th = th0 + a;
            V.emplace_back(s*std::cos(th), s*std::sin(th));
        }
    }
    return V;
}

Vec2 choose_velocity(const Vec2& p_robot, const Vec2& v_pref, const std::vector<Human>& humans,
                     double v_max=1.2, double w_track=1.0, double w_social=1.5, double w_clear=2.0,
                     double R=0.55, double tau=3.0){
    double bestJ = std::numeric_limits<double>::infinity();
    Vec2 bestV{0,0};
    const double dt_eval = 0.6;
    const double eps = 1e-3;

    for(const auto& v: sample_velocities(v_pref, v_max)){
        if(collision_within_tau(v, p_robot, humans, R, tau)) continue;

        Vec2 p_next = p_robot + v*dt_eval;
        double C = anisotropic_gaussian_cost(p_next, humans);
        double clear = 0.0;
        for(const auto& h: humans){
            double d = norm(p_next - h.p);
            clear += 1.0/(d + eps);
        }

        Vec2 dv = Vec2{v.x - v_pref.x, v.y - v_pref.y};
        double J = w_track*norm2(dv) + w_social*C + w_clear*clear;

        if(J < bestJ){
            bestJ = J;
            bestV = v;
        }
    }
    return bestV;
}

int main(){
    double dt = 0.1;
    double T = 26.0;
    int steps = (int)std::round(T/dt);

    Vec2 p{0.0, 0.0};
    Vec2 goal{10.0, 0.0};
    double v_max = 1.2;

    std::vector<Human> humans = {
        {Vec2{4.5,  1.2}, Vec2{ 0.35, -0.05} },
        {Vec2{4.0, -1.4}, Vec2{ 0.35,  0.08} },
        {Vec2{7.0,  0.3}, Vec2{-0.25,  0.02} }
    };

    std::ofstream fR("out_robot.csv");
    std::ofstream fH("out_humans.csv");
    fR << "t,x,y\n";
    fH << "t,id,x,y\n";

    for(int k=0;k<steps;k++){
        double t = k*dt;

        // log
        fR << t << "," << p.x << "," << p.y << "\n";
        for(size_t i=0;i<humans.size();i++){
            fH << t << "," << (i+1) << "," << humans[i].p.x << "," << humans[i].p.y << "\n";
        }

        // update humans
        for(auto& h: humans){
            h.p = h.p + h.v*dt;
        }

        // preferred velocity to goal
        Vec2 e = goal - p;
        double dist = norm(e);
        if(dist < 0.15) break;
        Vec2 v_pref = e*(1.0/(dist + 1e-12));
        double s = std::min(v_max, 0.8*dist);
        v_pref = v_pref*s;

        Vec2 v_cmd = choose_velocity(p, v_pref, humans, v_max, 1.0, 1.5, 2.0, 0.55, 3.0);
        p = p + v_cmd*dt;
    }

    std::cout << "Wrote out_robot.csv and out_humans.csv\n";
    return 0;
}

Code (Chapter16_Lesson3.java):

// Chapter16_Lesson3.java
// Socially aware local velocity selection (sampling-based) for a point robot.
// Output: CSV trajectories for robot and humans.
// Compile: javac Chapter16_Lesson3.java
// Run:     java Chapter16_Lesson3

import java.io.FileWriter;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;

public class Chapter16_Lesson3 {

    static class Vec2 {
        double x, y;
        Vec2(double x, double y){ this.x=x; this.y=y; }
        Vec2 add(Vec2 o){ return new Vec2(x+o.x, y+o.y); }
        Vec2 sub(Vec2 o){ return new Vec2(x-o.x, y-o.y); }
        Vec2 mul(double s){ return new Vec2(x*s, y*s); }
        double dot(Vec2 o){ return x*o.x + y*o.y; }
        double norm2(){ return dot(this); }
        double norm(){ return Math.sqrt(norm2()); }
    }

    static class Human {
        Vec2 p, v;
        Human(Vec2 p, Vec2 v){ this.p=p; this.v=v; }
    }

    static Vec2 rotT(Vec2 r, double th){
        double c = Math.cos(th), s = Math.sin(th);
        return new Vec2(c*r.x + s*r.y, -s*r.x + c*r.y);
    }

    static double anisotropicGaussianCost(Vec2 x, List<Human> humans,
                                          double sigmaFront, double sigmaSide, double sigmaBack){
        double cost = 0.0;
        for(Human h: humans){
            double spd = h.v.norm();
            double th = (spd > 1e-6) ? Math.atan2(h.v.y, h.v.x) : 0.0;
            Vec2 r = x.sub(h.p);
            Vec2 rh = rotT(r, th);
            double sx = (rh.x >= 0.0) ? sigmaFront : sigmaBack;
            double sy = sigmaSide;
            double q = (rh.x*rh.x)/(sx*sx) + (rh.y*rh.y)/(sy*sy);
            cost += Math.exp(-0.5*q);
        }
        return cost;
    }

    static boolean collisionWithinTau(Vec2 vRobot, Vec2 pRobot, List<Human> humans, double R, double tau){
        for(Human h: humans){
            Vec2 p = h.p.sub(pRobot);
            Vec2 vrel = vRobot.sub(h.v);
            double vv = vrel.norm2();
            if(vv < 1e-12){
                if(p.norm() < R) return true;
                continue;
            }
            double tstar = -p.dot(vrel)/vv;
            if(tstar < 0.0) tstar = 0.0;
            if(tstar > tau) tstar = tau;
            Vec2 closest = p.sub(vrel.mul(tstar));
            if(closest.norm() < R) return true;
        }
        return false;
    }

    static List<Vec2> sampleVelocities(Vec2 vPref, double vMax, int nSpeed, int nAngle, double spreadDeg){
        double spref = vPref.norm();
        double th0 = (spref > 1e-8) ? Math.atan2(vPref.y, vPref.x) : 0.0;
        double spread = spreadDeg * Math.PI / 180.0;

        List<Vec2> V = new ArrayList<>();
        for(int i=0;i<nSpeed;i++){
            double s = (double)i/(nSpeed-1) * vMax;
            for(int j=0;j<nAngle;j++){
                double a = -spread + 2.0*spread*(double)j/(nAngle-1);
                double th = th0 + a;
                V.add(new Vec2(s*Math.cos(th), s*Math.sin(th)));
            }
        }
        return V;
    }

    static Vec2 chooseVelocity(Vec2 pRobot, Vec2 vPref, List<Human> humans,
                              double vMax, double wTrack, double wSocial, double wClear,
                              double R, double tau){
        double bestJ = Double.POSITIVE_INFINITY;
        Vec2 bestV = new Vec2(0.0, 0.0);
        double dtEval = 0.6;
        double eps = 1e-3;

        for(Vec2 v: sampleVelocities(vPref, vMax, 9, 21, 80.0)){
            if(collisionWithinTau(v, pRobot, humans, R, tau)) continue;

            Vec2 pNext = pRobot.add(v.mul(dtEval));
            double C = anisotropicGaussianCost(pNext, humans, 1.2, 0.6, 0.8);
            double clear = 0.0;
            for(Human h: humans){
                double d = pNext.sub(h.p).norm();
                clear += 1.0/(d + eps);
            }
            Vec2 dv = v.sub(vPref);
            double J = wTrack*dv.norm2() + wSocial*C + wClear*clear;
            if(J < bestJ){
                bestJ = J;
                bestV = v;
            }
        }
        return bestV;
    }

    public static void main(String[] args) throws IOException {
        double dt = 0.1;
        double T = 26.0;
        int steps = (int)Math.round(T/dt);

        Vec2 p = new Vec2(0.0, 0.0);
        Vec2 goal = new Vec2(10.0, 0.0);
        double vMax = 1.2;

        List<Human> humans = new ArrayList<>();
        humans.add(new Human(new Vec2(4.5,  1.2), new Vec2( 0.35, -0.05)));
        humans.add(new Human(new Vec2(4.0, -1.4), new Vec2( 0.35,  0.08)));
        humans.add(new Human(new Vec2(7.0,  0.3), new Vec2(-0.25,  0.02)));

        try(FileWriter fR = new FileWriter("out_robot.csv");
            FileWriter fH = new FileWriter("out_humans.csv")){

            fR.write("t,x,y\n");
            fH.write("t,id,x,y\n");

            for(int k=0;k<steps;k++){
                double t = k*dt;

                // log
                fR.write(t + "," + p.x + "," + p.y + "\n");
                for(int i=0;i<humans.size();i++){
                    Human h = humans.get(i);
                    fH.write(t + "," + (i+1) + "," + h.p.x + "," + h.p.y + "\n");
                }

                // update humans
                for(Human h: humans){
                    h.p = h.p.add(h.v.mul(dt));
                }

                Vec2 e = goal.sub(p);
                double dist = e.norm();
                if(dist < 0.15) break;

                Vec2 dir = e.mul(1.0/(dist + 1e-12));
                double s = Math.min(vMax, 0.8*dist);
                Vec2 vPref = dir.mul(s);

                Vec2 vCmd = chooseVelocity(p, vPref, humans, vMax, 1.0, 1.5, 2.0, 0.55, 3.0);
                p = p.add(vCmd.mul(dt));
            }
        }
        System.out.println("Wrote out_robot.csv and out_humans.csv");
    }
}

Code (Chapter16_Lesson3.m):

% Chapter16_Lesson3.m
% Socially Aware Navigation (sampling-based local velocity selection)
% Run: Chapter16_Lesson3
%
% Simulink note:
%   Put the function "choose_velocity" in a MATLAB Function block.
%   Inputs: p (2x1), goal (2x1), humansP (2xN), humansV (2xN)
%   Output: v_cmd (2x1)

function Chapter16_Lesson3()
clc; close all;

dt = 0.1;
T  = 26.0;
steps = round(T/dt);

p    = [0;0];
goal = [10;0];
v_max = 1.2;

humansP = [ 4.5  4.0  7.0;
            1.2 -1.4  0.3 ];
humansV = [ 0.35  0.35 -0.25;
           -0.05  0.08  0.02 ];

traj = zeros(2, steps);
trajH = zeros(2, steps, size(humansP,2));

for k = 1:steps
    traj(:,k) = p;
    trajH(:,k,:) = humansP;

    % update humans
    humansP = humansP + dt*humansV;

    % preferred velocity to goal
    e = goal - p;
    dist = norm(e);
    if dist < 0.15
        traj = traj(:,1:k);
        trajH = trajH(:,1:k,:);
        break;
    end
    v_pref = (e/(dist+1e-12)) * min(v_max, 0.8*dist);

    v_cmd = choose_velocity(p, v_pref, humansP, humansV, v_max, 3.0);
    p = p + dt*v_cmd;
end

figure; hold on; grid on; axis equal;
plot(traj(1,:), traj(2,:), 'LineWidth', 2);
for i=1:size(humansP,2)
    plot(squeeze(trajH(1,:,i)), squeeze(trajH(2,:,i)), '--', 'LineWidth', 1.5);
end
plot(goal(1), goal(2), '*', 'MarkerSize', 12, 'LineWidth', 2);
title('Socially aware navigation: sampling-based local velocity selection');
legend(['robot', arrayfun(@(i) sprintf('human %d',i), 1:size(humansP,2), 'UniformOutput', false)]);
end

function v_best = choose_velocity(p, v_pref, humansP, humansV, v_max, tau)
w_track  = 1.0;
w_social = 1.5;
w_clear  = 2.0;
Rcol = 0.55;

bestJ = inf;
v_best = [0;0];

V = sample_velocities(v_pref, v_max, 9, 21, deg2rad(80));

for k=1:size(V,2)
    v = V(:,k);
    if collision_within_tau(v, p, humansP, humansV, Rcol, tau)
        continue;
    end

    dt_eval = 0.6;
    p_next = p + dt_eval*v;

    C = anisotropic_gaussian_cost(p_next, humansP, humansV, 1.2, 0.6, 0.8);

    clear = 0.0;
    eps = 1e-3;
    for i=1:size(humansP,2)
        d = norm(p_next - humansP(:,i));
        clear = clear + 1.0/(d + eps);
    end

    J = w_track*sum((v - v_pref).^2) + w_social*C + w_clear*clear;
    if J < bestJ
        bestJ = J;
        v_best = v;
    end
end
end

function V = sample_velocities(v_pref, v_max, n_speed, n_angle, spread)
spref = norm(v_pref);
th0 = atan2(v_pref(2), v_pref(1));
if spref < 1e-8
    th0 = 0.0;
end
speeds = linspace(0, v_max, n_speed);
angles = linspace(-spread, spread, n_angle);

V = zeros(2, n_speed*n_angle);
idx = 1;
for s = speeds
    for a = angles
        th = th0 + a;
        V(:,idx) = [s*cos(th); s*sin(th)];
        idx = idx + 1;
    end
end
end

function tf = collision_within_tau(v_robot, p_robot, humansP, humansV, R, tau)
tf = false;
for i=1:size(humansP,2)
    p = humansP(:,i) - p_robot;
    v_rel = v_robot - humansV(:,i);
    vv = sum(v_rel.^2);
    if vv < 1e-12
        if norm(p) < R, tf = true; return; end
        continue;
    end
    t_star = - (p.'*v_rel) / vv;
    t_star = max(0, min(tau, t_star));
    d_min = norm(p - t_star*v_rel);
    if d_min < R, tf = true; return; end
end
end

function cost = anisotropic_gaussian_cost(x, humansP, humansV, sigma_front, sigma_side, sigma_back)
cost = 0.0;
for i=1:size(humansP,2)
    p = humansP(:,i);
    v = humansV(:,i);
    spd = norm(v);
    th = 0.0;
    if spd > 1e-6, th = atan2(v(2), v(1)); end
    R = [cos(th) -sin(th); sin(th) cos(th)];

    r = x - p;
    r_h = R.'*r;
    sx = sigma_front; if r_h(1) < 0, sx = sigma_back; end
    sy = sigma_side;

    q = (r_h(1)^2)/(sx^2) + (r_h(2)^2)/(sy^2);
    cost = cost + exp(-0.5*q);
end
end

Code (Chapter16_Lesson3.nb):

(* Content-type: application/vnd.wolfram.mathematica *)

Notebook[{

Cell["Chapter16_Lesson3.nb  — Socially Aware Navigation (Wolfram Language)", "Title"],

Cell["1. Proxemic (anisotropic) comfort field", "Section"],
Cell[BoxData[
 RowBox[{
  RowBox[{"comfortCost", "[",
   RowBox[{"x_", ",", "p_", ",", "v_", ",", "sigF_", ",", "sigS_", ",", "sigB_"}],
   "]"}], ":=",
  RowBox[{"Module", "[",
   RowBox[{
    RowBox[{"{th, R, r, rh, sx, sy, q}"}], ",",
    RowBox[{
     RowBox[{"th", "=",
      RowBox[{"If", "[",
       RowBox[{
        RowBox[{
         RowBox[{"Norm", "[", "v", "]"}], ">", "10^-6"}], ",",
        RowBox[{"ArcTan", "[",
         RowBox[{
          RowBox[{"v", "[", "[", "1", "]"}], ",",
          RowBox[{"v", "[", "[", "2", "]"}]}], "]"}], ",", "0"}], "]"}]}], ";",
     RowBox[{"R", "=",
      RowBox[{"{",
       RowBox[{
        RowBox[{"{",
         RowBox[{
          RowBox[{"Cos", "[", "th", "]"}], ",",
          RowBox[{"-Sin", "[", "th", "]"}]}], "}"}], ",",
        RowBox[{"{",
         RowBox[{
          RowBox[{"Sin", "[", "th", "]"}], ",",
          RowBox[{"Cos", "[", "th", "]"}]}], "}"}]}], "}"}]}], ";",
     RowBox[{"r", "=", "x", "-", "p"}], ";",
     RowBox[{"rh", "=",
      RowBox[{"Transpose", "[", "R", "]"}], ".", "r"}], ";",
     RowBox[{"sx", "=",
      RowBox[{"If", "[",
       RowBox[{
        RowBox[{
         RowBox[{"rh", "[", "[", "1", "]"}], "\[GreaterEqual]", "0"}], ",",
        "sigF", ",", "sigB"}], "]"}]}], ";",
     RowBox[{"sy", "=", "sigS"}], ";",
     RowBox[{"q", "=",
      RowBox[{
       RowBox[{
        RowBox[{"rh", "[", "[", "1", "]"}], "^", "2"}], "/", "sx^2", "+",
       RowBox[{
        RowBox[{"rh", "[", "[", "2", "]"}], "^", "2"}], "/", "sy^2"}]}], ";",
     RowBox[{"Exp", "[",
      RowBox[{"-0.5", " ", "q"}], "]"}]}]}], "]"}]}]], "Input"],

Cell["2. Velocity selection (sampling + collision check)", "Section"],
Cell[BoxData[
 RowBox[{
  RowBox[{"collisionWithinTau", "[",
   RowBox[{"vR_", ",", "pR_", ",", "pH_", ",", "vH_", ",", "R_", ",", "tau_"}],
   "]"}], ":=",
  RowBox[{"Module", "[",
   RowBox[{
    RowBox[{"{p, vrel, vv, tstar, dmin}"}], ",",
    RowBox[{
     RowBox[{"p", "=", "pH", "-", "pR"}], ";",
     RowBox[{"vrel", "=", "vR", "-", "vH"}], ";",
     RowBox[{"vv", "=", "vrel.vrel"}], ";",
     RowBox[{"If", "[",
      RowBox[{
       RowBox[{"vv", "<", "10^-12"}], ",",
       RowBox[{"Return", "[",
        RowBox[{
         RowBox[{"Norm", "[", "p", "]"}], "<", "R"}], "]"}]}], "]"}], ";",
     RowBox[{"tstar", "=",
      RowBox[{"Clip", "[",
       RowBox[{
        RowBox[{"-", " ", "p.vrel", "/", "vv"}], ",",
        RowBox[{"{",
         RowBox[{"0", ",", "tau"}], "}"}]}], "]"}]}], ";",
     RowBox[{"dmin", "=",
      RowBox[{"Norm", "[",
       RowBox[{"p", "-", "tstar", " ", "vrel"}], "]"}]}], ";",
     RowBox[{"dmin", "<", "R"}]}]}], "]"}]}]], "Input"],

Cell[BoxData[
 RowBox[{
  RowBox[{"chooseVelocity", "[",
   RowBox[{"pR_", ",", "vPref_", ",", "humans_", ",", "vMax_", ",", "tau_"}],
   "]"}], ":=",
  RowBox[{"Module", "[",
   RowBox[{
    RowBox[{"{wT=1.0, wS=1.5, wC=2.0, dt=0.6, eps=10^-3, bestJ=\[Infinity], bestV={0,0}, V, v, pN, C, clear, J}"}], ",",
    RowBox[{
     RowBox[{"V", "=",
      RowBox[{"Flatten", "[",
       RowBox[{
        RowBox[{"Table", "[",
         RowBox[{
          RowBox[{"s",
           RowBox[{"{",
            RowBox[{
             RowBox[{"Cos", "[",
              RowBox[{"th0", "+", "a"}], "]"}], ",",
             RowBox[{"Sin", "[",
              RowBox[{"th0", "+", "a"}], "]"}]}], "}"}]}], ",",
          RowBox[{"{",
           RowBox[{"s", ",", "0", ",", "vMax", ",", "vMax/8"}], "}"}], ",",
          RowBox[{"{",
           RowBox[{"a", ",",
            RowBox[{"-80", " ", "Degree"}], ",",
            RowBox[{"80", " ", "Degree"}], ",",
            RowBox[{"160", " ", "Degree/20"}]}], "}"}]}], "]"}], ",", "1"}],
       "]"}]}], ";",
     RowBox[{"th0", "=",
      RowBox[{"If", "[",
       RowBox[{
        RowBox[{
         RowBox[{"Norm", "[", "vPref", "]"}], ">", "10^-8"}], ",",
        RowBox[{"ArcTan", "[",
         RowBox[{
          RowBox[{"vPref", "[", "[", "1", "]"}], ",",
          RowBox[{"vPref", "[", "[", "2", "]"}]}], "]"}], ",", "0"}], "]"}]}],
     ";",
     RowBox[{"Do", "[",
      RowBox[{
       RowBox[{
        RowBox[{"v", "=", "vi"}], ";",
        RowBox[{"If", "[",
         RowBox[{
          RowBox[{"AnyTrue", "[",
           RowBox[{"humans", ",",
            RowBox[{
             RowBox[{"collisionWithinTau", "[",
              RowBox[{"v", ",", "pR", ",",
               RowBox[{"#", "[", "\"p\"", "]"}], ",",
               RowBox[{"#", "[", "\"v\"", "]"}], ",", "0.55", ",", "tau"}],
              "]"}], "&"}]}], "]"}], ",",
          RowBox[{"Continue", "[", "]"}]}], "]"}], ";",
        RowBox[{"pN", "=", "pR", "+", "dt", " ", "v"}], ";",
        RowBox[{"C", "=",
         RowBox[{"Total", "[",
          RowBox[{
           RowBox[{"comfortCost", "[",
            RowBox[{"pN", ",",
             RowBox[{"#", "[", "\"p\"", "]"}], ",",
             RowBox[{"#", "[", "\"v\"", "]"}], ",", "1.2", ",", "0.6", ",",
             "0.8"}], "]"}], "&"}], "/@", "humans"}], "]"}]}], ";",
        RowBox[{"clear", "=",
         RowBox[{"Total", "[",
          RowBox[{
           RowBox[{"1.0", "/",
            RowBox[{"(",
             RowBox[{
              RowBox[{"Norm", "[",
               RowBox[{"pN", "-",
                RowBox[{"#", "[", "\"p\"", "]"}]}], "]"}], "+", "eps"}],
             ")"}]}], "&"}], "/@", "humans"}], "]"}]}], ";",
        RowBox[{"J", "=",
         RowBox[{"wT",
          RowBox[{"Norm", "[",
           RowBox[{"v", "-", "vPref"}], "]"}], "^", "2", "+", "wS", " ", "C",
          "+", "wC", " ", "clear"}]}], ";",
        RowBox[{"If", "[",
         RowBox[{
          RowBox[{"J", "<", "bestJ"}], ",",
          RowBox[{
           RowBox[{"bestJ", "=", "J"}], ";",
           RowBox[{"bestV", "=", "v"}]}]}], "]"}]}], ",",
       RowBox[{"{",
        RowBox[{"vi", ",", "V"}], "}"}]}], "]"}], ";",
     RowBox[{"{",
      RowBox[{"bestV", ",", "bestJ"}], "}"}]}]}], "]"}]}]], "Input"],

Cell["Example run", "Subsection"],
Cell[BoxData[
 RowBox[{
  RowBox[{"humans", "=",
   RowBox[{"{",
    RowBox[{
     RowBox[{"<|",
      RowBox[{
       RowBox[{"\"p\"", "->", "{4.5, 1.2}"}], ",",
       RowBox[{"\"v\"", "->", "{0.35, -0.05}"}]}], "|>"}], ",",
     RowBox[{"<|",
      RowBox[{
       RowBox[{"\"p\"", "->", "{4.0, -1.4}"}], ",",
       RowBox[{"\"v\"", "->", "{0.35, 0.08}"}]}], "|>"}], ",",
     RowBox[{"<|",
      RowBox[{
       RowBox[{"\"p\"", "->", "{7.0, 0.3}"}], ",",
       RowBox[{"\"v\"", "->", "{-0.25, 0.02}"}]}], "|>"}]}], "}"}]}], ";",
  RowBox[{"chooseVelocity", "[",
   RowBox[{"{0, 0}", ",", "{1.2, 0}", ",", "humans", ",", "1.2", ",", "3.0"}],
   "]"}]}]], "Input"]

},
WindowSize->{900, 700},
WindowMargins->{ {Automatic, 50}, {Automatic, 50} },
FrontEndVersion->"14.0 for Microsoft Windows (64-bit) (December 12, 2023)",
StyleDefinitions->"Default.nb"
]

Problem 1 (Elliptical Proxemics Geometry): Let \( \ell(p)=\tfrac{1}{2}(p-p_i)^\top \Sigma^{-1}(p-p_i) \) with \( \Sigma \succ 0 \). Show that the level set \( \{p : \ell(p) \le c\} \) is an ellipse, and derive its principal axes.

Solution: Diagonalize \( \Sigma \) as \( \Sigma = Q \Lambda Q^\top \), where \( \Lambda=\operatorname{diag}(\lambda_1,\lambda_2) \), \( \lambda_j > 0 \). Substitute into \( \ell \):

\[ \ell(p)=\tfrac{1}{2}(p-p_i)^\top Q \Lambda^{-1} Q^\top (p-p_i) = \tfrac{1}{2}\| \Lambda^{-1/2} Q^\top (p-p_i)\|^2 \]

Let \( z = \Lambda^{-1/2} Q^\top (p-p_i) \). Then \( \ell(p)\le c \iff \|z\|^2 \le 2c \), a disk in \( z \)-space. Mapping back yields an ellipse in \( p \)-space with principal directions given by columns of \( Q \) and semi-axis lengths \( \sqrt{2c\,\lambda_1} \) and \( \sqrt{2c\,\lambda_2} \). ■

Problem 2 (Convexity of Social Quadratic Sums): Suppose \( J(p)=\sum_{i=1}^{N} w_i \ell_i(p) \), where each \( \ell_i(p)=\tfrac{1}{2}(p-p_i)^\top \Sigma_i^{-1}(p-p_i) \) and \( w_i \ge 0 \). Prove that \( J \) is convex.

Solution: Each \( \ell_i \) has Hessian \( \nabla^2 \ell_i = \Sigma_i^{-1} \succ 0 \), hence is convex. The Hessian of the weighted sum is \( \nabla^2 J = \sum_i w_i \Sigma_i^{-1} \). A nonnegative weighted sum of PSD matrices is PSD, therefore \( \nabla^2 J \succeq 0 \). Hence \( J \) is convex. ■

Problem 3 (Collision Test by Closest Approach): For relative position \( p \) and relative velocity \( v \), define \( d(t)=\|p - t v\| \). Show that the minimizing time \( t^\star \) over \( 0 \le t \le \tau \) is \( t^\star = \operatorname{clip}\!\left(-\frac{p^\top v}{\|v\|^2}, 0, \tau\right) \), and that collision within horizon occurs if \( d(t^\star) \le R \).

Solution: Minimize \( d^2(t)=\|p-t v\|^2 = p^\top p - 2t p^\top v + t^2 v^\top v \), a convex quadratic in \( t \). The unconstrained minimizer satisfies \( \frac{d}{dt}d^2(t)= -2 p^\top v + 2t \|v\|^2 = 0 \Rightarrow t_0 = \frac{p^\top v}{\|v\|^2} \) for the form \( \|p-t v\| \), and with the sign convention in the lesson code (using \( p - t v \)), we obtain \( t_0 = -\frac{p^\top v}{\|v\|^2} \). Project onto the interval: \( t^\star=\operatorname{clip}(t_0,0,\tau) \). Collision within horizon exists iff the minimum distance over the interval is at most \( R \), i.e., \( d(t^\star) \le R \). ■

Problem 4 (QP Interpretation of ORCA-like Step): Consider the constrained velocity selection \( \min_v \|v - v_{\text{des} }\|^2 \) subject to linear constraints \( A v \le b \). Show that the optimality conditions are the KKT conditions and interpret the solution geometrically.

Solution: The Lagrangian is \( \mathcal{L}(v,\lambda)=\|v-v_{\text{des} }\|^2 + \lambda^\top (A v - b) \), with \( \lambda \ge 0 \). KKT: (i) stationarity \( 2(v-v_{\text{des} }) + A^\top \lambda = 0 \); (ii) primal feasibility \( A v \le b \); (iii) dual feasibility \( \lambda \ge 0 \); (iv) complementary slackness \( \lambda_j (a_j^\top v - b_j)=0 \). Geometrically, the solution is the Euclidean projection of \( v_{\text{des} } \) onto the convex polyhedron \( \{v : A v \le b\} \). This is exactly the “project desired velocity to safe feasible set” logic used by reciprocal methods. ■

We defined socially aware navigation as dynamic obstacle avoidance augmented with proxemics, comfort, and legibility. Mathematically, social behavior is captured by anisotropic cost fields around humans and inserted into local planning either as additive costs (sampling / optimization) or as modified velocity constraints (comfort-inflated safety). The provided multi-language implementations demonstrate a practical sampling-based design consistent with Chapter 15 planners and Chapter 16 velocity-horizon reasoning.