A candidate planar path is given as a continuous curve of configurations \( q(s) = (x(s), y(s), \theta(s)) \), parameterized by arc-length \( s \in [0, L] \). Candidate paths in this chapter are typically piecewise-smooth (e.g., lines + circular arcs, clothoid-like transitions, or primitive concatenations). The feasibility checker decides whether there exists an admissible control signal that can track the path without violating kinematic, actuation, and obstacle constraints.

We use the standard car-like kinematic model (front steering, wheelbase \( \ell \)) already aligned with earlier lessons:

\[ \dot{x} = v\cos\theta,\quad \dot{y} = v\sin\theta,\quad \dot{\theta} = \frac{v}{\ell}\tan\delta \]

The candidate path is geometric. Execution requires a time parameterization \( s(t) \) and a steering profile \( \delta(t) \). Define curvature of the planar path as \( \kappa(s) \). A central theme of feasibility is converting geometric constraints on \( \kappa(s) \) into control/actuator constraints on \( \delta(t) \), \( \dot{\delta}(t) \), \( v(t) \), and \( \dot{v}(t) \).

Assume the planar curve \( (x(s), y(s)) \) is at least twice differentiable on segments. The curvature of a general parameterization is \( \kappa = \frac{x' y'' - y' x''}{(x'^2 + y'^2)^{3/2}} \) (prime denotes derivative w.r.t. the chosen parameter).

\[ \kappa(s) = \frac{x'(s)\,y''(s) - y'(s)\,x''(s)}{\left(x'(s)^2 + y'(s)^2\right)^{3/2}} \]

When \( s \) is the arc-length parameter, we have \( x'(s)^2 + y'(s)^2 = 1 \), so the formula simplifies.

\[ x'(s)^2 + y'(s)^2 = 1 \;\;\Rightarrow\;\; \kappa(s) = x'(s)\,y''(s) - y'(s)\,x''(s) \]

Proposition 1 (Curvature–steering equivalence). For the car-like model above, tracking a geometric path with curvature \( \kappa(s) \) requires steering angle \( \delta(s) = \arctan(\ell\,\kappa(s)) \). In particular, if \( |\delta| \le \delta_{\max} \), then a necessary condition for feasibility is \( |\kappa(s)| \le \kappa_{\max} \), where \( \kappa_{\max} = \tan(\delta_{\max})/\ell \).

\[ \kappa(s) = \frac{\tan\delta(s)}{\ell} \;\;\Rightarrow\;\; \delta(s) = \arctan\!\big(\ell\,\kappa(s)\big), \quad |\kappa(s)| \le \kappa_{\max} := \frac{\tan(\delta_{\max})}{\ell} \]

Proof. From the kinematics, \( \dot{\theta} = \frac{v}{\ell}\tan\delta \). If \( s(t) \) is arc-length, then \( \dot{s} = v \) and \( \frac{d\theta}{ds} = \frac{\dot{\theta}}{\dot{s}} = \frac{1}{\ell}\tan\delta \). By definition, curvature is \( \kappa(s) = \frac{d\theta}{ds} \) for arc-length parameterization, hence \( \kappa = \tan\delta/\ell \), which gives the stated relation. The bound follows immediately by monotonicity of \( \tan(\cdot) \) on \( (-\pi/2, \pi/2) \). □

Continuity note (important for steering limits). If a candidate path is a concatenation of primitives with curvature jumps, then \( \kappa(s) \) is discontinuous and the required steering \( \delta(s) \) is also discontinuous; under any finite steering-rate limit, such a path is not executable without smoothing. Therefore, beyond \( |\kappa| \le \kappa_{\max} \), we must check curvature regularity.

A practical robot imposes a steering-rate bound \( |\dot{\delta}(t)| \le \dot{\delta}_{\max} \). This constraint couples the geometry (\( \kappa(s) \)) to the chosen speed profile (\( v(t) \)) via the chain rule.

Differentiate the relation \( \kappa = \tan\delta/\ell \) w.r.t. time:

\[ \dot{\kappa}(t) = \frac{1}{\ell}\sec^2\!\big(\delta(t)\big)\,\dot{\delta}(t) \;\;\Rightarrow\;\; \dot{\delta}(t) = \ell\cos^2\!\big(\delta(t)\big)\,\dot{\kappa}(t) \]

