After this lesson, you should be able to:

• Define curvature-bounded feasibility for car-like robots and state the canonical boundary-value problem in \( SE(2) \).
• Explain why shortest curvature-bounded paths are concatenations of constant-curvature arcs and straight segments.
• Enumerate the Dubins families \( \{\mathrm{LSL}, \mathrm{RSR}, \mathrm{LSR}, \mathrm{RSL}, \mathrm{RLR}, \mathrm{LRL}\} \) and interpret their geometry.
• Describe how allowing reverse motion expands the family to Reeds-Shepp paths, and how symmetry reduces the search.
• Implement Dubins shortest paths (and a reverse-only variant) and sample them for downstream feasibility checks (Lesson 5).

Assumed background (from earlier lessons): the Ackermann kinematic model and curvature limit interpretation. We will rely on the curvature bound from Chapter 3, Lesson 2: \( |\kappa| \le \kappa_{\max} \) with \( \kappa_{\max} = 1/R_{\min} \).

A car-like robot configuration is \( q = (x, y, \theta) \in SE(2) \). Using arc-length parameter \( s \) (so \( \mathrm{d}s \ge 0 \) along motion), the curvature-bounded kinematics can be written as

\[ \frac{\mathrm{d}x}{\mathrm{d}s} = \cos\theta,\quad \frac{\mathrm{d}y}{\mathrm{d}s} = \sin\theta,\quad \frac{\mathrm{d}\theta}{\mathrm{d}s} = \kappa(s),\quad |\kappa(s)| \le \frac{1}{R_{\min}}. \]

A path \( \gamma: [0, L] \to SE(2) \) is feasible if it satisfies the above ODEs for some measurable curvature function \( \kappa(s) \) and boundary conditions \( \gamma(0) = q_0 \), \( \gamma(L) = q_1 \). Among feasible paths, a central question is: which families contain the shortest path?

We will study the minimum-length problem:

\[ \min_{\kappa(\cdot),\,L} \; L \quad \text{subject to}\quad \gamma(0)=q_0,\; \gamma(L)=q_1,\; |\kappa(s)| \le \frac{1}{R_{\min}}. \]

This problem is nonconvex, but it admits a remarkably compact solution structure (Dubins, 1957).

The curvature-bounded feasibility problem is invariant under rigid transformations in the plane and admits a useful normalization by \( R_{\min} \).

Let a rigid transform act on configurations as \( T(q) = (R\,p + t,\, \theta + \phi) \), where \( p=(x,y) \), \( R \) is a planar rotation by \( \phi \), and \( t \) is a translation. If \( \gamma(s) \) is feasible from \( q_0 \) to \( q_1 \), then \( T\circ\gamma(s) \) is feasible from \( T(q_0) \) to \( T(q_1) \) with the same curvature profile.

Proposition (Rigid-motion invariance).

If \( \gamma(s) \) satisfies the curvature-bounded ODEs, then so does \( \tilde\gamma(s)=T(\gamma(s)) \).