If the candidate path is provided as \( \kappa(s) \), then \( \dot{\kappa}(t) = \frac{d\kappa}{ds}\dot{s} = \kappa'(s)\,v(t) \). Combining:

\[ \dot{\delta}(t) = \ell\cos^2\!\big(\delta(t)\big)\,v(t)\,\kappa'(s(t)) \]

Proposition 2 (Conservative steering-rate bound in curvature space). Suppose \( |\delta(t)| \le \delta_{\max} \). Then \( \cos^2(\delta(t)) \ge \cos^2(\delta_{\max}) \), so a sufficient condition for \( |\dot{\delta}(t)| \le \dot{\delta}_{\max} \) is

\[ |v(t)\,\kappa'(s(t))| \le \frac{\dot{\delta}_{\max}}{\ell\,\cos^2(\delta_{\max})} \;=:\; \dot{\kappa}_{\max}^{\,\text{eff}} \]

Proof. Using \( |\dot{\delta}| = \ell|\cos^2(\delta)|\,|v\kappa'| \) and the inequality \( \cos^2(\delta) \le 1 \), we obtain \( |\dot{\delta}| \le \ell\,|v\kappa'| \). To sharpen the bound while keeping it implementable, enforce \( |\cos^2(\delta)| \ge \cos^2(\delta_{\max}) \) over the admissible steering range. Rearranging yields the stated sufficient condition. □

This check reveals a key design principle: even if a path satisfies \( |\kappa| \le \kappa_{\max} \), it may still be infeasible at high speed because the robot cannot rotate the steering fast enough. Therefore feasibility must be evaluated jointly with time-scaling.

We now incorporate common execution limits: \( |v(t)| \le v_{\max} \), \( |\dot{v}(t)| \le a_{\max} \) (longitudinal acceleration), and a lateral acceleration bound due to tire friction / stability: \( |a_y(t)| \le a_{y,\max} \).

For planar motion with arc-length parameterization, the normal (lateral) acceleration satisfies:

\[ a_y(t) = v(t)^2\,\kappa(s(t)) \]

Proposition 3 (Lateral acceleration induces a curvature-dependent speed limit). If \( |v(t)^2\kappa(s(t))| \le a_{y,\max} \), then necessarily

\[ |v(t)| \le \sqrt{\frac{a_{y,\max}}{|\kappa(s(t))|}} \quad\text{for}\quad |\kappa(s(t))| > 0 \]

Proof. Rearranging \( |v^2\kappa| \le a_{y,\max} \) yields \( v^2 \le a_{y,\max}/|\kappa| \) when \( |\kappa| > 0 \). Taking square roots gives the bound. □

Therefore we define a pointwise speed cap \( v_{\text{cap}}(s) = \min\!\left(v_{\max}, \sqrt{a_{y,\max}/|\kappa(s)|}\right) \) (with the convention that the square-root term is \( +\infty \) when \( \kappa=0 \)).

Time-scaling with longitudinal acceleration. Let \( v(s) = \dot{s}(t) \). Using \( \dot{v} = \frac{dv}{ds}\dot{s} = v\frac{dv}{ds} \), the acceleration bound \( |\dot{v}| \le a_{\max} \) becomes

\[ \left| v(s)\frac{dv}{ds} \right| \le a_{\max} \;\;\Rightarrow\;\; \left|\frac{d}{ds}\left(\frac{1}{2}v(s)^2\right)\right| \le a_{\max} \]

Discretize the path by samples \( s_0, s_1, \dots, s_N \) with \( \Delta s_i = s_{i+1}-s_i > 0 \). The inequality implies the forward constraint (accelerating) and backward constraint (decelerating):

\[ v_{i+1}^2 \le v_i^2 + 2a_{\max}\Delta s_i, \qquad v_i^2 \le v_{i+1}^2 + 2a_{\max}\Delta s_i \]

A standard and effective feasibility algorithm is: initialize \( v_i \) by \( v_{\text{cap}}(s_i) \), apply a forward pass enforcing the first inequality, then a backward pass enforcing the second. The result is a speed profile that satisfies the pointwise cap and the acceleration coupling.

Remark (connection to steering-rate). With \( v(s) \) obtained, the steering-rate check reduces to verifying \( |\ell\cos^2(\delta(s))\,v(s)\,\kappa'(s)| \le \dot{\delta}_{\max} \) over samples (and conservatively between samples as in Section 5).

Let obstacles occupy a closed set \( \mathcal{O} \subset \mathbb{R}^2 \). The robot footprint at configuration \( q(s) \) is a set \( \mathcal{S}(q(s)) \subset \mathbb{R}^2 \). Collision-free execution requires: \( \mathcal{S}(q(s)) \cap \mathcal{O} = \emptyset \) for all \( s \in [0,L] \).

To keep the feasibility test implementable and fast, a common conservative approximation is a disk footprint: \( \mathcal{S}(q) = \{p\in\mathbb{R}^2 : \|p - (x,y)\| \le r\} \). Then collision-free is equivalent to a distance-to-obstacle constraint: \( d((x(s),y(s)),\mathcal{O}) \ge r \), where \( d(p,\mathcal{O}) = \inf_{o\in\mathcal{O}}\|p-o\| \).

\[ \mathcal{S}(q(s)) \cap \mathcal{O} = \emptyset \;\;\Longleftrightarrow\;\; d\big((x(s),y(s)),\mathcal{O}\big) \ge r \]

In grid maps, we precompute a distance field (distance transform) that provides \( d \) at each cell center. A practical rule is to enforce \( d \ge r + m \) with margin \( m > 0 \).

Lemma 1 (Distance-to-set is 1-Lipschitz). For any closed set \( \mathcal{O} \), the function \( d(p,\mathcal{O}) \) satisfies \( |d(p,\mathcal{O}) - d(p',\mathcal{O})| \le \|p-p'\| \).

Proof. Let \( o^\star \) be a point achieving (or approximating) the infimum for \( p \). Then \( d(p',\mathcal{O}) \le \|p' - o^\star\| \le \|p' - p\| + \|p - o^\star\| \). Thus \( d(p',\mathcal{O}) - d(p,\mathcal{O}) \le \|p'-p\| \). Swapping \( p \) and \( p' \) gives the reverse inequality. □

Conservative sampling principle. Suppose we sample the path at points \( p_i = (x(s_i),y(s_i)) \). If we can bound the maximum spatial deviation between samples, we can conservatively guarantee clearance between them. By Lemma 1, along any segment between samples:

\[ d(p(s),\mathcal{O}) \ge d(p_i,\mathcal{O}) - \|p(s)-p_i\| \]

In particular, if for each interval we ensure \( d(p_i,\mathcal{O}) \ge r + m + \Delta_i \) and \( d(p_{i+1},\mathcal{O}) \ge r + m + \Delta_i \), where \( \Delta_i \) upper-bounds the maximum deviation \( \sup_{s\in[s_i,s_{i+1}]}\min(\|p(s)-p_i\|,\|p(s)-p_{i+1}\|) \), then \( d(p(s),\mathcal{O}) \ge r+m \) holds throughout the interval.

flowchart TD
  A["Candidate path q(s)"] --> B["Compute arc length samples s_i"]
  B --> C["Estimate curvature kappa(s_i) and dkappa/ds"]
  C --> D["Check |kappa| <= kappa_max (steering bound)"]
  D --> E["Compute speed cap v_cap from v_max and lateral accel"]
  E --> F["Forward-backward pass to satisfy |dv/dt| <= a_max"]
  F --> G["Check steering-rate using v(s_i) and dkappa/ds"]
  G --> H["Compute distance field from occupancy grid"]
  H --> I["Check clearance >= r + margin at samples"]
  I --> J["Feasible? accept else reject"]
        

The feasibility gate above is intentionally ordered: the low-cost checks (curvature, time-scaling) should reject many candidates before expensive collision tests if your system generates many candidates.

Real candidate paths are often provided as discrete samples. The feasibility checks then rely on numerical differentiation. Two implementation rules matter:

• Curvature estimation: use a three-point circumcircle (or equivalent signed-area formula) to avoid noisy second derivatives.
• Derivative of curvature: compute \( \kappa'(s) \) using central differences on the arc-length axis, and enforce feasibility with a margin to cover discretization error.

Conservative acceptance. If you only sample constraints at \( s_i \), you should allocate margins: \( \kappa_{\max}^{\text{use}} = \kappa_{\max} - \epsilon_\kappa \), \( \dot{\delta}_{\max}^{\text{use}} = \dot{\delta}_{\max} - \epsilon_{\dot{\delta}} \), and \( r^{\text{use}} = r + m \), where the epsilons cover derivative noise and unmodeled effects.

Robotics libraries to know (Python): ROS2 rclpy, message types nav_msgs, geometry tf_transformations, and collision/geometry tooling such as shapely. The implementation below is standalone and focuses on the mathematics of the feasibility gate.

File: Chapter3_Lesson5.py

# Chapter3_Lesson5.py
"""
Autonomous Mobile Robots (Control Engineering major)
Chapter 3 — Nonholonomic Motion and Feasibility for AMR
Lesson 5 — Feasibility Checks for Candidate Paths

This module implements a practical feasibility checker for candidate paths of a car-like robot.
It verifies:
  1) curvature (steering) limits
  2) steering-rate limits (via curvature derivative + speed profile)
  3) speed and acceleration limits (forward-backward time scaling)
  4) collision clearance against an occupancy grid (disk footprint approximation)

Dependencies:
  - numpy
  - scipy (optional, for distance_transform_edt)
Robotics ecosystem references (not required to run this file):
  - ROS2 nav_msgs/Path, geometry_msgs/PoseStamped, nav2_costmap_2d (C++)
  - OMPL, FCL (collision), SBPL (grid planning)

Author: course content generator
"""

from __future__ import annotations
from dataclasses import dataclass
from typing import Tuple, Dict, Any, Optional
import math
import numpy as np

try:
    from scipy.ndimage import distance_transform_edt  # type: ignore
    _HAS_SCIPY = True
except Exception:
    _HAS_SCIPY = False


@dataclass
class Limits:
    wheelbase_m: float
    delta_max_rad: float          # |steering angle| <= delta_max
    delta_dot_max_rad_s: float    # |steering rate| <= delta_dot_max
    v_max_m_s: float              # |v| <= v_max (actuator/command limit)
    a_long_max_m_s2: float        # |v_dot| <= a_long_max
    a_lat_max_m_s2: float         # |v^2*kappa| <= a_lat_max
    robot_radius_m: float         # disk footprint radius
    clearance_margin_m: float     # extra safety margin


@dataclass
class FeasibilityReport:
    ok: bool
    reason: str
    stats: Dict[str, Any]


def _arc_length(xy: np.ndarray) -> np.ndarray:
    """Cumulative arc length s[i] for points xy[i] = [x,y]."""
    d = np.linalg.norm(np.diff(xy, axis=0), axis=1)
    s = np.zeros(xy.shape[0], dtype=float)
    s[1:] = np.cumsum(d)
    return s


def _curvature_discrete(xy: np.ndarray, s: np.ndarray) -> np.ndarray:
    """
    Discrete signed curvature estimate using three-point formula.
    For i in 1..N-2, fit circumcircle through (i-1,i,i+1).
    """
    n = xy.shape[0]
    kappa = np.zeros(n, dtype=float)
    eps = 1e-12
    for i in range(1, n - 1):
        p0 = xy[i - 1]
        p1 = xy[i]
        p2 = xy[i + 1]
        a = p1 - p0
        b = p2 - p1
        c = p2 - p0

        la = np.linalg.norm(a)
        lb = np.linalg.norm(b)
        lc = np.linalg.norm(c)

        # Twice signed area of triangle (p0,p1,p2) = cross(a, c)
        area2 = a[0] * c[1] - a[1] * c[0]
        denom = (la * lb * lc) + eps

        # curvature magnitude = 2*area / (la*lb*lc), signed by area
        kappa[i] = 2.0 * area2 / denom

    # endpoints: copy nearest interior
    if n >= 3:
        kappa[0] = kappa[1]
        kappa[-1] = kappa[-2]
    return kappa


def _dkappa_ds(kappa: np.ndarray, s: np.ndarray) -> np.ndarray:
    """Central difference derivative dkappa/ds with endpoint one-sided diffs."""
    dk = np.zeros_like(kappa)
    n = kappa.size
    if n < 3:
        return dk
    ds = np.diff(s)
    eps = 1e-12
    for i in range(1, n - 1):
        denom = (s[i + 1] - s[i - 1]) + eps
        dk[i] = (kappa[i + 1] - kappa[i - 1]) / denom
    dk[0] = (kappa[1] - kappa[0]) / (ds[0] + eps)
    dk[-1] = (kappa[-1] - kappa[-2]) / (ds[-1] + eps)
    return dk


def _speed_profile_time_scaling(
    s: np.ndarray,
    kappa: np.ndarray,
    limits: Limits
) -> Tuple[np.ndarray, np.ndarray]:
    """
    Compute a feasible speed profile v(s) satisfying:
      - v <= v_max
      - |v^2 * kappa| <= a_lat_max  (lateral accel bound)
      - |dv/dt| <= a_long_max      (longitudinal accel bound)

    Uses a standard forward-backward pass on v^2 with respect to arc length:
      v_{i+1}^2 <= v_i^2 + 2*a_long_max*ds
      v_i^2     <= v_{i+1}^2 + 2*a_long_max*ds
    Returns:
      v[i] (m/s), and dt[i] for each segment i->i+1.
    """
    n = s.size
    ds = np.diff(s)
    eps = 1e-12

    # Speed upper bound per point from lateral accel + actuator cap
    v_cap = np.full(n, limits.v_max_m_s, dtype=float)
    for i in range(n):
        if abs(kappa[i]) > 1e-9:
            v_lat = math.sqrt(max(limits.a_lat_max_m_s2 / abs(kappa[i]), 0.0))
            v_cap[i] = min(v_cap[i], v_lat)

    # Work with w = v^2 for convex-ish propagation under accel bounds
    w = np.minimum(v_cap, limits.v_max_m_s) ** 2

    # Forward pass (accelerating)
    for i in range(n - 1):
        w[i + 1] = min(w[i + 1], w[i] + 2.0 * limits.a_long_max_m_s2 * max(ds[i], 0.0))

    # Backward pass (decelerating)
    for i in range(n - 2, -1, -1):
        w[i] = min(w[i], w[i + 1] + 2.0 * limits.a_long_max_m_s2 * max(ds[i], 0.0))

    v = np.sqrt(np.maximum(w, 0.0))

    # Segment time using trapezoidal integration: dt = ds / v_avg
    dt = np.zeros(n - 1, dtype=float)
    for i in range(n - 1):
        v_avg = 0.5 * (v[i] + v[i + 1])
        dt[i] = ds[i] / (v_avg + eps)
    return v, dt


def _steering_from_curvature(kappa: np.ndarray, wheelbase_m: float) -> np.ndarray:
    """Ackermann relation: kappa = tan(delta)/L  => delta = atan(L*kappa)."""
    return np.arctan(wheelbase_m * kappa)


def _check_collision_disk(
    xy: np.ndarray,
    occ: np.ndarray,
    resolution_m: float,
    origin_xy_m: Tuple[float, float],
    limits: Limits
) -> Tuple[bool, Dict[str, Any]]:
    """
    Disk-footprint collision check against occupancy grid.
    occ: 2D array (H,W) with 1 = obstacle, 0 = free.
    resolution_m: meters per cell
    origin_xy_m: world coordinate of grid cell (0,0) corner (x0,y0)

    Condition: dist(center, obstacles) >= robot_radius + clearance_margin
    Uses distance transform in grid coordinates.
    """
    H, W = occ.shape
    x0, y0 = origin_xy_m
    needed = (limits.robot_radius_m + limits.clearance_margin_m)

    if _HAS_SCIPY:
        # distance_transform_edt returns distance in cells to nearest zero (False).
        # We want distance to obstacles (occ==1), so invert: free==1 => distance to obstacle
        free = (occ == 0)
        dist_cells = distance_transform_edt(free)
        dist_m = dist_cells * resolution_m
    else:
        # Fallback: crude 8-neighborhood multi-source BFS with chamfer costs
        dist_m = _fallback_distance_transform(occ, resolution_m)

    min_clear = float("inf")
    worst_idx = -1

    for i, (x, y) in enumerate(xy):
        gx = int(math.floor((x - x0) / resolution_m))
        gy = int(math.floor((y - y0) / resolution_m))
        if gx < 0 or gx >= W or gy < 0 or gy >= H:
            return False, {"reason": "path_outside_grid", "index": i}
        c = float(dist_m[gy, gx])
        if c < min_clear:
            min_clear = c
            worst_idx = i
        if c < needed:
            return False, {"reason": "insufficient_clearance", "index": i, "clearance_m": c, "needed_m": needed}

    return True, {"min_clearance_m": min_clear, "worst_index": worst_idx, "needed_m": needed}


def _fallback_distance_transform(occ: np.ndarray, resolution_m: float) -> np.ndarray:
    """
    Simple Dijkstra distance-to-obstacle on a grid (8-connected).
    Returns distances in meters. Obstacles have distance 0.
    """
    import heapq
    H, W = occ.shape
    INF = 1e18
    dist = np.full((H, W), INF, dtype=float)

    pq = []
    for y in range(H):
        for x in range(W):
            if occ[y, x] == 1:
                dist[y, x] = 0.0
                heapq.heappush(pq, (0.0, y, x))

    # 8-connected moves with costs (1 or sqrt(2)) * resolution
    moves = [(-1, 0, 1.0), (1, 0, 1.0), (0, -1, 1.0), (0, 1, 1.0),
             (-1, -1, math.sqrt(2.0)), (-1, 1, math.sqrt(2.0)),
             (1, -1, math.sqrt(2.0)), (1, 1, math.sqrt(2.0))]

    while pq:
        d, y, x = heapq.heappop(pq)
        if d > dist[y, x]:
            continue
        for dy, dx, w in moves:
            ny, nx = y + dy, x + dx
            if 0 <= ny < H and 0 <= nx < W:
                nd = d + w * resolution_m
                if nd < dist[ny, nx]:
                    dist[ny, nx] = nd
                    heapq.heappush(pq, (nd, ny, nx))
    return dist


def check_path_feasibility(
    xy: np.ndarray,
    occ: Optional[np.ndarray],
    resolution_m: float = 0.05,
    origin_xy_m: Tuple[float, float] = (0.0, 0.0),
    limits: Optional[Limits] = None
) -> FeasibilityReport:
    """
    Main feasibility check.
    Inputs:
      xy: N x 2 array of (x,y) world coordinates along candidate path.
      occ: occupancy grid (H x W) with 1 obstacle, 0 free; may be None to skip collision.
    """
    if limits is None:
        limits = Limits(
            wheelbase_m=0.33,
            delta_max_rad=0.45,
            delta_dot_max_rad_s=0.75,
            v_max_m_s=1.2,
            a_long_max_m_s2=0.8,
            a_lat_max_m_s2=1.5,
            robot_radius_m=0.25,
            clearance_margin_m=0.05,
        )

    if xy.ndim != 2 or xy.shape[1] != 2 or xy.shape[0] < 3:
        return FeasibilityReport(False, "invalid_path_shape", {})

    s = _arc_length(xy)
    kappa = _curvature_discrete(xy, s)
    dk_ds = _dkappa_ds(kappa, s)

    # Steering feasibility
    kappa_max = math.tan(limits.delta_max_rad) / limits.wheelbase_m
    if np.max(np.abs(kappa)) > (kappa_max + 1e-9):
        return FeasibilityReport(False, "curvature_limit_violation", {
            "kappa_max": kappa_max,
            "kappa_abs_max": float(np.max(np.abs(kappa))),
        })

    # Time scaling for v(s), dt
    v, dt = _speed_profile_time_scaling(s, kappa, limits)

    # Steering rate feasibility: delta_dot = (d delta/dt)
    delta = _steering_from_curvature(kappa, limits.wheelbase_m)
    # kappa_dot = v * dkappa/ds, delta_dot = (L * cos^2(delta)) * kappa_dot
    kappa_dot = v * dk_ds
    delta_dot = limits.wheelbase_m * (np.cos(delta) ** 2) * kappa_dot
    if np.max(np.abs(delta_dot)) > (limits.delta_dot_max_rad_s + 1e-9):
        return FeasibilityReport(False, "steering_rate_violation", {
            "delta_dot_max": limits.delta_dot_max_rad_s,
            "delta_dot_abs_max": float(np.max(np.abs(delta_dot))),
        })

    # Collision feasibility (disk approximation)
    if occ is not None:
        ok_col, col_stats = _check_collision_disk(xy, occ, resolution_m, origin_xy_m, limits)
        if not ok_col:
            return FeasibilityReport(False, "collision_violation", col_stats)
    else:
        col_stats = {"skipped": True}

    return FeasibilityReport(True, "ok", {
        "path_length_m": float(s[-1]),
        "kappa_abs_max": float(np.max(np.abs(kappa))),
        "kappa_max": float(kappa_max),
        "v_min_m_s": float(np.min(v)),
        "v_max_m_s": float(np.max(v)),
        "delta_abs_max_rad": float(np.max(np.abs(delta))),
        "delta_dot_abs_max_rad_s": float(np.max(np.abs(delta_dot))),
        "total_time_s": float(np.sum(dt)),
        "collision": col_stats,
    })


def _demo() -> None:
    # Simple demo: an arc that is feasible, and a grid with a single obstacle.
    t = np.linspace(0.0, 1.0, 200)
    R = 4.0
    ang = 0.6 * t
    xy = np.column_stack([R * np.sin(ang), R * (1.0 - np.cos(ang))])

    # Occupancy grid 10m x 10m with 0.05m resolution
    res = 0.05
    H = W = int(10.0 / res)
    occ = np.zeros((H, W), dtype=np.uint8)
    # Place a small obstacle block near (2.0, 1.0)
    x0, y0 = 0.0, 0.0
    ox, oy = 2.0, 1.0
    gx = int((ox - x0) / res)
    gy = int((oy - y0) / res)
    occ[max(0, gy - 3):min(H, gy + 3), max(0, gx - 3):min(W, gx + 3)] = 1

    rep = check_path_feasibility(xy, occ, resolution_m=res, origin_xy_m=(x0, y0))
    print(rep.ok, rep.reason)
    print(rep.stats)


if __name__ == "__main__":
    _demo()
      

Robotics libraries to know (C++): ROS2 rclcpp, Nav2 costmaps (nav2_costmap_2d), OMPL for sampling-based planning, and FCL for collision checking with arbitrary footprints. The code below is standalone and mirrors the mathematical checks from this lesson.

File: Chapter3_Lesson5.cpp

// Chapter3_Lesson5.cpp
/*
Autonomous Mobile Robots (Control Engineering major)
Chapter 3 — Nonholonomic Motion and Feasibility for AMR
Lesson 5 — Feasibility Checks for Candidate Paths

Self-contained C++17 feasibility checker for a car-like robot.
Checks:
  - curvature/steering bounds
  - steering-rate bound (via curvature derivative and speed profile)
  - speed & longitudinal acceleration bounds (forward-backward pass)
  - collision clearance on occupancy grid (disk approximation) using multi-source Dijkstra

Build (example):
  g++ -O2 -std=c++17 Chapter3_Lesson5.cpp -o feas

Robotics ecosystem references (not required here):
  - ROS2 rclcpp, nav_msgs/Path (path ingestion)
  - nav2_costmap_2d (costmap), OMPL (planning), FCL (collision)

Author: course content generator
*/

#include <cmath>
#include <cstdint>
#include <iostream>
#include <limits>
#include <queue>
#include <string>
#include <tuple>
#include <utility>
#include <vector>

struct Limits {
  double wheelbase_m = 0.33;
  double delta_max_rad = 0.45;
  double delta_dot_max_rad_s = 0.75;
  double v_max_m_s = 1.2;
  double a_long_max_m_s2 = 0.8;
  double a_lat_max_m_s2 = 1.5;
  double robot_radius_m = 0.25;
  double clearance_margin_m = 0.05;
};

struct Point2 {
  double x{0}, y{0};
};

struct Report {
  bool ok{false};
  std::string reason;
  double path_length_m{0};
  double kappa_abs_max{0};
  double kappa_max{0};
  double v_min{0};
  double v_max{0};
  double delta_dot_abs_max{0};
  double total_time_s{0};
};

static double norm2(const Point2& a) { return std::sqrt(a.x * a.x + a.y * a.y); }
static Point2 sub(const Point2& a, const Point2& b) { return {a.x - b.x, a.y - b.y}; }
static double cross2(const Point2& a, const Point2& b) { return a.x * b.y - a.y * b.x; }

static std::vector<double> arcLength(const std::vector<Point2>& xy) {
  std::vector<double> s(xy.size(), 0.0);
  for (size_t i = 1; i < xy.size(); ++i) {
    Point2 d = sub(xy[i], xy[i - 1]);
    s[i] = s[i - 1] + norm2(d);
  }
  return s;
}

// 3-point signed curvature estimate
static std::vector<double> curvatureDiscrete(const std::vector<Point2>& xy) {
  const size_t n = xy.size();
  std::vector<double> kappa(n, 0.0);
  const double eps = 1e-12;
  for (size_t i = 1; i + 1 < n; ++i) {
    Point2 p0 = xy[i - 1], p1 = xy[i], p2 = xy[i + 1];
    Point2 a = sub(p1, p0);
    Point2 b = sub(p2, p1);
    Point2 c = sub(p2, p0);
    double la = norm2(a), lb = norm2(b), lc = norm2(c);
    double area2 = cross2(a, c);
    double denom = la * lb * lc + eps;
    kappa[i] = 2.0 * area2 / denom;
  }
  if (n >= 3) {
    kappa[0] = kappa[1];
    kappa[n - 1] = kappa[n - 2];
  }
  return kappa;
}

static std::vector<double> dkappaDs(const std::vector<double>& kappa, const std::vector<double>& s) {
  const size_t n = kappa.size();
  std::vector<double> dk(n, 0.0);
  const double eps = 1e-12;
  if (n < 3) return dk;
  for (size_t i = 1; i + 1 < n; ++i) {
    double denom = (s[i + 1] - s[i - 1]) + eps;
    dk[i] = (kappa[i + 1] - kappa[i - 1]) / denom;
  }
  dk[0] = (kappa[1] - kappa[0]) / ((s[1] - s[0]) + eps);
  dk[n - 1] = (kappa[n - 1] - kappa[n - 2]) / ((s[n - 1] - s[n - 2]) + eps);
  return dk;
}

// forward-backward time scaling with lateral accel bound
static void speedProfile(
    const std::vector<double>& s,
    const std::vector<double>& kappa,
    const Limits& lim,
    std::vector<double>& v_out,
    std::vector<double>& dt_out) {

  const size_t n = s.size();
  v_out.assign(n, lim.v_max_m_s);
  dt_out.assign(n > 0 ? n - 1 : 0, 0.0);

  // per-point speed cap from lateral accel
  std::vector<double> vcap(n, lim.v_max_m_s);
  for (size_t i = 0; i < n; ++i) {
    double ak = std::fabs(kappa[i]);
    if (ak > 1e-9) {
      double vlat = std::sqrt(std::max(lim.a_lat_max_m_s2 / ak, 0.0));
      if (vlat < vcap[i]) vcap[i] = vlat;
    }
  }

  // w = v^2
  std::vector<double> w(n, 0.0);
  for (size_t i = 0; i < n; ++i) {
    double v0 = std::min(vcap[i], lim.v_max_m_s);
    w[i] = v0 * v0;
  }

  // forward (accelerate)
  for (size_t i = 0; i + 1 < n; ++i) {
    double ds = s[i + 1] - s[i];
    double w_next = w[i] + 2.0 * lim.a_long_max_m_s2 * std::max(ds, 0.0);
    if (w[i + 1] > w_next) w[i + 1] = w_next;
  }
  // backward (decelerate)
  for (size_t i = n; i-- > 1;) {
    double ds = s[i] - s[i - 1];
    double w_prev = w[i] + 2.0 * lim.a_long_max_m_s2 * std::max(ds, 0.0);
    if (w[i - 1] > w_prev) w[i - 1] = w_prev;
  }

  for (size_t i = 0; i < n; ++i) v_out[i] = std::sqrt(std::max(w[i], 0.0));

  const double eps = 1e-12;
  for (size_t i = 0; i + 1 < n; ++i) {
    double ds = s[i + 1] - s[i];
    double vavg = 0.5 * (v_out[i] + v_out[i + 1]);
    dt_out[i] = ds / (vavg + eps);
  }
}

// Grid distance to obstacles via multi-source Dijkstra (8-connected)
static std::vector<double> distanceToObstacles(
    const std::vector<uint8_t>& occ, int H, int W, double res_m) {
  const double INF = 1e18;
  std::vector<double> dist(H * W, INF);

  struct Node {
    double d;
    int idx;
    bool operator>(const Node& other) const { return d > other.d; }
  };

  std::priority_queue<Node, std::vector<Node>, std::greater<Node>> pq;

  auto push_if = [&](int idx) {
    if (occ[idx] == 1) {
      dist[idx] = 0.0;
      pq.push({0.0, idx});
    }
  };

  for (int i = 0; i < H * W; ++i) push_if(i);

  const int dx[8] = {1, -1, 0, 0,  1,  1, -1, -1};
  const int dy[8] = {0, 0, 1, -1, 1, -1,  1, -1};
  const double w[8] = {1, 1, 1, 1, std::sqrt(2.0), std::sqrt(2.0), std::sqrt(2.0), std::sqrt(2.0)};

  while (!pq.empty()) {
    Node cur = pq.top();
    pq.pop();
    if (cur.d > dist[cur.idx]) continue;
    int y = cur.idx / W;
    int x = cur.idx % W;
    for (int k = 0; k < 8; ++k) {
      int nx = x + dx[k];
      int ny = y + dy[k];
      if (nx < 0 || nx >= W || ny < 0 || ny >= H) continue;
      int nidx = ny * W + nx;
      double nd = cur.d + w[k] * res_m;
      if (nd < dist[nidx]) {
        dist[nidx] = nd;
        pq.push({nd, nidx});
      }
    }
  }
  return dist;
}

static Report checkFeasible(
    const std::vector<Point2>& xy,
    const std::vector<uint8_t>* occ, int H, int W,
    double res_m, Point2 origin,
    const Limits& lim) {

  Report rep;
  if (xy.size() < 3) {
    rep.ok = false;
    rep.reason = "invalid_path";
    return rep;
  }

  std::vector<double> s = arcLength(xy);
  std::vector<double> kappa = curvatureDiscrete(xy);
  std::vector<double> dkds = dkappaDs(kappa, s);

  double kappa_max = std::tan(lim.delta_max_rad) / lim.wheelbase_m;
  double kappa_abs_max = 0.0;
  for (double ki : kappa) kappa_abs_max = std::max(kappa_abs_max, std::fabs(ki));
  rep.kappa_abs_max = kappa_abs_max;
  rep.kappa_max = kappa_max;
  rep.path_length_m = s.back();

  if (kappa_abs_max > kappa_max + 1e-9) {
    rep.ok = false;
    rep.reason = "curvature_limit_violation";
    return rep;
  }

  // speed profile and time
  std::vector<double> v, dt;
  speedProfile(s, kappa, lim, v, dt);
  rep.v_min = *std::min_element(v.begin(), v.end());
  rep.v_max = *std::max_element(v.begin(), v.end());
  double total_t = 0.0;
  for (double ti : dt) total_t += ti;
  rep.total_time_s = total_t;

  // steering rate check
  double delta_dot_abs_max = 0.0;
  for (size_t i = 0; i < xy.size(); ++i) {
    double delta = std::atan(lim.wheelbase_m * kappa[i]);
    double kappa_dot = v[i] * dkds[i];
    double delta_dot = lim.wheelbase_m * std::pow(std::cos(delta), 2.0) * kappa_dot;
    delta_dot_abs_max = std::max(delta_dot_abs_max, std::fabs(delta_dot));
  }
  rep.delta_dot_abs_max = delta_dot_abs_max;

  if (delta_dot_abs_max > lim.delta_dot_max_rad_s + 1e-9) {
    rep.ok = false;
    rep.reason = "steering_rate_violation";
    return rep;
  }

  // collision check
  if (occ) {
    std::vector<double> dist = distanceToObstacles(*occ, H, W, res_m);
    const double needed = lim.robot_radius_m + lim.clearance_margin_m;
    for (size_t i = 0; i < xy.size(); ++i) {
      int gx = static_cast<int>(std::floor((xy[i].x - origin.x) / res_m));
      int gy = static_cast<int>(std::floor((xy[i].y - origin.y) / res_m));
      if (gx < 0 || gx >= W || gy < 0 || gy >= H) {
        rep.ok = false;
        rep.reason = "path_outside_grid";
        return rep;
      }
      double c = dist[gy * W + gx];
      if (c < needed) {
        rep.ok = false;
        rep.reason = "collision_violation";
        return rep;
      }
    }
  }

  rep.ok = true;
  rep.reason = "ok";
  return rep;
}

int main() {
  // Demo: arc path
  std::vector<Point2> path;
  const int N = 200;
  const double R = 4.0;
  for (int i = 0; i < N; ++i) {
    double t = static_cast<double>(i) / (N - 1);
    double ang = 0.6 * t;
    path.push_back({R * std::sin(ang), R * (1.0 - std::cos(ang))});
  }

  // Demo grid: 10m x 10m at 0.05m resolution, one obstacle patch
  double res = 0.05;
  int W = static_cast<int>(10.0 / res);
  int H = static_cast<int>(10.0 / res);
  std::vector<uint8_t> occ(H * W, 0);

  auto idx = [&](int y, int x) { return y * W + x; };
  int ox = static_cast<int>(2.0 / res);
  int oy = static_cast<int>(1.0 / res);
  for (int y = std::max(0, oy - 3); y < std::min(H, oy + 3); ++y)
    for (int x = std::max(0, ox - 3); x < std::min(W, ox + 3); ++x)
      occ[idx(y, x)] = 1;

  Limits lim;
  Point2 origin{0.0, 0.0};

  Report rep = checkFeasible(path, &occ, H, W, res, origin, lim);
  std::cout << "ok=" << (rep.ok ? "true" : "false") << " reason=" << rep.reason << "\n";
  std::cout << "L=" << rep.path_length_m << " kappa_abs_max=" << rep.kappa_abs_max
            << " kappa_max=" << rep.kappa_max << "\n";
  std::cout << "v_min=" << rep.v_min << " v_max=" << rep.v_max
            << " delta_dot_abs_max=" << rep.delta_dot_abs_max
            << " T=" << rep.total_time_s << "\n";
  return 0;
}
      

Robotics libraries to know (Java): ROSJava for ROS integration, EJML for matrix computations, and JavaCPP presets to access native libraries (e.g., OpenCV) when needed. The code below is standalone and implements the same checks.

File: Chapter3_Lesson5.java

// Chapter3_Lesson5.java
/*
Autonomous Mobile Robots (Control Engineering major)
Chapter 3 — Nonholonomic Motion and Feasibility for AMR
Lesson 5 — Feasibility Checks for Candidate Paths

Java feasibility checker for a car-like robot. Self-contained (no external libs).
Checks:
  - curvature/steering bounds
  - steering-rate bound (via curvature derivative and speed profile)
  - speed & longitudinal acceleration bounds (forward-backward pass)
  - collision clearance on occupancy grid (disk approximation) using Dijkstra

Robotics ecosystem references (not required here):
  - ROSJava (path message integration)
  - JOML (vector math), EJML (linear algebra), JavaCPP OpenCV (distance transform)

Compile/run:
  javac Chapter3_Lesson5.java
  java Chapter3_Lesson5

Author: course content generator
*/

import java.util.*;

public class Chapter3_Lesson5 {

  static class Limits {
    double wheelbase_m = 0.33;
    double delta_max_rad = 0.45;
    double delta_dot_max_rad_s = 0.75;
    double v_max_m_s = 1.2;
    double a_long_max_m_s2 = 0.8;
    double a_lat_max_m_s2 = 1.5;
    double robot_radius_m = 0.25;
    double clearance_margin_m = 0.05;
  }

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

  static class Report {
    boolean ok;
    String reason;
    double pathLength;
    double kappaAbsMax;
    double kappaMax;
    double vMin;
    double vMax;
    double deltaDotAbsMax;
    double totalTime;
  }

  static double norm(Point2 a) { return Math.hypot(a.x, a.y); }
  static Point2 sub(Point2 a, Point2 b) { return new Point2(a.x - b.x, a.y - b.y); }
  static double cross(Point2 a, Point2 b) { return a.x * b.y - a.y * b.x; }

  static double[] arcLength(List<Point2> xy) {
    int n = xy.size();
    double[] s = new double[n];
    for (int i = 1; i < n; i++) {
      Point2 d = sub(xy.get(i), xy.get(i - 1));
      s[i] = s[i - 1] + norm(d);
    }
    return s;
  }

  static double[] curvatureDiscrete(List<Point2> xy) {
    int n = xy.size();
    double[] kappa = new double[n];
    double eps = 1e-12;
    for (int i = 1; i < n - 1; i++) {
      Point2 p0 = xy.get(i - 1), p1 = xy.get(i), p2 = xy.get(i + 1);
      Point2 a = sub(p1, p0);
      Point2 b = sub(p2, p1);
      Point2 c = sub(p2, p0);
      double la = norm(a), lb = norm(b), lc = norm(c);
      double area2 = cross(a, c);
      double denom = la * lb * lc + eps;
      kappa[i] = 2.0 * area2 / denom;
    }
    if (n >= 3) {
      kappa[0] = kappa[1];
      kappa[n - 1] = kappa[n - 2];
    }
    return kappa;
  }

  static double[] dkappaDs(double[] kappa, double[] s) {
    int n = kappa.length;
    double[] dk = new double[n];
    double eps = 1e-12;
    if (n < 3) return dk;
    for (int i = 1; i < n - 1; i++) {
      double denom = (s[i + 1] - s[i - 1]) + eps;
      dk[i] = (kappa[i + 1] - kappa[i - 1]) / denom;
    }
    dk[0] = (kappa[1] - kappa[0]) / ((s[1] - s[0]) + eps);
    dk[n - 1] = (kappa[n - 1] - kappa[n - 2]) / ((s[n - 1] - s[n - 2]) + eps);
    return dk;
  }

  static void speedProfile(double[] s, double[] kappa, Limits lim, double[] v, double[] dt) {
    int n = s.length;
    double[] vcap = new double[n];
    for (int i = 0; i < n; i++) vcap[i] = lim.v_max_m_s;

    for (int i = 0; i < n; i++) {
      double ak = Math.abs(kappa[i]);
      if (ak > 1e-9) {
        double vlat = Math.sqrt(Math.max(lim.a_lat_max_m_s2 / ak, 0.0));
        vcap[i] = Math.min(vcap[i], vlat);
      }
    }

    double[] w = new double[n];
    for (int i = 0; i < n; i++) {
      double vv = Math.min(vcap[i], lim.v_max_m_s);
      w[i] = vv * vv;
    }

    // forward
    for (int i = 0; i < n - 1; i++) {
      double ds = s[i + 1] - s[i];
      double wNext = w[i] + 2.0 * lim.a_long_max_m_s2 * Math.max(ds, 0.0);
      if (w[i + 1] > wNext) w[i + 1] = wNext;
    }
    // backward
    for (int i = n - 1; i >= 1; i--) {
      double ds = s[i] - s[i - 1];
      double wPrev = w[i] + 2.0 * lim.a_long_max_m_s2 * Math.max(ds, 0.0);
      if (w[i - 1] > wPrev) w[i - 1] = wPrev;
    }

    for (int i = 0; i < n; i++) v[i] = Math.sqrt(Math.max(w[i], 0.0));

    double eps = 1e-12;
    for (int i = 0; i < n - 1; i++) {
      double ds = s[i + 1] - s[i];
      double vAvg = 0.5 * (v[i] + v[i + 1]);
      dt[i] = ds / (vAvg + eps);
    }
  }

  // Multi-source Dijkstra distance-to-obstacle on 8-connected grid.
  static double[] distanceToObstacles(byte[] occ, int H, int W, double resM) {
    double INF = 1e18;
    double[] dist = new double[H * W];
    Arrays.fill(dist, INF);

    class Node {
      double d; int idx;
      Node(double d, int idx) { this.d = d; this.idx = idx; }
    }
    PriorityQueue<Node> pq = new PriorityQueue<>(Comparator.comparingDouble(a -> a.d));

    for (int i = 0; i < H * W; i++) {
      if (occ[i] == 1) {
        dist[i] = 0.0;
        pq.add(new Node(0.0, i));
      }
    }

    int[] dx = {1, -1, 0, 0,  1,  1, -1, -1};
    int[] dy = {0, 0, 1, -1, 1, -1,  1, -1};
    double[] w = {1, 1, 1, 1, Math.sqrt(2.0), Math.sqrt(2.0), Math.sqrt(2.0), Math.sqrt(2.0)};

    while (!pq.isEmpty()) {
      Node cur = pq.poll();
      if (cur.d > dist[cur.idx]) continue;
      int y = cur.idx / W;
      int x = cur.idx % W;
      for (int k = 0; k < 8; k++) {
        int nx = x + dx[k];
        int ny = y + dy[k];
        if (nx < 0 || nx >= W || ny < 0 || ny >= H) continue;
        int nidx = ny * W + nx;
        double nd = cur.d + w[k] * resM;
        if (nd < dist[nidx]) {
          dist[nidx] = nd;
          pq.add(new Node(nd, nidx));
        }
      }
    }
    return dist;
  }

  static Report checkFeasible(
      List<Point2> xy,
      byte[] occ, int H, int W,
      double resM, Point2 origin,
      Limits lim) {

    Report rep = new Report();
    if (xy.size() < 3) {
      rep.ok = false;
      rep.reason = "invalid_path";
      return rep;
    }

    double[] s = arcLength(xy);
    double[] kappa = curvatureDiscrete(xy);
    double[] dkds = dkappaDs(kappa, s);

    double kappaMax = Math.tan(lim.delta_max_rad) / lim.wheelbase_m;
    double kappaAbsMax = 0.0;
    for (double ki : kappa) kappaAbsMax = Math.max(kappaAbsMax, Math.abs(ki));

    rep.kappaMax = kappaMax;
    rep.kappaAbsMax = kappaAbsMax;
    rep.pathLength = s[s.length - 1];

    if (kappaAbsMax > kappaMax + 1e-9) {
      rep.ok = false;
      rep.reason = "curvature_limit_violation";
      return rep;
    }

    double[] v = new double[s.length];
    double[] dt = new double[s.length - 1];
    speedProfile(s, kappa, lim, v, dt);

    rep.vMin = Arrays.stream(v).min().orElse(0.0);
    rep.vMax = Arrays.stream(v).max().orElse(0.0);
    rep.totalTime = Arrays.stream(dt).sum();

    double deltaDotAbsMax = 0.0;
    for (int i = 0; i < s.length; i++) {
      double delta = Math.atan(lim.wheelbase_m * kappa[i]);
      double kappaDot = v[i] * dkds[i];
      double deltaDot = lim.wheelbase_m * Math.pow(Math.cos(delta), 2.0) * kappaDot;
      deltaDotAbsMax = Math.max(deltaDotAbsMax, Math.abs(deltaDot));
    }
    rep.deltaDotAbsMax = deltaDotAbsMax;

    if (deltaDotAbsMax > lim.delta_dot_max_rad_s + 1e-9) {
      rep.ok = false;
      rep.reason = "steering_rate_violation";
      return rep;
    }

    if (occ != null) {
      double[] dist = distanceToObstacles(occ, H, W, resM);
      double needed = lim.robot_radius_m + lim.clearance_margin_m;
      for (int i = 0; i < xy.size(); i++) {
        Point2 p = xy.get(i);
        int gx = (int)Math.floor((p.x - origin.x) / resM);
        int gy = (int)Math.floor((p.y - origin.y) / resM);
        if (gx < 0 || gx >= W || gy < 0 || gy >= H) {
          rep.ok = false;
          rep.reason = "path_outside_grid";
          return rep;
        }
        double c = dist[gy * W + gx];
        if (c < needed) {
          rep.ok = false;
          rep.reason = "collision_violation";
          return rep;
        }
      }
    }

    rep.ok = true;
    rep.reason = "ok";
    return rep;
  }

  public static void main(String[] args) {
    // Demo: arc path
    ArrayList<Point2> path = new ArrayList<>();
    int N = 200;
    double R = 4.0;
    for (int i = 0; i < N; i++) {
      double t = (double)i / (N - 1);
      double ang = 0.6 * t;
      path.add(new Point2(R * Math.sin(ang), R * (1.0 - Math.cos(ang))));
    }

    // Demo occupancy grid 10m x 10m at 0.05m resolution
    double res = 0.05;
    int W = (int)(10.0 / res);
    int H = (int)(10.0 / res);
    byte[] occ = new byte[H * W]; // 0 free, 1 obstacle

    int ox = (int)(2.0 / res);
    int oy = (int)(1.0 / res);
    for (int y = Math.max(0, oy - 3); y < Math.min(H, oy + 3); y++)
      for (int x = Math.max(0, ox - 3); x < Math.min(W, ox + 3); x++)
        occ[y * W + x] = 1;

    Limits lim = new Limits();
    Point2 origin = new Point2(0.0, 0.0);

    Report rep = checkFeasible(path, occ, H, W, res, origin, lim);
    System.out.println("ok=" + rep.ok + " reason=" + rep.reason);
    System.out.println("L=" + rep.pathLength + " kappaAbsMax=" + rep.kappaAbsMax + " kappaMax=" + rep.kappaMax);
    System.out.println("vMin=" + rep.vMin + " vMax=" + rep.vMax + " deltaDotAbsMax=" + rep.deltaDotAbsMax + " T=" + rep.totalTime);
  }
}
      

Robotics libraries to know (MATLAB): Robotics System Toolbox (ROS/ROS2 interface), Navigation Toolbox (maps and planners), and Image Processing Toolbox (distance transforms). The script below implements the feasibility gate and optionally creates a minimal Simulink model that calls it.

File: Chapter3_Lesson5.m

% Chapter3_Lesson5.m
% Autonomous Mobile Robots (Control Engineering major)
% Chapter 3 — Nonholonomic Motion and Feasibility for AMR
% Lesson 5 — Feasibility Checks for Candidate Paths
%
% This MATLAB script implements feasibility checks for a candidate path:
%  1) curvature/steering bound
%  2) steering rate bound (via dkappa/ds and time scaling)
%  3) speed and acceleration bounds (forward-backward pass)
%  4) collision clearance on occupancy grid (disk approximation) via bwdist
%
% Toolboxes that help (optional):
%  - Robotics System Toolbox / Navigation Toolbox (maps, planners)
%  - Image Processing Toolbox (bwdist distance transform)
%
% The code is written to be readable and course-aligned; students can refactor
% into functions/classes as an exercise.

clear; clc;

%% --- Limits (example) ---
lim.wheelbase_m          = 0.33;
lim.delta_max_rad        = 0.45;
lim.delta_dot_max_rad_s  = 0.75;
lim.v_max_m_s            = 1.2;
lim.a_long_max_m_s2      = 0.8;
lim.a_lat_max_m_s2       = 1.5;
lim.robot_radius_m       = 0.25;
lim.clearance_margin_m   = 0.05;

%% --- Candidate path (example: circular arc) ---
N = 200;
t = linspace(0,1,N)';
R = 4.0;
ang = 0.6*t;
xy = [R*sin(ang), R*(1-cos(ang))];

%% --- Occupancy grid (example) ---
res = 0.05;       % meters per cell
W = round(10.0/res);
H = round(10.0/res);
occ = zeros(H,W); % 1 obstacle, 0 free

% obstacle patch near (2,1)
ox = round(2.0/res);
oy = round(1.0/res);
occ(max(1,oy-3):min(H,oy+3), max(1,ox-3):min(W,ox+3)) = 1;

origin = [0,0];   % (x0,y0) world coordinates of cell (1,1)

%% --- Feasibility check ---
[ok, rep] = check_path_feasibility(xy, occ, res, origin, lim);
disp(ok);
disp(rep);

%% --- Simulink (optional): create a minimal model programmatically ---
% This section demonstrates how you could wrap feasibility checking into a
% MATLAB Function block inside Simulink. It is optional and may require
% Simulink installed and licensed.

% Uncomment to generate the model:
% create_simulink_feasibility_model();

%% ======= Functions =======

function [ok, rep] = check_path_feasibility(xy, occ, res, origin, lim)
  if size(xy,2) ~= 2 || size(xy,1) < 3
    ok = false; rep.reason = 'invalid_path_shape'; return;
  end

  s = arc_length(xy);
  kappa = curvature_discrete(xy);
  dkds = dkappa_ds(kappa, s);

  kappa_max = tan(lim.delta_max_rad)/lim.wheelbase_m;
  if max(abs(kappa)) > kappa_max + 1e-9
    ok = false;
    rep.reason = 'curvature_limit_violation';
    rep.kappa_max = kappa_max;
    rep.kappa_abs_max = max(abs(kappa));
    return;
  end

  [v, dt] = time_scaling(s, kappa, lim);

  delta = atan(lim.wheelbase_m * kappa);
  kappa_dot = v .* dkds;
  delta_dot = lim.wheelbase_m .* (cos(delta).^2) .* kappa_dot;

  if max(abs(delta_dot)) > lim.delta_dot_max_rad_s + 1e-9
    ok = false;
    rep.reason = 'steering_rate_violation';
    rep.delta_dot_abs_max = max(abs(delta_dot));
    rep.delta_dot_max = lim.delta_dot_max_rad_s;
    return;
  end

  if ~isempty(occ)
    [okcol, colrep] = collision_check_disk(xy, occ, res, origin, lim);
    if ~okcol
      ok = false;
      rep.reason = 'collision_violation';
      rep.collision = colrep;
      return;
    end
  else
    colrep.skipped = true;
  end

  ok = true;
  rep.reason = 'ok';
  rep.path_length_m = s(end);
  rep.kappa_abs_max = max(abs(kappa));
  rep.kappa_max = kappa_max;
  rep.v_min = min(v);
  rep.v_max = max(v);
  rep.delta_dot_abs_max = max(abs(delta_dot));
  rep.total_time_s = sum(dt);
  rep.collision = colrep;
end

function s = arc_length(xy)
  d = sqrt(sum(diff(xy,1,1).^2,2));
  s = [0; cumsum(d)];
end

function kappa = curvature_discrete(xy)
  n = size(xy,1);
  kappa = zeros(n,1);
  epsv = 1e-12;
  for i = 2:n-1
    p0 = xy(i-1,:); p1 = xy(i,:); p2 = xy(i+1,:);
    a = p1 - p0; c = p2 - p0; b = p2 - p1;
    la = norm(a); lb = norm(b); lc = norm(c);
    area2 = a(1)*c(2) - a(2)*c(1);
    denom = la*lb*lc + epsv;
    kappa(i) = 2*area2/denom;
  end
  kappa(1) = kappa(2);
  kappa(end) = kappa(end-1);
end

function dk = dkappa_ds(kappa, s)
  n = numel(kappa);
  dk = zeros(n,1);
  epsv = 1e-12;
  for i = 2:n-1
    dk(i) = (kappa(i+1)-kappa(i-1))/((s(i+1)-s(i-1))+epsv);
  end
  dk(1) = (kappa(2)-kappa(1))/((s(2)-s(1))+epsv);
  dk(end) = (kappa(end)-kappa(end-1))/((s(end)-s(end-1))+epsv);
end

function [v, dt] = time_scaling(s, kappa, lim)
  n = numel(s);
  ds = diff(s);
  vcap = lim.v_max_m_s * ones(n,1);

  for i = 1:n
    if abs(kappa(i)) > 1e-9
      vlat = sqrt(max(lim.a_lat_max_m_s2/abs(kappa(i)), 0));
      vcap(i) = min(vcap(i), vlat);
    end
  end

  w = min(vcap, lim.v_max_m_s).^2;

  % forward pass
  for i = 1:n-1
    w(i+1) = min(w(i+1), w(i) + 2*lim.a_long_max_m_s2*max(ds(i),0));
  end
  % backward pass
  for i = n-1:-1:1
    w(i) = min(w(i), w(i+1) + 2*lim.a_long_max_m_s2*max(ds(i),0));
  end

  v = sqrt(max(w,0));
  epsv = 1e-12;
  dt = zeros(n-1,1);
  for i = 1:n-1
    vavg = 0.5*(v(i)+v(i+1));
    dt(i) = ds(i)/(vavg+epsv);
  end
end

function [ok, rep] = collision_check_disk(xy, occ, res, origin, lim)
  needed = lim.robot_radius_m + lim.clearance_margin_m;

  % Distance to nearest obstacle: compute on "free" cells.
  % bwdist computes distance to nearest nonzero; so use occ==1 as "object".
  if exist('bwdist','file')
    dist_cells = bwdist(occ==1);
    dist_m = dist_cells * res;
  else
    % Fallback: chamfer distance using iterative relaxation
    dist_m = chamfer_distance(occ, res);
  end

  H = size(occ,1); W = size(occ,2);
  min_clear = inf; worst = 1;

  for i = 1:size(xy,1)
    gx = floor((xy(i,1)-origin(1))/res) + 1;
    gy = floor((xy(i,2)-origin(2))/res) + 1;
    if gx < 1 || gx > W || gy < 1 || gy > H
      ok = false; rep.reason='path_outside_grid'; rep.index=i; return;
    end
    c = dist_m(gy,gx);
    if c < min_clear
      min_clear = c; worst = i;
    end
    if c < needed
      ok = false; rep.reason='insufficient_clearance'; rep.index=i;
      rep.clearance_m = c; rep.needed_m = needed; return;
    end
  end

  ok = true;
  rep.min_clearance_m = min_clear;
  rep.worst_index = worst;
  rep.needed_m = needed;
end

function dist = chamfer_distance(occ, res)
  % Very simple chamfer-like distance (not exact Euclidean). Obstacles dist=0.
  [H,W] = size(occ);
  INF = 1e9;
  dist = INF*ones(H,W);
  dist(occ==1) = 0;

  % forward
  for y=2:H
    for x=2:W-1
      dist(y,x) = min([dist(y,x), dist(y-1,x)+1, dist(y,x-1)+1, dist(y-1,x-1)+sqrt(2), dist(y-1,x+1)+sqrt(2)]);
    end
  end
  % backward
  for y=H-1:-1:1
    for x=W-1:-1:2
      dist(y,x) = min([dist(y,x), dist(y+1,x)+1, dist(y,x+1)+1, dist(y+1,x+1)+sqrt(2), dist(y+1,x-1)+sqrt(2)]);
    end
  end
  dist = dist * res;
end

function create_simulink_feasibility_model()
  model = 'Chapter3_Lesson5_FeasibilityModel';
  if bdIsLoaded(model), close_system(model,0); end
  new_system(model); open_system(model);

  add_block('simulink/Sources/Constant', [model '/xy'], 'Value', 'xy', 'Position', [30 50 130 90]);
  add_block('simulink/Sources/Constant', [model '/occ'], 'Value', 'occ', 'Position', [30 120 130 160]);
  add_block('simulink/User-Defined Functions/MATLAB Function', [model '/FeasibilityCheck'], 'Position', [220 70 420 160]);
  add_block('simulink/Sinks/Display', [model '/Display'], 'Position', [520 95 620 135]);

  % MATLAB Function block code
  code = [
    "function ok = FeasibilityCheck(xy_in, occ_in)\n" + ...
    "%#codegen\n" + ...
    "% Minimal wrapper around check_path_feasibility (workspace function).\n" + ...
    "[ok, ~] = check_path_feasibility(xy_in, occ_in, " + num2str(0.05) + ", [0,0], struct(...\n" + ...
    " 'wheelbase_m',0.33,'delta_max_rad',0.45,'delta_dot_max_rad_s',0.75,'v_max_m_s',1.2,\n" + ...
    " 'a_long_max_m_s2',0.8,'a_lat_max_m_s2',1.5,'robot_radius_m',0.25,'clearance_margin_m',0.05));\n" + ...
    "end\n"
  ];
  set_param([model '/FeasibilityCheck'], 'Script', code);

  add_line(model, 'xy/1', 'FeasibilityCheck/1');
  add_line(model, 'occ/1', 'FeasibilityCheck/2');
  add_line(model, 'FeasibilityCheck/1', 'Display/1');

  save_system(model);
  fprintf('Created Simulink model: %s.slx\n', model);
end
      

Mathematica is useful for symbolic inspection (e.g., verifying curvature formulas) and rapid prototyping. The following is a plain-text notebook expression saved as Chapter3_Lesson5.nb.

File: Chapter3_Lesson5.nb

(* Chapter3_Lesson5.nb
   Autonomous Mobile Robots (Control Engineering major)
   Chapter 3 — Nonholonomic Motion and Feasibility for AMR
   Lesson 5 — Feasibility Checks for Candidate Paths

   This is a plain-text Mathematica notebook expression. Save as .nb and open.
*)

Notebook[{
  Cell["Chapter 3, Lesson 5 — Feasibility Checks for Candidate Paths", "Title"],
  Cell["This notebook implements curvature, time-scaling, and collision clearance checks for a 2D candidate path (disk footprint approximation on a grid).", "Text"],

  Cell["1. Limits and a candidate path", "Section"],
  Cell[BoxData[ToBoxes[
    lim = <|
      "wheelbase_m" -> 0.33,
      "delta_max_rad" -> 0.45,
      "deltaDot_max_rad_s" -> 0.75,
      "v_max_m_s" -> 1.2,
      "a_long_max_m_s2" -> 0.8,
      "a_lat_max_m_s2" -> 1.5,
      "robot_radius_m" -> 0.25,
      "clearance_margin_m" -> 0.05
    |>;
  ]], "Input"],

  Cell[BoxData[ToBoxes[
    n = 200;
    t = Subdivide[0, 1, n - 1];
    R = 4.0;
    ang = 0.6 t;
    xy = Transpose[{R Sin[ang], R (1 - Cos[ang])}];
  ]], "Input"],

  Cell["2. Arc length and discrete curvature", "Section"],
  Cell[BoxData[ToBoxes[
    arcLength[pts_] := Prepend[Accumulate[Norm /@ Differences[pts]], 0.0];
    curvatureDiscrete[pts_] := Module[{n = Length[pts], k = ConstantArray[0.0, Length[pts]], eps = 10^-12},
      Do[
        With[{p0 = pts[[i - 1]], p1 = pts[[i]], p2 = pts[[i + 1]]},
          Module[{a = p1 - p0, b = p2 - p1, c = p2 - p0, la, lb, lc, area2, denom},
            la = Norm[a]; lb = Norm[b]; lc = Norm[c];
            area2 = Det[{a, c}];
            denom = la lb lc + eps;
            k[[i]] = 2.0 area2/denom;
          ]
        ],
        {i, 2, n - 1}
      ];
      If[n >= 3, k[[1]] = k[[2]]; k[[n]] = k[[n - 1]]];
      k
    ];
  ]], "Input"],

  Cell[BoxData[ToBoxes[
    s = arcLength[xy];
    kappa = curvatureDiscrete[xy];
    kappaMax = Tan[lim["delta_max_rad"]]/lim["wheelbase_m"];
    {Max[Abs[kappa]], kappaMax}
  ]], "Input"],

  Cell["3. Time scaling (speed profile) with lateral and longitudinal constraints", "Section"],
  Cell[BoxData[ToBoxes[
    dkappaDs[k_, s_] := Module[{n = Length[k], dk = ConstantArray[0.0, Length[k]], eps = 10^-12},
      Do[
        dk[[i]] = (k[[i + 1]] - k[[i - 1]])/((s[[i + 1]] - s[[i - 1]]) + eps),
        {i, 2, n - 1}
      ];
      dk[[1]] = (k[[2]] - k[[1]])/((s[[2]] - s[[1]]) + eps);
      dk[[n]] = (k[[n]] - k[[n - 1]])/((s[[n]] - s[[n - 1]]) + eps);
      dk
    ];
    timeScaling[s_, k_, lim_] := Module[{n = Length[s], ds, vcap, w, i, v, dt, eps = 10^-12},
      ds = Differences[s];
      vcap = ConstantArray[lim["v_max_m_s"], n];
      Do[
        If[Abs[k[[i]]] > 10^-9,
          vcap[[i]] = Min[vcap[[i]], Sqrt[Max[lim["a_lat_max_m_s2"]/Abs[k[[i]]], 0.0]]]
        ],
        {i, 1, n}
      ];
      w = vcap^2;
      Do[w[[i + 1]] = Min[w[[i + 1]], w[[i]] + 2 lim["a_long_max_m_s2"] Max[ds[[i]], 0.0]], {i, 1, n - 1}];
      Do[w[[i]] = Min[w[[i]], w[[i + 1]] + 2 lim["a_long_max_m_s2"] Max[ds[[i]], 0.0]], {i, n - 1, 1, -1}];
      v = Sqrt[Max[#, 0.0]] & /@ w;
      dt = Table[ ds[[i]] / (0.5 (v[[i]] + v[[i + 1]]) + eps), {i, 1, n - 1} ];
      <|"v" -> v, "dt" -> dt|>
    ];
  ]], "Input"],

  Cell[BoxData[ToBoxes[
    ts = timeScaling[s, kappa, lim];
    v = ts["v"]; dt = ts["dt"];
    totalT = Total[dt];
    {Min[v], Max[v], totalT}
  ]], "Input"],

  Cell["4. Steering-rate check", "Section"],
  Cell[BoxData[ToBoxes[
    dkds = dkappaDs[kappa, s];
    delta = ArcTan[lim["wheelbase_m"] kappa];
    kappaDot = v dkds;
    deltaDot = lim["wheelbase_m"] (Cos[delta]^2) kappaDot;
    {Max[Abs[deltaDot]], lim["deltaDot_max_rad_s"]}
  ]], "Input"],

  Cell["5. Collision clearance on an occupancy grid (disk approximation)", "Section"],
  Cell[BoxData[ToBoxes[
    res = 0.05;
    W = Round[10/res]; H = Round[10/res];
    occ = ConstantArray[0, {H, W}];
    ox = Round[2/res]; oy = Round[1/res];
    Do[occ[[y, x]] = 1, {y, Max[1, oy - 3], Min[H, oy + 3]}, {x, Max[1, ox - 3], Min[W, ox + 3]}];

    distCells = ImageData@DistanceTransform[Image[Boole[occ == 0]]];
    distM = distCells res;

    needed = lim["robot_radius_m"] + lim["clearance_margin_m"];
    origin = {0.0, 0.0};
    toGrid[p_] := {Floor[(p[[1]] - origin[[1]])/res] + 1, Floor[(p[[2]] - origin[[2]])/res] + 1};
    clearances = Table[
      With[{g = toGrid[xy[[i]]]}, distM[[g[[2]], g[[1]]]]],
      {i, 1, Length[xy]}
    ];
    {Min[clearances], needed}
  ]], "Input"],

  Cell["6. Overall feasibility verdict", "Section"],
  Cell[BoxData[ToBoxes[
    feasibleQ = (Max[Abs[kappa]] <= kappaMax) && (Max[Abs[deltaDot]] <= lim["deltaDot_max_rad_s"]) && (Min[clearances] >= needed);
    feasibleQ
  ]], "Input"]
}]
      

Problem 1 (Curvature bound from steering bound): Assume a car-like robot with wheelbase \( \ell \) and steering bound \( |\delta(t)| \le \delta_{\max} \). Show that any trackable arc-length parameterized path must satisfy \( |\kappa(s)| \le \tan(\delta_{\max})/\ell \).

Solution: From Proposition 1, \( \kappa(s) = \tan(\delta(s))/\ell \). Since \( |\delta(s)| \le \delta_{\max} \), we have \( |\tan(\delta(s))| \le \tan(\delta_{\max}) \) (monotonicity on \( (-\pi/2,\pi/2) \)), hence \( |\kappa(s)| \le \tan(\delta_{\max})/\ell \). □

Problem 2 (Steering-rate coupling to speed): Assume curvature profile \( \kappa(s) \) is differentiable and speed profile is \( v(t) \). Derive a sufficient feasibility inequality involving \( v(t)\kappa'(s(t)) \) under \( |\dot{\delta}(t)| \le \dot{\delta}_{\max} \) and \( |\delta(t)| \le \delta_{\max} \).

Solution: Using \( \dot{\delta}(t) = \ell\cos^2(\delta(t))\,v(t)\,\kappa'(s(t)) \) and \( \cos^2(\delta(t)) \ge \cos^2(\delta_{\max}) \), a sufficient condition is

\[ |v(t)\kappa'(s(t))| \le \frac{\dot{\delta}_{\max}}{\ell\,\cos^2(\delta_{\max})} \]

Problem 3 (Lateral acceleration speed limit): Show that the constraint \( |a_y(t)| \le a_{y,\max} \) with \( a_y(t) = v(t)^2\kappa(s(t)) \) implies \( |v(t)| \le \sqrt{a_{y,\max}/|\kappa(s(t))|} \) when \( |\kappa| > 0 \).

Solution: Immediate rearrangement: \( |v^2\kappa| \le a_{y,\max} \Rightarrow v^2 \le a_{y,\max}/|\kappa| \) for \( |\kappa| > 0 \), then take square roots. □

Problem 4 (Sampling-based clearance guarantee): Let \( d(p,\mathcal{O}) \) be distance to obstacles and assume a disk robot of radius \( r \). Suppose the path is sampled at points \( p_i \) such that for each interval \( [s_i,s_{i+1}] \) we have an upper bound \( \|p(s)-p_i\| \le \Delta \) for all \( s \in [s_i,s_{i+1}] \). Prove that if \( d(p_i,\mathcal{O}) \ge r + m + \Delta \), then \( d(p(s),\mathcal{O}) \ge r + m \) throughout the interval.

Solution: By Lemma 1 (1-Lipschitz), for any \( s \in [s_i,s_{i+1}] \), \( d(p(s),\mathcal{O}) \ge d(p_i,\mathcal{O}) - \|p(s)-p_i\| \). Using the assumed bounds, \( d(p(s),\mathcal{O}) \ge (r+m+\Delta) - \Delta = r+m \). □

Problem 5 (Feasibility-gate design): Propose an order of checks to reject infeasible candidates quickly, and justify the order based on computational cost and rejection power.

Solution: First reject by curvature (\( |\kappa| \)) because it is cheap and immediately eliminates many candidates. Next compute time-scaling and reject by acceleration limits (still cheap). Then reject by steering-rate (requires \( v(s) \) and \( \kappa'(s) \), moderate cost). Finally run collision clearance against a distance field (often the most expensive if map queries dominate).

flowchart TD
  P0["Sample points p_i"] --> P1["Compute clearance d_i"]
  P1 --> P2["Pick bound Delta for interval"]
  P2 --> P3["Require d_i >= r + margin + Delta"]
  P3 --> P4["Then all points between samples satisfy d >= r + margin"]
        

We converted candidate-path feasibility into verifiable mathematical constraints on curvature \( \kappa(s) \), its derivative \( \kappa'(s) \), and a feasible speed profile \( v(s) \) obtained by time-scaling under lateral and longitudinal acceleration limits. We then formalized collision feasibility via distance-to-obstacle constraints and proved a conservative sampling guarantee using the 1-Lipschitz property of distance functions. The resulting feasibility gate is implementable across common AMR software stacks and acts as a principled bridge between geometric planning and executable controls.