Proof. Since the rotation \( R \) preserves norms and angles, we have \( \frac{\mathrm{d}}{\mathrm{d}s}(R p(s)+t) = R p'(s) \). Because \( p'(s) = (\cos\theta,\sin\theta) \), we obtain \( R p'(s) = (\cos(\theta+\phi),\sin(\theta+\phi)) \). Also \( \tilde\theta(s)=\theta(s)+\phi \) implies \( \tilde\theta'(s)=\theta'(s)=\kappa(s) \), hence \( |\tilde\kappa(s)|=|\kappa(s)| \le 1/R_{\min} \). Therefore feasibility is preserved. ■

Normalization. Scale lengths by \( R_{\min} \). Define \( \bar x = x/R_{\min} \), \( \bar y = y/R_{\min} \). Then the constraint becomes \( |\bar\kappa(s)| \le 1 \). This reduction means we can solve once for unit radius and rescale by \( R_{\min} \).

flowchart TD
  A["Given q0=(x0,y0,theta0), q1=(x1,y1,theta1), Rmin"] --> B["Translate/rotate so q0 -> origin, heading -> 0"]
  B --> C["Scale: (x,y) := (x/Rmin, y/Rmin) so |kappa| <= 1"]
  C --> D["Solve canonical problem (unit radius) over candidate families"]
  D --> E["Rescale lengths by Rmin and transform back to world frame"]
        

The Dubins problem restricts the vehicle to forward motion (arc-length increases along the vehicle's forward direction). In normalized form, set the control \( u(s)=R_{\min}\,\kappa(s) \) so that \( |u(s)| \le 1 \). The dynamics become:

\[ \frac{\mathrm{d}x}{\mathrm{d}s} = \cos\theta,\quad \frac{\mathrm{d}y}{\mathrm{d}s} = \sin\theta,\quad \frac{\mathrm{d}\theta}{\mathrm{d}s} = u(s),\quad |u(s)| \le 1. \]

The cost is \( \int_0^L 1\,\mathrm{d}s = L \). This is a minimum-time optimal control problem with time variable equal to arc length.

Theorem (Dubins structure). Any shortest feasible path is a concatenation of at most three primitives drawn from: \( L \) (max-left turn, \( u=+1 \)), \( R \) (max-right turn, \( u=-1 \)), and \( S \) (straight, \( u=0 \)), and must be one of the six types: \( \{\mathrm{LSL},\mathrm{RSR},\mathrm{LSR},\mathrm{RSL},\mathrm{RLR},\mathrm{LRL}\} \).

Proof sketch via Pontryagin’s Maximum Principle (PMP).

Introduce costates \( \lambda_x,\lambda_y,\lambda_\theta \) and Hamiltonian

\[ H = \lambda_x\cos\theta + \lambda_y\sin\theta + \lambda_\theta u + 1. \]

The costate dynamics are (from \( \dot\lambda = -\partial H/\partial x \)):

\[ \frac{\mathrm{d}\lambda_x}{\mathrm{d}s} = 0,\quad \frac{\mathrm{d}\lambda_y}{\mathrm{d}s} = 0,\quad \frac{\mathrm{d}\lambda_\theta}{\mathrm{d}s} = \lambda_x\sin\theta - \lambda_y\cos\theta. \]

The control appears linearly in \( H \) through \( \lambda_\theta u \). Therefore, minimizing \( H \) over \( u \in [-1,1] \) yields the bang-bang rule:

\[ u^\star(s) = \begin{cases} -1 & \text{if } \lambda_\theta(s) > 0,\\ +1 & \text{if } \lambda_\theta(s) < 0,\\ 0 & \text{if } \lambda_\theta(s) = 0 \text{ on an interval (singular arc).} \end{cases} \]

Hence optimal motion uses only max-curvature turns or (possibly) a singular arc with \( u=0 \), i.e., straight motion. One shows that a singular arc requires \( \lambda_\theta = 0 \) and \( \mathrm{d}\lambda_\theta/\mathrm{d}s = 0 \) on an interval, which forces \( \lambda_x\sin\theta - \lambda_y\cos\theta = 0 \), implying constant heading and thus a straight line segment. Finally, analysis of the switching function \( \lambda_\theta \) shows that optimal trajectories have at most two switches, giving at most three segments and yielding the six canonical types. ■

The key engineering takeaway: to search for the shortest curvature-bounded path, it suffices to evaluate a finite family of analytic candidates.

For a pose \( q=(x,y,\theta) \) and radius \( R_{\min} \), the instantaneous left/right turning circle centers are:

\[ c_L(q) = \begin{bmatrix} x - R_{\min}\sin\theta \\ y + R_{\min}\cos\theta \end{bmatrix}, \qquad c_R(q) = \begin{bmatrix} x + R_{\min}\sin\theta \\ y - R_{\min}\cos\theta \end{bmatrix}. \]

Each Dubins candidate corresponds to selecting a pair of circles (left or right at start and goal) and connecting them by a straight tangent segment (CSC types), or by a third circle arc (CCC types). In practice, an implementation often proceeds as:

flowchart TD
  Q0["Start pose q0"] --> CL0["Left circle center \ncL(q0)"]
  Q0 --> CR0["Right circle center \ncR(q0)"]
  Q1["Goal pose q1"] --> CL1["Left circle center \ncL(q1)"]
  Q1 --> CR1["Right circle center \ncR(q1)"]
  CL0 --> T1["Compute tangents: external (LL) and internal (LR)"]
  CR0 --> T1
  CL1 --> T1
  CR1 --> T1
  T1 --> EVAL["Evaluate path length for each family; pick minimum"]
        

The closed-form approach used in the provided code evaluates the six Dubins families using a normalized canonical frame. Let \( q_0 \) be transformed to the origin with heading 0, and scale by \( R_{\min} \). Let \( (x, y, \phi) \) be the goal in that frame, and define:

\[ d = \sqrt{x^2 + y^2},\quad \vartheta = \operatorname{atan2}(y, x),\quad \alpha = \operatorname{mod}( -\vartheta, 2\pi),\quad \beta = \operatorname{mod}(\phi - \vartheta, 2\pi). \]

Each family yields parameters \( (t,p,q) \) such that the path is Turn(\( t \)) → Straight/Turn(\( p \)) → Turn(\( q \)). Feasibility appears as nonnegativity constraints such as \( p^2 \ge 0 \) and trigonometric bounds (for CCC types).

For example, one of the CSC candidates (LSL) has:

\[ p^2 = 2 + d^2 - 2\cos(\alpha-\beta) + 2d(\sin\alpha - \sin\beta),\quad p = \sqrt{p^2}, \]

with corresponding angles \( t = \operatorname{mod}(-\alpha + \operatorname{atan2}(\cos\beta - \cos\alpha,\, d + \sin\alpha - \sin\beta), 2\pi) \) and \( q = \operatorname{mod}(\beta - \operatorname{atan2}(\cos\beta - \cos\alpha,\, d + \sin\alpha - \sin\beta), 2\pi) \). The other families are analogous and are implemented in the code attachments.

In many ground robots (especially low-speed indoor AMRs), reverse motion is feasible. A simple modeling step is to allow the signed speed to change sign, meaning that arc length is no longer monotone along the vehicle's forward direction. Reeds and Shepp (1990) proved that the shortest path in this setting is still composed of constant-curvature arcs and straight segments, but the optimal word may include direction switches.

A convenient formulation uses time \( t \) with bounded speed and steering:

\[ \dot x = v\cos\theta,\quad \dot y = v\sin\theta,\quad \dot\theta = v\kappa,\quad |\kappa| \le \frac{1}{R_{\min}},\quad v \in [-v_{\max}, v_{\max}]. \]

For fixed \( |v|=v_{\max} \), minimizing time is equivalent to minimizing length, but the sign of \( v \) introduces additional switching structure. Reeds-Shepp paths can be classified into a finite set of families (often represented as words over \( \{L,R,S\} \) with forward/backward markers).

Engineering note. Full Reeds-Shepp enumeration is longer than Dubins (dozens of symmetric cases). In practice, robotics libraries (e.g., OMPL) provide thoroughly tested implementations. This lesson’s code includes:

• A complete from-scratch Dubins solver (all six families).
• A “backward-only” Dubins variant (drive in reverse everywhere) as a simple baseline.
• Optional OMPL snippets showing how to compute exact Reeds-Shepp lengths and sampling.

Real Ackermann steering often has a bounded steering rate \( |\dot\delta| \le \dot\delta_{\max} \), which translates into a curvature rate bound. The Dubins/Reeds-Shepp families allow instantaneous jumps in curvature (from \( 0 \) to \( \pm 1/R_{\min} \)), which is a good low-level planning abstraction but may need smoothing before tracking.

A classical continuous-curvature primitive is the Euler spiral (clothoid), whose curvature varies linearly with arc length:

\[ \kappa(s) = \kappa_0 + a s,\qquad \theta(s) = \theta_0 + \int_0^s \kappa(\sigma)\,\mathrm{d}\sigma = \theta_0 + \kappa_0 s + \frac{a}{2}s^2. \]

Clothoids are typically used as transition curves to achieve \( G^2 \) continuity (continuous curvature). We will revisit practical smoothing and tracking in Chapter 15 (local motion generation).

The following implementations compute the shortest Dubins path and sample it for downstream collision/feasibility checks. They are intentionally self-contained and rely only on standard numerical libraries (or optional robotics libraries).

8.1 Python (NumPy/Matplotlib; optional OMPL)

File: Chapter3_Lesson3.py

# Chapter3_Lesson3.py
# Autonomous Mobile Robots - Chapter 3 Lesson 3
# Feasible Path Families for Car-Like Robots (Dubins + simple reverse-only baseline)
#
# This script implements a numerically robust Dubins shortest-path solver from scratch
# (six families: LSL, RSR, LSR, RSL, RLR, LRL), plus sampling utilities.
#
# Optional note: for full Reeds-Shepp (forward+reverse with direction switches) you may use OMPL,
# but that is not required for this lesson.

from __future__ import annotations

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

import numpy as np


def mod2pi(angle: float) -> float:
    """Map angle to [0, 2*pi)."""
    a = angle % (2.0 * math.pi)
    return a


def angdiff(a: float, b: float) -> float:
    """Shortest signed difference a-b in [-pi, pi)."""
    d = (a - b + math.pi) % (2.0 * math.pi) - math.pi
    return d


@dataclass(frozen=True)
class Pose2:
    x: float
    y: float
    theta: float  # heading [rad]


@dataclass
class DubinsPath:
    path_type: str                  # e.g., "LSL"
    params: Tuple[float, float, float]  # (t, p, q) in normalized units (angles for turns, length for straight)
    R_min: float                    # minimum turning radius

    @property
    def length(self) -> float:
        t, p, q = self.params
        return self.R_min * (t + p + q)


# -----------------------------
# Canonical frame normalization
# -----------------------------

def to_canonical(q0: Pose2, q1: Pose2, R_min: float) -> Tuple[float, float, float]:
    """
    Transform so that q0 becomes (0,0,0), scale by R_min:
      x' = (cos th0 * dx + sin th0 * dy)/R_min
      y' = (-sin th0 * dx + cos th0 * dy)/R_min
      phi = wrap(th1 - th0)
    Returns (x, y, phi) in canonical frame.
    """
    dx = q1.x - q0.x
    dy = q1.y - q0.y
    c0 = math.cos(q0.theta)
    s0 = math.sin(q0.theta)

    x = (c0 * dx + s0 * dy) / R_min
    y = (-s0 * dx + c0 * dy) / R_min
    phi = mod2pi(q1.theta - q0.theta)
    return x, y, phi


def dubins_alpha_beta_d(x: float, y: float, phi: float) -> Tuple[float, float, float]:
    """
    Standard Dubins reduction:
      d = sqrt(x^2+y^2)
      theta = atan2(y,x)
      alpha = wrap(-theta)
      beta  = wrap(phi - theta)
    """
    d = math.hypot(x, y)
    theta = math.atan2(y, x) if d > 0 else 0.0
    alpha = mod2pi(-theta)
    beta = mod2pi(phi - theta)
    return alpha, beta, d


# ---------------------------------
# Candidate formulas (Dubins paths)
# ---------------------------------
# These are the standard closed-forms used in many reference implementations.
# Feasibility is expressed via nonnegativity constraints on derived squared lengths.

def _LSL(alpha: float, beta: float, d: float) -> Optional[Tuple[float, float, float]]:
    sa, sb = math.sin(alpha), math.sin(beta)
    ca, cb = math.cos(alpha), math.cos(beta)
    cab = math.cos(alpha - beta)

    tmp0 = d + sa - sb
    p2 = 2.0 + d * d - 2.0 * cab + 2.0 * d * (sa - sb)
    if p2 < 0.0:
        return None
    p = math.sqrt(p2)
    tmp1 = math.atan2(cb - ca, tmp0)
    t = mod2pi(-alpha + tmp1)
    q = mod2pi(beta - tmp1)
    return t, p, q


def _RSR(alpha: float, beta: float, d: float) -> Optional[Tuple[float, float, float]]:
    sa, sb = math.sin(alpha), math.sin(beta)
    ca, cb = math.cos(alpha), math.cos(beta)
    cab = math.cos(alpha - beta)

    tmp0 = d - sa + sb
    p2 = 2.0 + d * d - 2.0 * cab + 2.0 * d * (-sa + sb)
    if p2 < 0.0:
        return None
    p = math.sqrt(p2)
    tmp1 = math.atan2(ca - cb, tmp0)
    t = mod2pi(alpha - tmp1)
    q = mod2pi(-beta + tmp1)
    return t, p, q


def _LSR(alpha: float, beta: float, d: float) -> Optional[Tuple[float, float, float]]:
    sa, sb = math.sin(alpha), math.sin(beta)
    ca, cb = math.cos(alpha), math.cos(beta)
    cab = math.cos(alpha - beta)

    p2 = -2.0 + d * d + 2.0 * cab + 2.0 * d * (sa + sb)
    if p2 < 0.0:
        return None
    p = math.sqrt(p2)
    tmp0 = math.atan2(-ca - cb, d + sa + sb)
    tmp1 = math.atan2(-2.0, p)
    t = mod2pi(-alpha + tmp0 - tmp1)
    q = mod2pi(-beta + tmp0 - tmp1)
    return t, p, q


def _RSL(alpha: float, beta: float, d: float) -> Optional[Tuple[float, float, float]]:
    sa, sb = math.sin(alpha), math.sin(beta)
    ca, cb = math.cos(alpha), math.cos(beta)
    cab = math.cos(alpha - beta)

    p2 = -2.0 + d * d + 2.0 * cab - 2.0 * d * (sa + sb)
    if p2 < 0.0:
        return None
    p = math.sqrt(p2)
    tmp0 = math.atan2(ca + cb, d - sa - sb)
    tmp1 = math.atan2(2.0, p)
    t = mod2pi(alpha - tmp0 + tmp1)
    q = mod2pi(beta - tmp0 + tmp1)
    return t, p, q


def _RLR(alpha: float, beta: float, d: float) -> Optional[Tuple[float, float, float]]:
    sa, sb = math.sin(alpha), math.sin(beta)
    ca, cb = math.cos(alpha), math.cos(beta)
    cab = math.cos(alpha - beta)

    tmp0 = (6.0 - d * d + 2.0 * cab + 2.0 * d * (sa - sb)) / 8.0
    if abs(tmp0) > 1.0:
        return None
    p = mod2pi(2.0 * math.pi - math.acos(tmp0))
    tmp1 = math.atan2(ca - cb, d - sa + sb)
    t = mod2pi(alpha - tmp1 + p / 2.0)
    q = mod2pi(alpha - beta - t + p)
    return t, p, q


def _LRL(alpha: float, beta: float, d: float) -> Optional[Tuple[float, float, float]]:
    # Symmetry: LRL(alpha,beta,d) == RLR(-alpha,-beta,d) with swapped turns
    return _RLR(mod2pi(-alpha), mod2pi(-beta), d)


CANDIDATES: Dict[str, callable] = {
    "LSL": _LSL,
    "RSR": _RSR,
    "LSR": _LSR,
    "RSL": _RSL,
    "RLR": _RLR,
    "LRL": _LRL,
}


# -------------------------
# Validation by propagation
# -------------------------

def _segment_unit(pose: Pose2, seg_type: str, seg_len: float) -> Pose2:
    """
    Propagate one segment in normalized units:
      - Turn segments: seg_len is turn angle [rad], radius 1
      - Straight: seg_len is distance
    """
    x, y, th = pose.x, pose.y, pose.theta

    if seg_type == "S":
        x += seg_len * math.cos(th)
        y += seg_len * math.sin(th)
        return Pose2(x, y, th)

    if seg_type == "L":
        x += math.sin(th + seg_len) - math.sin(th)
        y += -math.cos(th + seg_len) + math.cos(th)
        th = mod2pi(th + seg_len)
        return Pose2(x, y, th)

    if seg_type == "R":
        x += -math.sin(th - seg_len) + math.sin(th)
        y += math.cos(th - seg_len) - math.cos(th)
        th = mod2pi(th - seg_len)
        return Pose2(x, y, th)

    raise ValueError(f"Unknown seg_type: {seg_type}")


def _endpoint_error(alpha: float, d: float, beta: float, path_type: str, params: Tuple[float, float, float]) -> float:
    """
    Validate a candidate by simulating in canonical coordinates:
      start pose is (0,0,alpha) in rotated frame, goal is (d,0,beta).
    """
    t, p, q = params
    segs = list(path_type)
    lengths = [t, p, q]
    pose = Pose2(0.0, 0.0, alpha)
    for st, sl in zip(segs, lengths):
        pose = _segment_unit(pose, "S" if st == "S" else st, sl)
    pos_err = math.hypot(pose.x - d, pose.y)
    ang_err = abs(angdiff(pose.theta, beta))
    return pos_err + ang_err


# -----------------------
# Main shortest-path API
# -----------------------

def dubins_shortest_path(q0: Pose2, q1: Pose2, R_min: float) -> DubinsPath:
    """
    Compute the shortest Dubins path from q0 to q1 with curvature bound 1/R_min.

    Returns a DubinsPath containing:
      - path_type: one of "LSL","RSR","LSR","RSL","RLR","LRL"
      - params (t,p,q) in normalized units
      - R_min for scaling
    """
    if R_min <= 0:
        raise ValueError("R_min must be positive.")

    x, y, phi = to_canonical(q0, q1, R_min)
    alpha, beta, d = dubins_alpha_beta_d(x, y, phi)

    best: Optional[DubinsPath] = None
    best_len = float("inf")

    for path_type, fn in CANDIDATES.items():
        prm = fn(alpha, beta, d)
        if prm is None:
            continue

        # Validate numerically (useful for robustness against edge cases)
        err = _endpoint_error(alpha, d, beta, path_type, prm)
        if err > 1e-6:
            continue

        L = R_min * sum(prm)
        if L < best_len:
            best_len = L
            best = DubinsPath(path_type=path_type, params=prm, R_min=R_min)

    if best is None:
        raise RuntimeError("No feasible Dubins path found (unexpected for Dubins problem).")
    return best


# --------------------------------------
# Sampling the path back in world frame
# --------------------------------------

def sample_dubins(q0: Pose2, path: DubinsPath, step: float = 0.05) -> List[Pose2]:
    """
    Sample a DubinsPath into a list of world-frame poses.
    step is an approximate spatial step in world units.
    """
    if step <= 0:
        raise ValueError("step must be positive")

    t, p, q = path.params
    seg_types = list(path.path_type)
    seg_lengths = [t, p, q]

    points: List[Pose2] = [Pose2(q0.x, q0.y, mod2pi(q0.theta))]

    cur = Pose2(q0.x, q0.y, mod2pi(q0.theta))

    for st, sl in zip(seg_types, seg_lengths):
        if st == "S":
            # Straight length in world
            L = sl * path.R_min
            n = max(1, int(math.ceil(L / step)))
            ds = L / n
            for _ in range(n):
                cur = Pose2(
                    cur.x + ds * math.cos(cur.theta),
                    cur.y + ds * math.sin(cur.theta),
                    cur.theta,
                )
                points.append(cur)
        else:
            # Arc: sl is turn angle in rad (normalized), world arc length = R_min * sl
            arc_len = path.R_min * sl
            n = max(1, int(math.ceil(arc_len / step)))
            da = sl / n
            for _ in range(n):
                if st == "L":
                    cur = Pose2(
                        cur.x + path.R_min * (math.sin(cur.theta + da) - math.sin(cur.theta)),
                        cur.y + path.R_min * (-math.cos(cur.theta + da) + math.cos(cur.theta)),
                        mod2pi(cur.theta + da),
                    )
                else:  # "R"
                    cur = Pose2(
                        cur.x + path.R_min * (-math.sin(cur.theta - da) + math.sin(cur.theta)),
                        cur.y + path.R_min * (math.cos(cur.theta - da) - math.cos(cur.theta)),
                        mod2pi(cur.theta - da),
                    )
                points.append(cur)

    return points


# ------------------------
# Reverse-only (baseline)
# ------------------------

def dubins_backward_only(q0: Pose2, q1: Pose2, R_min: float) -> DubinsPath:
    """
    A simple baseline: drive backward everywhere.
    Driving backward with heading theta is equivalent to driving forward with heading theta+pi.
    We therefore solve a forward Dubins problem between 'flipped' headings.
    """
    q0b = Pose2(q0.x, q0.y, mod2pi(q0.theta + math.pi))
    q1b = Pose2(q1.x, q1.y, mod2pi(q1.theta + math.pi))
    return dubins_shortest_path(q0b, q1b, R_min)


# ------------------------
# Demonstration / testing
# ------------------------

def _demo() -> None:
    q0 = Pose2(0.0, 0.0, math.radians(10.0))
    q1 = Pose2(8.0, 4.0, math.radians(110.0))
    R_min = 2.0

    path = dubins_shortest_path(q0, q1, R_min)
    pts = sample_dubins(q0, path, step=0.05)

    print("Dubins shortest:")
    print("  type:", path.path_type)
    print("  params (t,p,q):", path.params)
    print("  length:", path.length)

    back = dubins_backward_only(q0, q1, R_min)
    print("Backward-only baseline:")
    print("  type:", back.path_type)
    print("  length:", back.length)

    # Save to CSV for plotting elsewhere if desired
    arr = np.array([[p.x, p.y, p.theta] for p in pts], dtype=float)
    np.savetxt("dubins_path.csv", arr, delimiter=",", header="x,y,theta", comments="")
    print("Wrote dubins_path.csv with", len(pts), "samples")

    # Optional visualization
    try:
        import matplotlib.pyplot as plt

        plt.figure()
        plt.plot(arr[:, 0], arr[:, 1])
        plt.scatter([q0.x, q1.x], [q0.y, q1.y])
        plt.axis("equal")
        plt.grid(True)
        plt.title("Dubins shortest path (sampled)")
        plt.xlabel("x")
        plt.ylabel("y")
        plt.show()
    except Exception as e:
        print("Matplotlib plot skipped:", e)


if __name__ == "__main__":
    _demo()
      

8.2 C++ (Standard Library; optional OMPL)

File: Chapter3_Lesson3.cpp

// Chapter3_Lesson3.cpp
// Autonomous Mobile Robots — Chapter 3 Lesson 3
// Feasible Path Families for Car-Like Robots (Dubins from scratch; RS via OMPL optional)
//
// Build (no OMPL):
//   g++ -O2 -std=c++17 Chapter3_Lesson3.cpp -o Chapter3_Lesson3
//
// Run:
//   ./Chapter3_Lesson3
//
// Output:
//   dubins_path.csv (x,y,theta)

#include <cmath>
#include <cfloat>
#include <fstream>
#include <iostream>
#include <string>
#include <tuple>
#include <vector>

static constexpr double PI = 3.14159265358979323846;

static double mod2pi(double a) {
    a = std::fmod(a, 2.0 * PI);
    if (a < 0.0) a += 2.0 * PI;
    return a;
}

static double angdiff(double a, double b) {
    // minimal signed angle difference a-b in [-pi, pi)
    double d = std::fmod(a - b + PI, 2.0 * PI);
    if (d < 0.0) d += 2.0 * PI;
    return d - PI;
}

struct Pose2 {
    double x{0.0};
    double y{0.0};
    double theta{0.0};
};

struct DubinsPath {
    std::string type;              // LSL, RSR, ...
    std::string seg_types;         // length 3 string, e.g., "LSL"
    std::tuple<double,double,double> params; // (t,p,q) in normalized units; turns are angles [rad]
    double R_min{1.0};

    double length() const {
        auto [t,p,q] = params;
        return R_min * (t + p + q);
    }
};

static Pose2 dubinsSegmentUnit(const Pose2& s, char segType, double segLen) {
    Pose2 p = s;
    if (segType == 'S') {
        p.x += segLen * std::cos(p.theta);
        p.y += segLen * std::sin(p.theta);
        return p;
    }
    if (segType == 'L') {
        p.x += std::sin(p.theta + segLen) - std::sin(p.theta);
        p.y += -std::cos(p.theta + segLen) + std::cos(p.theta);
        p.theta = mod2pi(p.theta + segLen);
        return p;
    }
    if (segType == 'R') {
        p.x += -std::sin(p.theta - segLen) + std::sin(p.theta);
        p.y += std::cos(p.theta - segLen) - std::cos(p.theta);
        p.theta = mod2pi(p.theta - segLen);
        return p;
    }
    throw std::runtime_error("Unknown segType");
}

static double endpointError(double alpha, double d, double beta,
                            const std::string& segTypes,
                            const std::tuple<double,double,double>& prm) {
    Pose2 p{0.0, 0.0, alpha};
    auto [t, s, q] = prm;
    p = dubinsSegmentUnit(p, segTypes[0], t);
    p = dubinsSegmentUnit(p, segTypes[1], s);
    p = dubinsSegmentUnit(p, segTypes[2], q);
    return std::hypot(p.x - d, p.y) + std::fabs(angdiff(p.theta, beta));
}

// ---- Candidate formulas (standard Dubins closed forms) ----

static bool LSL(double alpha, double beta, double d, std::tuple<double,double,double>& out) {
    double sa = std::sin(alpha), sb = std::sin(beta);
    double ca = std::cos(alpha), cb = std::cos(beta);
    double cab = std::cos(alpha - beta);

    double tmp0 = d + sa - sb;
    double p2 = 2.0 + d*d - 2.0*cab + 2.0*d*(sa - sb);
    if (p2 < 0.0) return false;
    double p = std::sqrt(p2);
    double tmp1 = std::atan2(cb - ca, tmp0);
    double t = mod2pi(-alpha + tmp1);
    double q = mod2pi(beta - tmp1);
    out = {t,p,q};
    return true;
}

static bool RSR(double alpha, double beta, double d, std::tuple<double,double,double>& out) {
    double sa = std::sin(alpha), sb = std::sin(beta);
    double ca = std::cos(alpha), cb = std::cos(beta);
    double cab = std::cos(alpha - beta);

    double tmp0 = d - sa + sb;
    double p2 = 2.0 + d*d - 2.0*cab + 2.0*d*(-sa + sb);
    if (p2 < 0.0) return false;
    double p = std::sqrt(p2);
    double tmp1 = std::atan2(ca - cb, tmp0);
    double t = mod2pi(alpha - tmp1);
    double q = mod2pi(-beta + tmp1);
    out = {t,p,q};
    return true;
}

static bool LSR(double alpha, double beta, double d, std::tuple<double,double,double>& out) {
    double sa = std::sin(alpha), sb = std::sin(beta);
    double ca = std::cos(alpha), cb = std::cos(beta);
    double cab = std::cos(alpha - beta);

    double p2 = -2.0 + d*d + 2.0*cab + 2.0*d*(sa + sb);
    if (p2 < 0.0) return false;
    double p = std::sqrt(p2);
    double tmp0 = std::atan2(-ca - cb, d + sa + sb);
    double tmp1 = std::atan2(-2.0, p);
    double t = mod2pi(-alpha + tmp0 - tmp1);
    double q = mod2pi(-beta + tmp0 - tmp1);
    out = {t,p,q};
    return true;
}

static bool RSL(double alpha, double beta, double d, std::tuple<double,double,double>& out) {
    double sa = std::sin(alpha), sb = std::sin(beta);
    double ca = std::cos(alpha), cb = std::cos(beta);
    double cab = std::cos(alpha - beta);

    double p2 = -2.0 + d*d + 2.0*cab - 2.0*d*(sa + sb);
    if (p2 < 0.0) return false;
    double p = std::sqrt(p2);
    double tmp0 = std::atan2(ca + cb, d - sa - sb);
    double tmp1 = std::atan2(2.0, p);
    double t = mod2pi(alpha - tmp0 + tmp1);
    double q = mod2pi(beta - tmp0 + tmp1);
    out = {t,p,q};
    return true;
}

static bool RLR(double alpha, double beta, double d, std::tuple<double,double,double>& out) {
    double sa = std::sin(alpha), sb = std::sin(beta);
    double ca = std::cos(alpha), cb = std::cos(beta);
    double cab = std::cos(alpha - beta);

    double tmp0 = (6.0 - d*d + 2.0*cab + 2.0*d*(sa - sb)) / 8.0;
    if (std::fabs(tmp0) > 1.0) return false;
    double p = mod2pi(2.0*PI - std::acos(tmp0));
    double tmp1 = std::atan2(ca - cb, d - sa + sb);
    double t = mod2pi(alpha - tmp1 + p/2.0);
    double q = mod2pi(alpha - beta - t + p);
    out = {t,p,q};
    return true;
}

static bool LRL(double alpha, double beta, double d, std::tuple<double,double,double>& out) {
    // symmetry: LRL(alpha,beta,d) = RLR(-alpha,-beta,d)
    return RLR(mod2pi(-alpha), mod2pi(-beta), d, out);
}

// ---- Main solver ----

static DubinsPath dubinsShortest(const Pose2& q0, const Pose2& q1, double R_min) {
    if (R_min <= 0.0) throw std::runtime_error("R_min must be positive");

    // frame of q0
    double dx = q1.x - q0.x;
    double dy = q1.y - q0.y;
    double c0 = std::cos(q0.theta), s0 = std::sin(q0.theta);

    double x = (c0*dx + s0*dy) / R_min;
    double y = (-s0*dx + c0*dy) / R_min;
    double phi = mod2pi(q1.theta - q0.theta);

    double d = std::hypot(x, y);
    double theta = (d > 0.0) ? std::atan2(y, x) : 0.0;

    double alpha = mod2pi(-theta);
    double beta  = mod2pi(phi - theta);

    struct Cand { std::string type; std::string seg; bool (*fn)(double,double,double,std::tuple<double,double,double>&); };
    std::vector<Cand> cands = {
        {"LSL","LSL", LSL},
        {"RSR","RSR", RSR},
        {"LSR","LSR", LSR},
        {"RSL","RSL", RSL},
        {"RLR","RLR", RLR},
        {"LRL","LRL", LRL}
    };

    double bestLen = DBL_MAX;
    DubinsPath best;

    for (const auto& c : cands) {
        std::tuple<double,double,double> prm;
        if (!c.fn(alpha, beta, d, prm)) continue;
        double err = endpointError(alpha, d, beta, c.seg, prm);
        if (err > 1e-6) continue;
        double L = R_min * (std::get<0>(prm) + std::get<1>(prm) + std::get<2>(prm));
        if (L < bestLen) {
            bestLen = L;
            best = DubinsPath{c.type, c.seg, prm, R_min};
        }
    }

    if (bestLen >= DBL_MAX/2.0) throw std::runtime_error("No feasible Dubins path found");
    return best;
}

static std::vector<Pose2> samplePath(const Pose2& q0, const DubinsPath& path, double step) {
    auto [t,p,q] = path.params;
    std::vector<std::pair<char,double>> segs = {
        {path.seg_types[0], t},
        {path.seg_types[1], p},
        {path.seg_types[2], q}
    };

    std::vector<Pose2> pts;
    pts.push_back(q0);

    Pose2 cur = q0;

    for (auto [st, sl] : segs) {
        if (st == 'S') {
            double L = sl * path.R_min;
            int n = std::max(1, (int)std::ceil(L / step));
            double ds = L / n;
            for (int i = 0; i < n; ++i) {
                cur.x += ds * std::cos(cur.theta);
                cur.y += ds * std::sin(cur.theta);
                pts.push_back(cur);
            }
        } else {
            double a = sl;
            double arc = a * path.R_min;
            int n = std::max(1, (int)std::ceil(arc / step));
            double da = a / n;
            for (int i = 0; i < n; ++i) {
                if (st == 'L') {
                    cur.x += path.R_min * (std::sin(cur.theta + da) - std::sin(cur.theta));
                    cur.y += path.R_min * (-std::cos(cur.theta + da) + std::cos(cur.theta));
                    cur.theta = mod2pi(cur.theta + da);
                } else {
                    cur.x += path.R_min * (-std::sin(cur.theta - da) + std::sin(cur.theta));
                    cur.y += path.R_min * (std::cos(cur.theta - da) - std::cos(cur.theta));
                    cur.theta = mod2pi(cur.theta - da);
                }
                pts.push_back(cur);
            }
        }
    }
    return pts;
}

int main() {
    Pose2 q0{0.0, 0.0, 10.0 * PI / 180.0};
    Pose2 q1{8.0, 4.0, 110.0 * PI / 180.0};
    double R_min = 2.0;

    DubinsPath path = dubinsShortest(q0, q1, R_min);
    std::cout << "Dubins type: " << path.type << ", length = " << path.length() << "\n";

    auto pts = samplePath(q0, path, 0.05);

    std::ofstream f("dubins_path.csv");
    f << "x,y,theta\n";
    for (const auto& p : pts) {
        f << p.x << "," << p.y << "," << p.theta << "\n";
    }
    f.close();

    std::cout << "Wrote dubins_path.csv (" << pts.size() << " samples)\n";

    // Optional (exact Reeds–Shepp) via OMPL:
    //   #include <ompl/base/spaces/ReedsSheppStateSpace.h>
    //   Use ompl::base::ReedsSheppStateSpace(R_min) to compute RS distance and interpolate.
    return 0;
}
      

8.3 Java (Standard Library)

File: Chapter3_Lesson3.java

// Chapter3_Lesson3.java
// Autonomous Mobile Robots - Chapter 3 Lesson 3
// Feasible Path Families for Car-Like Robots (Dubins from scratch; reverse-only variant)
//
// Compile:
//   javac Chapter3_Lesson3.java
// Run:
//   java Chapter3_Lesson3
//
// Output:
//   dubins_path_java.csv

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

public class Chapter3_Lesson3 {

    static final double PI = Math.PI;

    static double mod2pi(double a) {
        a = a % (2.0 * PI);
        if (a < 0.0) a += 2.0 * PI;
        return a;
    }

    static double angdiff(double a, double b) {
        double d = (a - b + PI) % (2.0 * PI);
        if (d < 0.0) d += 2.0 * PI;
        return d - PI;
    }

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

    static class DubinsPath {
        String type;      // LSL, RSR, ...
        String segTypes;  // e.g., "LSL"
        double t, p, q;   // normalized: turns are angles [rad]
        double Rmin;

        DubinsPath(String type, String segTypes, double t, double p, double q, double Rmin) {
            this.type = type;
            this.segTypes = segTypes;
            this.t = t; this.p = p; this.q = q;
            this.Rmin = Rmin;
        }

        double length() { return Rmin * (t + p + q); }
    }

    static Pose2 segmentUnit(Pose2 s, char st, double sl) {
        double x = s.x, y = s.y, th = s.theta;
        if (st == 'S') {
            x += sl * Math.cos(th);
            y += sl * Math.sin(th);
            return new Pose2(x, y, th);
        } else if (st == 'L') {
            x += Math.sin(th + sl) - Math.sin(th);
            y += -Math.cos(th + sl) + Math.cos(th);
            th = mod2pi(th + sl);
            return new Pose2(x, y, th);
        } else if (st == 'R') {
            x += -Math.sin(th - sl) + Math.sin(th);
            y += Math.cos(th - sl) - Math.cos(th);
            th = mod2pi(th - sl);
            return new Pose2(x, y, th);
        }
        throw new IllegalArgumentException("Unknown segment type");
    }

    static double endpointError(double alpha, double d, double beta, String seg, double t, double p, double q) {
        Pose2 cur = new Pose2(0.0, 0.0, alpha);
        cur = segmentUnit(cur, seg.charAt(0), t);
        cur = segmentUnit(cur, seg.charAt(1), p);
        cur = segmentUnit(cur, seg.charAt(2), q);
        return Math.hypot(cur.x - d, cur.y) + Math.abs(angdiff(cur.theta, beta));
    }

    interface CandidateFn {
        boolean compute(double alpha, double beta, double d, double[] out);
    }

    // ---- Candidate formulas ----

    static boolean LSL(double alpha, double beta, double d, double[] out) {
        double sa = Math.sin(alpha), sb = Math.sin(beta);
        double ca = Math.cos(alpha), cb = Math.cos(beta);
        double cab = Math.cos(alpha - beta);

        double tmp0 = d + sa - sb;
        double p2 = 2.0 + d*d - 2.0*cab + 2.0*d*(sa - sb);
        if (p2 < 0.0) return false;
        double p = Math.sqrt(p2);
        double tmp1 = Math.atan2(cb - ca, tmp0);
        double t = mod2pi(-alpha + tmp1);
        double q = mod2pi(beta - tmp1);
        out[0] = t; out[1] = p; out[2] = q;
        return true;
    }

    static boolean RSR(double alpha, double beta, double d, double[] out) {
        double sa = Math.sin(alpha), sb = Math.sin(beta);
        double ca = Math.cos(alpha), cb = Math.cos(beta);
        double cab = Math.cos(alpha - beta);

        double tmp0 = d - sa + sb;
        double p2 = 2.0 + d*d - 2.0*cab + 2.0*d*(-sa + sb);
        if (p2 < 0.0) return false;
        double p = Math.sqrt(p2);
        double tmp1 = Math.atan2(ca - cb, tmp0);
        double t = mod2pi(alpha - tmp1);
        double q = mod2pi(-beta + tmp1);
        out[0] = t; out[1] = p; out[2] = q;
        return true;
    }

    static boolean LSR(double alpha, double beta, double d, double[] out) {
        double sa = Math.sin(alpha), sb = Math.sin(beta);
        double ca = Math.cos(alpha), cb = Math.cos(beta);
        double cab = Math.cos(alpha - beta);

        double p2 = -2.0 + d*d + 2.0*cab + 2.0*d*(sa + sb);
        if (p2 < 0.0) return false;
        double p = Math.sqrt(p2);
        double tmp0 = Math.atan2(-ca - cb, d + sa + sb);
        double tmp1 = Math.atan2(-2.0, p);
        double t = mod2pi(-alpha + tmp0 - tmp1);
        double q = mod2pi(-beta + tmp0 - tmp1);
        out[0] = t; out[1] = p; out[2] = q;
        return true;
    }

    static boolean RSL(double alpha, double beta, double d, double[] out) {
        double sa = Math.sin(alpha), sb = Math.sin(beta);
        double ca = Math.cos(alpha), cb = Math.cos(beta);
        double cab = Math.cos(alpha - beta);

        double p2 = -2.0 + d*d + 2.0*cab - 2.0*d*(sa + sb);
        if (p2 < 0.0) return false;
        double p = Math.sqrt(p2);
        double tmp0 = Math.atan2(ca + cb, d - sa - sb);
        double tmp1 = Math.atan2(2.0, p);
        double t = mod2pi(alpha - tmp0 + tmp1);
        double q = mod2pi(beta - tmp0 + tmp1);
        out[0] = t; out[1] = p; out[2] = q;
        return true;
    }

    static boolean RLR(double alpha, double beta, double d, double[] out) {
        double sa = Math.sin(alpha), sb = Math.sin(beta);
        double ca = Math.cos(alpha), cb = Math.cos(beta);
        double cab = Math.cos(alpha - beta);

        double tmp0 = (6.0 - d*d + 2.0*cab + 2.0*d*(sa - sb)) / 8.0;
        if (Math.abs(tmp0) > 1.0) return false;
        double p = mod2pi(2.0*PI - Math.acos(tmp0));
        double tmp1 = Math.atan2(ca - cb, d - sa + sb);
        double t = mod2pi(alpha - tmp1 + p/2.0);
        double q = mod2pi(alpha - beta - t + p);
        out[0] = t; out[1] = p; out[2] = q;
        return true;
    }

    static boolean LRL(double alpha, double beta, double d, double[] out) {
        // symmetry: LRL(alpha,beta,d) = RLR(-alpha,-beta,d)
        return RLR(mod2pi(-alpha), mod2pi(-beta), d, out);
    }

    static DubinsPath dubinsShortest(Pose2 q0, Pose2 q1, double Rmin) {
        if (Rmin <= 0.0) throw new IllegalArgumentException("Rmin must be positive");

        double dx = q1.x - q0.x;
        double dy = q1.y - q0.y;
        double c0 = Math.cos(q0.theta), s0 = Math.sin(q0.theta);

        double x = (c0*dx + s0*dy) / Rmin;
        double y = (-s0*dx + c0*dy) / Rmin;
        double phi = mod2pi(q1.theta - q0.theta);

        double d = Math.hypot(x, y);
        double theta = (d > 0.0) ? Math.atan2(y, x) : 0.0;

        double alpha = mod2pi(-theta);
        double beta  = mod2pi(phi - theta);

        class Cand {
            String type, seg; CandidateFn fn;
            Cand(String type, String seg, CandidateFn fn) { this.type = type; this.seg = seg; this.fn = fn; }
        }

        List<Cand> cands = new ArrayList<>();
        cands.add(new Cand("LSL","LSL", Chapter3_Lesson3::LSL));
        cands.add(new Cand("RSR","RSR", Chapter3_Lesson3::RSR));
        cands.add(new Cand("LSR","LSR", Chapter3_Lesson3::LSR));
        cands.add(new Cand("RSL","RSL", Chapter3_Lesson3::RSL));
        cands.add(new Cand("RLR","RLR", Chapter3_Lesson3::RLR));
        cands.add(new Cand("LRL","LRL", Chapter3_Lesson3::LRL));

        double bestLen = Double.POSITIVE_INFINITY;
        DubinsPath best = null;

        for (Cand c : cands) {
            double[] out = new double[3];
            if (!c.fn.compute(alpha, beta, d, out)) continue;
            double err = endpointError(alpha, d, beta, c.seg, out[0], out[1], out[2]);
            if (err > 1e-6) continue;
            double L = Rmin * (out[0] + out[1] + out[2]);
            if (L < bestLen) {
                bestLen = L;
                best = new DubinsPath(c.type, c.seg, out[0], out[1], out[2], Rmin);
            }
        }

        if (best == null) throw new RuntimeException("No feasible Dubins path found");
        return best;
    }

    static List<Pose2> sample(Pose2 q0, DubinsPath path, double step) {
        List<Pose2> pts = new ArrayList<>();
        pts.add(new Pose2(q0.x, q0.y, mod2pi(q0.theta)));

        Pose2 cur = new Pose2(q0.x, q0.y, mod2pi(q0.theta));
        double[] lens = new double[]{path.t, path.p, path.q};

        for (int i = 0; i < 3; i++) {
            char st = path.segTypes.charAt(i);
            double sl = lens[i];

            if (st == 'S') {
                double L = sl * path.Rmin;
                int n = Math.max(1, (int)Math.ceil(L / step));
                double ds = L / n;
                for (int k = 0; k < n; k++) {
                    cur.x += ds * Math.cos(cur.theta);
                    cur.y += ds * Math.sin(cur.theta);
                    pts.add(new Pose2(cur.x, cur.y, cur.theta));
                }
            } else {
                double a = sl;
                double arc = a * path.Rmin;
                int n = Math.max(1, (int)Math.ceil(arc / step));
                double da = a / n;
                for (int k = 0; k < n; k++) {
                    if (st == 'L') {
                        cur.x += path.Rmin * (Math.sin(cur.theta + da) - Math.sin(cur.theta));
                        cur.y += path.Rmin * (-Math.cos(cur.theta + da) + Math.cos(cur.theta));
                        cur.theta = mod2pi(cur.theta + da);
                    } else {
                        cur.x += path.Rmin * (-Math.sin(cur.theta - da) + Math.sin(cur.theta));
                        cur.y += path.Rmin * (Math.cos(cur.theta - da) - Math.cos(cur.theta));
                        cur.theta = mod2pi(cur.theta - da);
                    }
                    pts.add(new Pose2(cur.x, cur.y, cur.theta));
                }
            }
        }
        return pts;
    }

    static DubinsPath dubinsBackwardOnly(Pose2 q0, Pose2 q1, double Rmin) {
        // drive backward everywhere: equivalent to forward Dubins on headings shifted by pi
        Pose2 q0b = new Pose2(q0.x, q0.y, mod2pi(q0.theta + PI));
        Pose2 q1b = new Pose2(q1.x, q1.y, mod2pi(q1.theta + PI));
        return dubinsShortest(q0b, q1b, Rmin);
    }

    public static void main(String[] args) throws IOException {
        Pose2 q0 = new Pose2(0.0, 0.0, Math.toRadians(10.0));
        Pose2 q1 = new Pose2(8.0, 4.0, Math.toRadians(110.0));
        double Rmin = 2.0;

        DubinsPath fwd = dubinsShortest(q0, q1, Rmin);
        System.out.println("Dubins forward: " + fwd.type + ", length=" + fwd.length());

        DubinsPath back = dubinsBackwardOnly(q0, q1, Rmin);
        System.out.println("Dubins backward-only: " + back.type + ", length=" + back.length());

        List<Pose2> pts = sample(q0, fwd, 0.05);
        try (FileWriter w = new FileWriter("dubins_path_java.csv")) {
            w.write("x,y,theta\n");
            for (Pose2 p : pts) {
                w.write(p.x + "," + p.y + "," + p.theta + "\n");
            }
        }
        System.out.println("Wrote dubins_path_java.csv (" + pts.size() + " samples)");

        // For exact Reeds-Shepp (allowing direction switches), use a library such as OMPL in C++.
    }
}
      

8.4 MATLAB/Simulink (Toolbox + From-Scratch)

File: Chapter3_Lesson3.m

% Chapter3_Lesson3.m
% Autonomous Mobile Robots — Chapter 3 Lesson 3
% Feasible Path Families for Car-Like Robots (Dubins)
%
% Requirements:
%   - For toolbox-based approach: Robotics System Toolbox (dubinsConnection)
%   - For from-scratch approach: no extra toolboxes
%
% Usage:
%   Chapter3_Lesson3

function Chapter3_Lesson3()
clc; close all;

q0 = [0.0, 0.0, deg2rad(10.0)];      % [x,y,theta]
q1 = [8.0, 4.0, deg2rad(110.0)];
Rmin = 2.0;

fprintf('--- Dubins path demo ---\n');

if exist('dubinsConnection','class') == 8
    fprintf('Using Robotics System Toolbox: dubinsConnection\n');
    dubConn = dubinsConnection('MinTurningRadius', Rmin);
    [pathSegObj, pathLen] = connect(dubConn, q0, q1);
    fprintf('Toolbox length = %.4f\n', pathLen);

    % Interpolate
    s = linspace(0, pathLen, 300);
    states = interpolate(pathSegObj{1}, s);
    x = states(:,1); y = states(:,2); th = states(:,3);
else
    fprintf('Using from-scratch Dubins solver (no toolbox)\n');
    path = dubinsShortest(q0, q1, Rmin);
    fprintf('Type = %s, length = %.4f\n', path.type, path.length);

    pts = sampleDubins(q0, path, 0.05);
    x = pts(:,1); y = pts(:,2); th = pts(:,3);
end

figure; plot(x,y,'LineWidth',1.5); hold on;
plot(q0(1),q0(2),'ko','MarkerFaceColor','k');
plot(q1(1),q1(2),'ks','MarkerFaceColor','k');
axis equal; grid on;
title('Dubins feasible path (bounded curvature)');
xlabel('x'); ylabel('y');

% Optional: build a simple Simulink model that integrates kinematics
% buildSimulinkDubinsModel();  % Uncomment if you want auto-generated model

end

% -------------------- From-scratch implementation --------------------

function a = mod2pi(a)
a = mod(a, 2*pi);
if a < 0
    a = a + 2*pi;
end
end

function d = angdiff(a,b)
d = mod(a-b+pi, 2*pi);
d = d - pi;
end

function pose = segUnit(pose, st, sl)
x = pose(1); y = pose(2); th = pose(3);
switch st
    case 'S'
        x = x + sl*cos(th);
        y = y + sl*sin(th);
    case 'L'
        x = x + (sin(th+sl) - sin(th));
        y = y + (-cos(th+sl) + cos(th));
        th = mod2pi(th + sl);
    case 'R'
        x = x + (-sin(th-sl) + sin(th));
        y = y + (cos(th-sl) - cos(th));
        th = mod2pi(th - sl);
    otherwise
        error('Unknown segment type');
end
pose = [x;y;th];
end

function err = endpointError(alpha, d, beta, seg, prm)
pose = [0;0;alpha];
pose = segUnit(pose, seg(1), prm(1));
pose = segUnit(pose, seg(2), prm(2));
pose = segUnit(pose, seg(3), prm(3));
err = hypot(pose(1)-d, pose(2)) + abs(angdiff(pose(3), beta));
end

function prm = cand_LSL(alpha,beta,d)
sa = sin(alpha); sb = sin(beta);
ca = cos(alpha); cb = cos(beta);
cab = cos(alpha-beta);
tmp0 = d + sa - sb;
p2 = 2 + d^2 - 2*cab + 2*d*(sa - sb);
if p2 < 0, prm = []; return; end
p = sqrt(p2);
tmp1 = atan2(cb - ca, tmp0);
t = mod2pi(-alpha + tmp1);
q = mod2pi(beta - tmp1);
prm = [t;p;q];
end

function prm = cand_RSR(alpha,beta,d)
sa = sin(alpha); sb = sin(beta);
ca = cos(alpha); cb = cos(beta);
cab = cos(alpha-beta);
tmp0 = d - sa + sb;
p2 = 2 + d^2 - 2*cab + 2*d*(-sa + sb);
if p2 < 0, prm = []; return; end
p = sqrt(p2);
tmp1 = atan2(ca - cb, tmp0);
t = mod2pi(alpha - tmp1);
q = mod2pi(-beta + tmp1);
prm = [t;p;q];
end

function prm = cand_LSR(alpha,beta,d)
sa = sin(alpha); sb = sin(beta);
ca = cos(alpha); cb = cos(beta);
cab = cos(alpha-beta);
p2 = -2 + d^2 + 2*cab + 2*d*(sa + sb);
if p2 < 0, prm = []; return; end
p = sqrt(p2);
tmp0 = atan2(-ca - cb, d + sa + sb);
tmp1 = atan2(-2, p);
t = mod2pi(-alpha + tmp0 - tmp1);
q = mod2pi(-beta + tmp0 - tmp1);
prm = [t;p;q];
end

function prm = cand_RSL(alpha,beta,d)
sa = sin(alpha); sb = sin(beta);
ca = cos(alpha); cb = cos(beta);
cab = cos(alpha-beta);
p2 = -2 + d^2 + 2*cab - 2*d*(sa + sb);
if p2 < 0, prm = []; return; end
p = sqrt(p2);
tmp0 = atan2(ca + cb, d - sa - sb);
tmp1 = atan2(2, p);
t = mod2pi(alpha - tmp0 + tmp1);
q = mod2pi(beta - tmp0 + tmp1);
prm = [t;p;q];
end

function prm = cand_RLR(alpha,beta,d)
sa = sin(alpha); sb = sin(beta);
ca = cos(alpha); cb = cos(beta);
cab = cos(alpha-beta);
tmp0 = (6 - d^2 + 2*cab + 2*d*(sa - sb))/8;
if abs(tmp0) > 1, prm = []; return; end
p = mod2pi(2*pi - acos(tmp0));
tmp1 = atan2(ca - cb, d - sa + sb);
t = mod2pi(alpha - tmp1 + p/2);
q = mod2pi(alpha - beta - t + p);
prm = [t;p;q];
end

function prm = cand_LRL(alpha,beta,d)
% symmetry: LRL(alpha,beta,d) = RLR(-alpha,-beta,d)
prm = cand_RLR(mod2pi(-alpha), mod2pi(-beta), d);
end

function path = dubinsShortest(q0,q1,Rmin)
dx = q1(1)-q0(1);
dy = q1(2)-q0(2);
c0 = cos(q0(3)); s0 = sin(q0(3));

x = (c0*dx + s0*dy)/Rmin;
y = (-s0*dx + c0*dy)/Rmin;
phi = mod2pi(q1(3) - q0(3));

d = hypot(x,y);
theta = 0;
if d > 0
    theta = atan2(y,x);
end
alpha = mod2pi(-theta);
beta  = mod2pi(phi - theta);

types = {'LSL','RSR','LSR','RSL','RLR','LRL'};
segs  = {'LSL','RSR','LSR','RSL','RLR','LRL'};
cands = {@cand_LSL,@cand_RSR,@cand_LSR,@cand_RSL,@cand_RLR,@cand_LRL};

bestLen = inf;
best = struct();

for i=1:numel(types)
    prm = cands{i}(alpha,beta,d);
    if isempty(prm), continue; end
    err = endpointError(alpha,d,beta,segs{i},prm);
    if err > 1e-6, continue; end
    L = Rmin * sum(prm);
    if L < bestLen
        bestLen = L;
        best.type = types{i};
        best.seg  = segs{i};
        best.prm  = prm;
        best.Rmin = Rmin;
        best.length = L;
    end
end

if isinf(bestLen)
    error('No feasible Dubins path found');
end
path = best;
end

function pts = sampleDubins(q0, path, step)
t = path.prm(1); p = path.prm(2); q = path.prm(3);
segTypes = path.seg;
lens = [t,p,q];

cur = [q0(1);q0(2);mod2pi(q0(3))];
pts = cur.';
for i=1:3
    st = segTypes(i);
    sl = lens(i);
    if st == 'S'
        L = sl * path.Rmin;
        n = max(1, ceil(L/step));
        ds = L/n;
        for k=1:n
            cur(1) = cur(1) + ds*cos(cur(3));
            cur(2) = cur(2) + ds*sin(cur(3));
            pts(end+1,:) = cur.'; %#ok<AGROW>
        end
    else
        a = sl;
        arc = a * path.Rmin;
        n = max(1, ceil(arc/step));
        da = a/n;
        for k=1:n
            if st == 'L'
                cur(1) = cur(1) + path.Rmin*(sin(cur(3)+da)-sin(cur(3)));
                cur(2) = cur(2) + path.Rmin*(-cos(cur(3)+da)+cos(cur(3)));
                cur(3) = mod2pi(cur(3)+da);
            else
                cur(1) = cur(1) + path.Rmin*(-sin(cur(3)-da)+sin(cur(3)));
                cur(2) = cur(2) + path.Rmin*(cos(cur(3)-da)-cos(cur(3)));
                cur(3) = mod2pi(cur(3)-da);
            end
            pts(end+1,:) = cur.'; %#ok<AGROW>
        end
    end
end
end

% -------------------- Simulink (optional) --------------------
function buildSimulinkDubinsModel()
% This helper creates a minimal Simulink model that integrates:
%   xdot = v cos(theta), ydot = v sin(theta), thetadot = v * kappa
% for a piecewise-constant curvature profile (e.g., Dubins primitives).
%
% It is provided as an engineering template and is not required to run the main demo.

model = 'Chapter3_Lesson3_DubinsSim';
if bdIsLoaded(model), close_system(model,0); end
new_system(model); open_system(model);

add_block('simulink/Sources/Constant',[model '/v'],'Value','1.0','Position',[30 30 80 60]);
add_block('simulink/Sources/Constant',[model '/kappa'],'Value','0.2','Position',[30 90 80 120]);

add_block('simulink/Math Operations/Trigonometric Function',[model '/cos'],'Operator','cos','Position',[140 20 170 50]);
add_block('simulink/Math Operations/Trigonometric Function',[model '/sin'],'Operator','sin','Position',[140 80 170 110]);

add_block('simulink/Math Operations/Product',[model '/vx'],'Position',[220 30 250 60]);
add_block('simulink/Math Operations/Product',[model '/vy'],'Position',[220 90 250 120]);

add_block('simulink/Continuous/Integrator',[model '/Int_x'],'Position',[310 30 340 60]);
add_block('simulink/Continuous/Integrator',[model '/Int_y'],'Position',[310 90 340 120]);
add_block('simulink/Continuous/Integrator',[model '/Int_theta'],'Position',[310 150 340 180]);

add_block('simulink/Math Operations/Product',[model '/v_kappa'],'Position',[220 150 250 180]);

% theta feedback to trig blocks
add_line(model,'Int_theta/1','cos/1');
add_line(model,'Int_theta/1','sin/1');

% v and trig to vx, vy
add_line(model,'v/1','vx/1'); add_line(model,'cos/1','vx/2');
add_line(model,'v/1','vy/1'); add_line(model,'sin/1','vy/2');

% integrate
add_line(model,'vx/1','Int_x/1');
add_line(model,'vy/1','Int_y/1');

% theta dot
add_line(model,'v/1','v_kappa/1');
add_line(model,'kappa/1','v_kappa/2');
add_line(model,'v_kappa/1','Int_theta/1');

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

8.5 Wolfram Mathematica

File: Chapter3_Lesson3.nb

(* Chapter3_Lesson3.nb
   Autonomous Mobile Robots — Chapter 3 Lesson 3
   Feasible Path Families for Car-Like Robots (Dubins, from scratch)

   Open this notebook in Wolfram Mathematica.
*)

Notebook[{
  Cell["Chapter 3 — Lesson 3: Dubins feasible paths (bounded curvature)", "Title"],
  Cell["From-scratch Dubins solver (LSL, RSR, LSR, RSL, RLR, LRL) + sampling and plot.", "Text"],

  Cell[BoxData@ToBoxes[
    ClearAll[mod2pi, angdiff, segUnit, endpointError, candLSL, candRSR, candLSR, candRSL, candRLR, candLRL,
      DubinsShortest, SampleDubins];

    mod2pi[a_] := Mod[a, 2 Pi];

    angdiff[a_, b_] := Module[{d = Mod[a - b + Pi, 2 Pi] - Pi}, d];

    segUnit[{x_, y_, th_}, st_, sl_] := Which[
      st === "S", {x + sl Cos[th], y + sl Sin[th], th},
      st === "L", {x + Sin[th + sl] - Sin[th], y - Cos[th + sl] + Cos[th], mod2pi[th + sl]},
      st === "R", {x - Sin[th - sl] + Sin[th], y + Cos[th - sl] - Cos[th], mod2pi[th - sl]},
      True, $Failed
    ];

    endpointError[alpha_, d_, beta_, seg_, {t_, p_, q_}] := Module[{pose},
      pose = {0.0, 0.0, alpha};
      pose = segUnit[pose, StringTake[seg, {1}], t];
      pose = segUnit[pose, StringTake[seg, {2}], p];
      pose = segUnit[pose, StringTake[seg, {3}], q];
      Norm[pose[[{1, 2}]] - {d, 0.0}] + Abs[angdiff[pose[[3]], beta]]
    ];

    candLSL[alpha_, beta_, d_] := Module[{sa, sb, ca, cb, cab, tmp0, p2, p, tmp1, t, q},
      sa = Sin[alpha]; sb = Sin[beta];
      ca = Cos[alpha]; cb = Cos[beta];
      cab = Cos[alpha - beta];
      tmp0 = d + sa - sb;
      p2 = 2 + d^2 - 2 cab + 2 d (sa - sb);
      If[p2 < 0, Return[{}]];
      p = Sqrt[p2];
      tmp1 = ArcTan[cb - ca, tmp0];
      t = mod2pi[-alpha + tmp1];
      q = mod2pi[beta - tmp1];
      {t, p, q}
    ];

    candRSR[alpha_, beta_, d_] := Module[{sa, sb, ca, cb, cab, tmp0, p2, p, tmp1, t, q},
      sa = Sin[alpha]; sb = Sin[beta];
      ca = Cos[alpha]; cb = Cos[beta];
      cab = Cos[alpha - beta];
      tmp0 = d - sa + sb;
      p2 = 2 + d^2 - 2 cab + 2 d (-sa + sb);
      If[p2 < 0, Return[{}]];
      p = Sqrt[p2];
      tmp1 = ArcTan[ca - cb, tmp0];
      t = mod2pi[alpha - tmp1];
      q = mod2pi[-beta + tmp1];
      {t, p, q}
    ];

    candLSR[alpha_, beta_, d_] := Module[{sa, sb, ca, cb, cab, p2, p, tmp0, tmp1, t, q},
      sa = Sin[alpha]; sb = Sin[beta];
      ca = Cos[alpha]; cb = Cos[beta];
      cab = Cos[alpha - beta];
      p2 = -2 + d^2 + 2 cab + 2 d (sa + sb);
      If[p2 < 0, Return[{}]];
      p = Sqrt[p2];
      tmp0 = ArcTan[-ca - cb, d + sa + sb];
      tmp1 = ArcTan[-2, p];
      t = mod2pi[-alpha + tmp0 - tmp1];
      q = mod2pi[-beta + tmp0 - tmp1];
      {t, p, q}
    ];

    candRSL[alpha_, beta_, d_] := Module[{sa, sb, ca, cb, cab, p2, p, tmp0, tmp1, t, q},
      sa = Sin[alpha]; sb = Sin[beta];
      ca = Cos[alpha]; cb = Cos[beta];
      cab = Cos[alpha - beta];
      p2 = -2 + d^2 + 2 cab - 2 d (sa + sb);
      If[p2 < 0, Return[{}]];
      p = Sqrt[p2];
      tmp0 = ArcTan[ca + cb, d - sa - sb];
      tmp1 = ArcTan[2, p];
      t = mod2pi[alpha - tmp0 + tmp1];
      q = mod2pi[beta - tmp0 + tmp1];
      {t, p, q}
    ];

    candRLR[alpha_, beta_, d_] := Module[{sa, sb, ca, cb, cab, tmp0, p, tmp1, t, q},
      sa = Sin[alpha]; sb = Sin[beta];
      ca = Cos[alpha]; cb = Cos[beta];
      cab = Cos[alpha - beta];
      tmp0 = (6 - d^2 + 2 cab + 2 d (sa - sb))/8;
      If[Abs[tmp0] > 1, Return[{}]];
      p = mod2pi[2 Pi - ArcCos[tmp0]];
      tmp1 = ArcTan[ca - cb, d - sa + sb];
      t = mod2pi[alpha - tmp1 + p/2];
      q = mod2pi[alpha - beta - t + p];
      {t, p, q}
    ];

    candLRL[alpha_, beta_, d_] := candRLR[mod2pi[-alpha], mod2pi[-beta], d];

    DubinsShortest[q0 : {x0_, y0_, th0_}, q1 : {x1_, y1_, th1_}, Rmin_?Positive] := Module[
      {dx, dy, c0, s0, x, y, phi, d, theta, alpha, beta, cands, best = <||>, bestLen = Infinity, prm, err, L},
      dx = x1 - x0; dy = y1 - y0;
      c0 = Cos[th0]; s0 = Sin[th0];
      x = (c0 dx + s0 dy)/Rmin;
      y = (-s0 dx + c0 dy)/Rmin;
      phi = mod2pi[th1 - th0];
      d = Sqrt[x^2 + y^2];
      theta = If[d > 0, ArcTan[y, x], 0.0];
      alpha = mod2pi[-theta];
      beta = mod2pi[phi - theta];

      cands = {
        {"LSL","LSL", candLSL},
        {"RSR","RSR", candRSR},
        {"LSR","LSR", candLSR},
        {"RSL","RSL", candRSL},
        {"RLR","RLR", candRLR},
        {"LRL","LRL", candLRL}
      };

      Do[
        prm = cands[[i,3]][alpha,beta,d];
        If[prm === {} , Continue[]];
        err = endpointError[alpha,d,beta,cands[[i,2]],prm];
        If[err > 10^-6, Continue[]];
        L = Rmin Total[prm];
        If[L < bestLen,
          bestLen = L;
          best = <|"Type"->cands[[i,1]], "Seg"->cands[[i,2]], "Params"->prm, "Rmin"->Rmin, "Length"->L|>;
        ];
        ,
        {i, Length[cands]}
      ];
      If[best === <||>, Return[$Failed], best]
    ];

    SampleDubins[q0 : {x0_, y0_, th0_}, path_Association, step_?Positive] := Module[
      {seg = path["Seg"], prm = path["Params"], R = path["Rmin"], cur, pts = {}, lens, st, sl, L, n, ds, a, arc, da},
      cur = {x0, y0, mod2pi[th0]};
      AppendTo[pts, cur];
      lens = prm;
      Do[
        st = StringTake[seg, {i}];
        sl = lens[[i]];
        If[st === "S",
          L = sl R;
          n = Max[1, Ceiling[L/step]];
          ds = L/n;
          Do[
            cur = {cur[[1]] + ds Cos[cur[[3]]], cur[[2]] + ds Sin[cur[[3]]], cur[[3]]};
            AppendTo[pts, cur],
            {k, n}
          ],
          a = sl;
          arc = a R;
          n = Max[1, Ceiling[arc/step]];
          da = a/n;
          Do[
            If[st === "L",
              cur = {cur[[1]] + R (Sin[cur[[3]] + da] - Sin[cur[[3]]]),
                     cur[[2]] + R (-Cos[cur[[3]] + da] + Cos[cur[[3]]]),
                     mod2pi[cur[[3]] + da]},
              cur = {cur[[1]] + R (-Sin[cur[[3]] - da] + Sin[cur[[3]]]),
                     cur[[2]] + R (Cos[cur[[3]] - da] - Cos[cur[[3]]]),
                     mod2pi[cur[[3]] - da]}
            ];
            AppendTo[pts, cur],
            {k, n}
          ]
        ];
        ,
        {i, 3}
      ];
      pts
    ];
  ], "Input"],

  Cell["Demo", "Section"],
  Cell[BoxData@ToBoxes[
    Module[{q0, q1, Rmin, path, pts},
      q0 = {0.0, 0.0, 10 Degree};
      q1 = {8.0, 4.0, 110 Degree};
      Rmin = 2.0;

      path = DubinsShortest[q0, q1, Rmin];
      Print[path];

      pts = SampleDubins[q0, path, 0.05];
      ListLinePlot[pts[[All, {1, 2}]], AspectRatio -> Automatic, GridLines -> Automatic,
        PlotLabel -> "Dubins feasible path (bounded curvature)"]
    ]
  ], "Input"]
},
WindowSize -> {1000, 800},
StyleDefinitions -> "Default.nb"
]
      

All source files are provided for download as a single ZIP archive at the end of this page.

Problem 1 (Ackermann to curvature bound). A car-like robot has wheelbase \( L \) and steering angle limit \( |\delta| \le \delta_{\max} \). Using the kinematic relation \( \kappa = \tan\delta / L \), derive \( R_{\min} \).

Solution. The curvature satisfies \( |\kappa| \le \tan(\delta_{\max})/L \), so

\[ \kappa_{\max} = \frac{\tan(\delta_{\max})}{L} \quad \Rightarrow \quad R_{\min} = \frac{1}{\kappa_{\max}} = \frac{L}{\tan(\delta_{\max})}. \]

Problem 2 (Scaling law). Show that if \( L_\ast(1) \) is the shortest Dubins length for unit radius, then for general \( R_{\min} \) the shortest length is \( L_\ast(R_{\min}) = R_{\min} L_\ast(1) \).

Solution. Under the scaling \( (x,y) \leftarrow (x/R_{\min}, y/R_{\min}) \), any feasible path becomes feasible for the unit-radius problem, with length scaled by \( 1/R_{\min} \). Conversely, any unit-radius solution rescales back. Therefore the optimal value scales linearly:

\[ L_\ast(R_{\min}) = R_{\min} L_\ast(1). \]

Problem 3 (Rigid-motion invariance). Let \( T \) be a planar rigid transformation. Prove that feasibility is invariant under \( T \).

Solution. This is Proposition 3.1: rotations preserve unit vectors and translations do not affect derivatives. The curvature constraint is unchanged because \( \theta'(s) \) is unchanged under constant heading offset. ■

Problem 4 (Bang-bang and straight singular arc). For the normalized Dubins problem, show that if \( \lambda_\theta(s)=0 \) on an interval, then the optimal control on that interval is \( u(s)=0 \) and the path segment is straight.

Solution. If \( \lambda_\theta=0 \) on an interval, then \( \partial H/\partial u = \lambda_\theta = 0 \) does not determine \( u \). Differentiate: \( 0 = \frac{\mathrm{d}}{\mathrm{d}s}\lambda_\theta = \lambda_x\sin\theta - \lambda_y\cos\theta \). Since \( \lambda_x,\lambda_y \) are constant, this constrains \( \theta \) to be constant on the interval, implying \( \theta'(s)=u(s)=0 \). Hence the segment is straight. ■

Problem 5 (Feasibility condition for an LSR candidate). In the normalized canonical frame, an LSR candidate requires \( p^2 = -2 + d^2 + 2\cos(\alpha-\beta) + 2d(\sin\alpha+\sin\beta) \ge 0 \). Explain geometrically why a negative \( p^2 \) means “no tangent exists”.

Solution. LSR corresponds to an internal tangent between a left circle at start and a right circle at goal. An internal tangent exists only when the circle centers are separated enough relative to the radius. The closed-form \( p^2 \) is exactly the squared straight-line distance between the computed tangent points; if it is negative, the construction would require an imaginary tangent length, indicating the circles overlap too much for an internal tangent. ■

Problem 6 (Backward-only comparison). Define \( q_0'=(x_0,y_0,\theta_0+\pi) \) and \( q_1'=(x_1,y_1,\theta_1+\pi) \). Show that a forward Dubins solution from \( q_0' \) to \( q_1' \) corresponds to driving backward from \( q_0 \) to \( q_1 \) with the same curvature bound.

Solution. Driving backward flips the vehicle’s body x-axis by \( \pi \). If we represent the reversed orientation as \( \theta' = \theta + \pi \), then the backward motion in world coordinates matches forward motion in the primed frame. Curvature magnitude is unchanged because \( \kappa = \tan\delta/L \) depends only on steering geometry, not on speed sign. Therefore, a forward curvature-bounded path between the primed poses maps to a backward-only feasible path between the original poses. ■

We defined curvature-bounded feasibility for car-like robots and derived why shortest feasible paths lie in small, analytically describable families. The Dubins family (forward-only) reduces the search to six candidate words built from max-curvature turns and straight segments. Allowing reverse motion expands the family to Reeds-Shepp paths, commonly implemented via robotics libraries. The provided multi-language implementations compute and sample Dubins paths, which will be used in Chapter 3, Lesson 5 for feasibility checks of candidate paths.