Add Olfati-Saber flocking simulation

PathPlanning/Flocking/flocking.py

@@ -0,0 +1,155 @@
+"""Olfati-Saber's Algorithm 2 for flocking in an obstacle-free plane.
+
+Run this file to animate 25 double-integrator agents following a moving
+reference. Arrows show velocities; lines connect neighbors within range.
+
+Reference: R. Olfati-Saber, Flocking for Multi-Agent Dynamic Systems:
+Algorithms and Theory, IEEE TAC 51(3), 401-420, 2006.
+https://doi.org/10.1109/TAC.2005.864190
+"""
+
+import matplotlib.pyplot as plt
+from matplotlib.animation import FuncAnimation
+from matplotlib.collections import LineCollection
+import numpy as np
+
+EPSILON = 0.1
+DESIRED_DISTANCE = 5.0
+INTERACTION_RANGE = 1.2 * DESIRED_DISTANCE
+BUMP_H = 0.2
+POTENTIAL_GAIN = 5.0  # Symmetric action function: a = b = 5, c = 0.
+POSITION_GAIN = 0.1
+VELOCITY_GAIN = 2.0 * np.sqrt(POSITION_GAIN)
+DT = 0.02
+show_animation = True
+
+
+def sigma_norm(vectors):
+    """Smooth norm of vectors along the last axis (equation 8)."""
+    squared_norm = np.sum(np.asarray(vectors) ** 2, axis=-1)
+    # Rationalized form avoids cancellation close to zero.
+    return squared_norm / (np.sqrt(1.0 + EPSILON * squared_norm) + 1.0)
+
+
+def bump_function(z):
+    """Smooth adjacency weight, with a finite cutoff at z = 1 (equation 10)."""
+    phase = np.clip((np.asarray(z) - BUMP_H) / (1.0 - BUMP_H), 0.0, 1.0)
+    return np.where(np.asarray(z) >= 0.0, 0.5 * (1.0 + np.cos(np.pi * phase)), 0.0)
+
+
+def interaction_terms(positions, velocities):
+    """Return separation/cohesion and alignment accelerations (equation 23).
+
+    Both inputs have shape (number of agents, 2). All agents use the same
+    state snapshot; self-interactions are excluded from the adjacency matrix.
+    """
+    displacement = positions[np.newaxis, :, :] - positions[:, np.newaxis, :]
+    distance = sigma_norm(displacement)
+    adjacency = bump_function(distance / sigma_norm([INTERACTION_RANGE]))
+    np.fill_diagonal(adjacency, 0.0)
+
+    offset = distance - sigma_norm([DESIRED_DISTANCE])
+    action = POTENTIAL_GAIN * adjacency * offset / np.sqrt(1.0 + offset ** 2)
+    direction = displacement / (1.0 + EPSILON * distance[:, :, np.newaxis])
+    gradient = np.sum(action[:, :, np.newaxis] * direction, axis=1)
+
+    velocity_difference = velocities[np.newaxis, :, :] - velocities[:, np.newaxis, :]
+    alignment = np.sum(adjacency[:, :, np.newaxis] * velocity_difference, axis=1)
+    return gradient, alignment
+
+
+def flocking_control(positions, velocities, reference_position, reference_velocity):
+    """Compute Algorithm 2 acceleration with linear navigation (equation 24)."""
+    gradient, alignment = interaction_terms(positions, velocities)
+    navigation = (-POSITION_GAIN * (positions - reference_position)
+                  - VELOCITY_GAIN * (velocities - reference_velocity))
+    return gradient + alignment + navigation
+
+
+def simulate(simulation_time=40.0, dt=DT, seed=0):
+    """Return times, position/velocity histories and moving reference positions.
+
+    The fixed-seed perturbed grid avoids coincident initial agents. Controls
+    are held over each step, integrating the double-integrator model exactly
+    for that constant acceleration. The reference has constant velocity.
+    """
+    rng = np.random.default_rng(seed)
+    x, y = np.meshgrid(np.arange(5), np.arange(5))
+    initial_positions = 4.5 * np.column_stack((x.ravel(), y.ravel()))
+    initial_positions += rng.uniform(-0.6, 0.6, initial_positions.shape)
+    times = np.arange(int(round(simulation_time / dt)) + 1) * dt
+    positions = np.empty((len(times), len(initial_positions), 2))
+    velocities = np.empty_like(positions)
+    positions[0] = initial_positions
+    velocities[0] = rng.uniform(-1.0, 1.0, initial_positions.shape)
+    reference_velocity = np.array([1.0, 0.5])
+    reference_start = initial_positions.mean(axis=0) + [4.0, 2.0]
+    references = reference_start + times[:, np.newaxis] * reference_velocity
+
+    for k in range(len(times) - 1):
+        acceleration = flocking_control(positions[k], velocities[k],
+                                        references[k], reference_velocity)
+        positions[k + 1] = positions[k] + dt * velocities[k] + 0.5 * dt ** 2 * acceleration
+        velocities[k + 1] = velocities[k] + dt * acceleration
+    return times, positions, velocities, references
+
+
+def create_animation(times, positions, velocities, references):
+    """Animate a simulation result; the returned animation can also be saved."""
+    fig, (ax, error_ax) = plt.subplots(1, 2, figsize=(10, 4.5))
+    colors = plt.colormaps["viridis"](np.linspace(0.1, 0.9, positions.shape[1]))
+    edges = LineCollection([], colors="0.8", linewidths=0.7, zorder=1)
+    ax.add_collection(edges)
+    agents = ax.scatter(*positions[0].T, c=colors, s=25, zorder=3, label="Agents")
+    arrows = ax.quiver(*positions[0].T, *velocities[0].T, color=colors,
+                       angles="xy", scale_units="xy", scale=0.6)
+    reference, = ax.plot([], [], "r*", markersize=12, label="Moving reference")
+    ax.plot(*references.T, "r--", alpha=0.4)
+    all_points = np.concatenate((positions.reshape(-1, 2), references))
+    ax.set(xlim=(all_points[:, 0].min() - 3, all_points[:, 0].max() + 3),
+           ylim=(all_points[:, 1].min() - 3, all_points[:, 1].max() + 3),
+           xlabel="x [m]", ylabel="y [m]", aspect="equal")
+    ax.legend(loc="upper left")
+    ax.grid(True)
+
+    reference_velocity = (references[-1] - references[0]) / (times[-1] - times[0])
+    velocity_error = velocities - reference_velocity
+    rms_error = np.sqrt(np.mean(np.sum(velocity_error ** 2, axis=2), axis=1))
+    error_ax.plot(times, rms_error, color="0.85")
+    error_line, = error_ax.plot([], [], color="tab:blue")
+    error_ax.set(xlabel="Time [s]", ylabel="RMS velocity error [m/s]",
+                 title="Velocity convergence", xlim=(times[0], times[-1]),
+                 ylim=(0.0, max(0.1, rms_error.max() * 1.1)))
+    error_ax.grid(True)
+    fig.tight_layout()
+
+    def update(k):
+        q, p = positions[k], velocities[k]
+        distance = np.linalg.norm(q[:, np.newaxis, :] - q[np.newaxis, :, :], axis=2)
+        i, j = np.nonzero(np.triu(distance < INTERACTION_RANGE, k=1))
+        edges.set_segments(np.stack((q[i], q[j]), axis=1))
+        agents.set_offsets(q)
+        arrows.set_offsets(q)
+        arrows.set_UVC(*p.T)
+        reference.set_data([references[k, 0]], [references[k, 1]])
+        error_line.set_data(times[:k + 1], rms_error[:k + 1])
+        ax.set_title(f"Olfati-Saber flocking: t = {times[k]:.1f} s")
+        return edges, agents, arrows, reference, error_line
+
+    stride = max(1, int(round(0.1 / (times[1] - times[0]))))
+    frames = list(range(0, len(times) - 1, stride)) + [len(times) - 1]
+    return FuncAnimation(fig, update, frames=frames, interval=100, repeat=False)
+
+
+def main():
+    """Run the flocking example with optional Matplotlib animation."""
+    result = simulate()
+    if show_animation:
+        animation = create_animation(*result)
+        plt.show()
+        return animation
+    return None
+
+
+if __name__ == "__main__":
+    main()

README.md

@@ -29,6 +29,7 @@ Python codes and [textbook](https://atsushisakai.github.io/PythonRobotics/index.
       * [FastSLAM 1.0](#fastslam-10)
    * [Path Planning](#path-planning)
       * [Dynamic Window Approach](#dynamic-window-approach)
+      * [Flocking](#flocking)
       * [Grid based search](#grid-based-search)
          * [Dijkstra algorithm](#dijkstra-algorithm)
          * [A* algorithm](#a-algorithm)
@@ -294,6 +295,16 @@ This is a 2D navigation sample code with Dynamic Window Approach.
 ![2](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/DynamicWindowApproach/animation.gif)
 
 
+## Flocking
+
+Olfati-Saber’s Algorithm 2 coordinates a group of agents through local spacing
+and velocity alignment while following a moving reference in free space.
+
+![Flocking](https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/ca4eb94b0876aa666e44b2125bcce450f7594f30/PathPlanning/Flocking/animation.gif)
+
+- [Algorithm documentation](https://atsushisakai.github.io/PythonRobotics/modules/5_path_planning/flocking/flocking.html)
+- [Flocking for Multi-Agent Dynamic Systems: Algorithms and Theory](https://doi.org/10.1109/TAC.2005.864190)
+
 ## Grid based search
 
 ### Dijkstra algorithm

docs/modules/5_path_planning/flocking/flocking_main.rst

@@ -0,0 +1,87 @@
+Flocking
+========
+
+Olfati-Saber's Algorithm 2 coordinates double-integrator agents in free space:
+:math:`\dot q_i=p_i,\ \dot p_i=u_i`.
+Nearby agents adjust their spacing and velocities while tracking a shared,
+constant-velocity reference :math:`(q_r,p_r)`.
+
+.. image:: https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/ca4eb94b0876aa666e44b2125bcce450f7594f30/PathPlanning/Flocking/animation.gif
+   :alt: Twenty-five agents forming a flock and following a moving reference
+
+Local interactions
+------------------
+
+For :math:`\epsilon>0`, use the smooth distance and its gradient:
+
+.. math::
+
+   \|z\|_\sigma=\frac{\sqrt{1+\epsilon\|z\|^2}-1}{\epsilon},
+   \qquad n_{ij}=\frac{q_j-q_i}{\sqrt{1+\epsilon\|q_j-q_i\|^2}}.
+
+The adjacency weight vanishes beyond interaction range :math:`r`:
+
+.. math::
+
+   \rho_h(s)=\begin{cases}
+     1 & 0\le s<h,\\
+     \frac{1+\cos(\pi(s-h)/(1-h))}{2} & h\le s\le1,\\
+     0 & \text{otherwise},
+   \end{cases}
+   \qquad a_{ij}=\rho_h(\|q_j-q_i\|_\sigma/\|r\|_\sigma),
+   \quad a_{ii}=0.
+
+With desired distance :math:`d<r`, this example uses the symmetric choice
+:math:`a=b=5` of the paper's action function (equation 15):
+
+.. math::
+
+   \phi_\alpha(s)=5\rho_h(s/\|r\|_\sigma)
+     \frac{s-\|d\|_\sigma}{\sqrt{1+(s-\|d\|_\sigma)^2}}.
+
+The sign gives repulsion below :math:`d` and attraction above :math:`d`,
+within range. Algorithm 2 (equation 24) combines this with alignment and
+linear navigation:
+
+.. math::
+
+   u_i=\underbrace{\sum_{j\ne i}\phi_\alpha(\|q_j-q_i\|_\sigma)n_{ij}}_{\text{spacing}}
+       +\underbrace{\sum_{j\ne i}a_{ij}(p_j-p_i)}_{\text{alignment}}
+       -\underbrace{c_1(q_i-q_r)+c_2(p_i-p_r)}_{\text{navigation}}.
+
+Simulation
+----------
+
+The example starts 25 agents on a perturbed grid, with random velocities.
+Parameters are :math:`d=5`, :math:`r=6`, :math:`\epsilon=0.1`,
+:math:`h=0.2`, :math:`c_1=0.1`, and :math:`c_2=2\sqrt{c_1}`.
+Accelerations are held over each :math:`0.02` s step:
+
+.. math::
+
+   q_i^{k+1}=q_i^k+\Delta t\,p_i^k+\tfrac12\Delta t^2u_i^k,
+   \qquad p_i^{k+1}=p_i^k+\Delta t\,u_i^k.
+
+Arrows show velocities; edges show neighbors. The second panel measures
+RMS velocity error relative to the moving reference.
+
+These are point agents without obstacles or actuator limits. Finite-step
+simulation does not guarantee collision avoidance for arbitrary initial
+conditions; exactly coincident agents have zero separation gradient.
+
+Code
+----
+
+.. autofunction:: PathPlanning.Flocking.flocking.main
+
+.. autofunction:: PathPlanning.Flocking.flocking.flocking_control
+
+.. autofunction:: PathPlanning.Flocking.flocking.simulate
+
+Reference
+---------
+
+R. Olfati-Saber, `Flocking for Multi-Agent Dynamic Systems: Algorithms and Theory
+<https://doi.org/10.1109/TAC.2005.864190>`__, IEEE Transactions on Automatic
+Control, 51(3), 401–420, 2006.
+`Author manuscript <https://hal.elte.hu/~vicsek/downloads/papers/flocking_tac06-engineering.pdf>`__.

docs/modules/5_path_planning/path_planning_main.rst

@@ -10,6 +10,7 @@ Path planning is the ability of a robot to search feasible and efficient path to
    :caption: Contents
 
    dynamic_window_approach/dynamic_window_approach
+   flocking/flocking
    bugplanner/bugplanner
    grid_base_search/grid_base_search
    time_based_grid_search/time_based_grid_search

tests/test_flocking.py

@@ -0,0 +1,115 @@
+import conftest  # Add root path to sys.path
+import matplotlib.pyplot as plt
+from matplotlib.animation import PillowWriter
+import numpy as np
+from numpy.testing import assert_allclose
+import pytest
+
+from PathPlanning.Flocking import flocking as m
+
+
+def test_sigma_norm_and_gradient_at_zero():
+    assert m.sigma_norm([0.0, 0.0]) == 0.0
+    assert_allclose(m.sigma_norm([3.0, 4.0]),
+                    (np.sqrt(1.0 + 25.0 * m.EPSILON) - 1.0) / m.EPSILON)
+    assert m.sigma_norm([1e-10, 0.0]) > 0.0
+    gradient, alignment = m.interaction_terms(np.zeros((2, 2)), np.zeros((2, 2)))
+    assert_allclose(gradient, 0.0)
+    assert_allclose(alignment, 0.0)
+
+
+def test_bump_cutoff_and_transition():
+    values = m.bump_function(np.array([-0.1, 0.0, m.BUMP_H,
+                                       (1.0 + m.BUMP_H) / 2, 1.0, 1.1]))
+    assert_allclose(values, [0.0, 1.0, 1.0, 0.5, 0.0, 0.0], atol=1e-15)
+
+
+@pytest.mark.parametrize("distance, sign", [(2.5, -1), (5.0, 0), (5.5, 1),
+                                           (6.0, 0), (7.0, 0)])
+def test_pair_repulsion_equilibrium_attraction_and_cutoff(distance, sign):
+    positions = np.array([[0.0, 0.0], [distance, 0.0]])
+    gradient, _ = m.interaction_terms(positions, np.zeros((2, 2)))
+    assert np.sign(gradient[0, 0]) == sign
+    assert_allclose(gradient[0], -gradient[1])
+    assert_allclose(gradient[:, 1], 0.0)
+
+
+def test_alignment_dissipates_relative_velocity_and_has_finite_range():
+    positions = np.array([[0.0, 0.0], [m.DESIRED_DISTANCE, 0.0]])
+    velocities = np.array([[2.0, -1.0], [-1.0, 3.0]])
+    _, alignment = m.interaction_terms(positions, velocities)
+    assert_allclose(alignment.sum(axis=0), 0.0)
+    assert np.sum(velocities * alignment) < 0.0
+    _, matched = m.interaction_terms(positions, np.ones((2, 2)))
+    assert_allclose(matched, 0.0)
+    positions[1, 0] = m.INTERACTION_RANGE
+    _, disconnected = m.interaction_terms(positions, velocities)
+    assert_allclose(disconnected, 0.0)
+
+
+def test_centroid_acceleration_depends_only_on_navigation():
+    rng = np.random.default_rng(5)
+    positions = rng.uniform(-5.0, 5.0, (10, 2))
+    velocities = rng.normal(size=(10, 2))
+    reference, reference_velocity = np.array([7.0, 3.0]), np.array([1.0, 0.5])
+    acceleration = m.flocking_control(positions, velocities, reference, reference_velocity)
+    expected = (-m.POSITION_GAIN * (positions.mean(axis=0) - reference)
+                - m.VELOCITY_GAIN * (velocities.mean(axis=0) - reference_velocity))
+    assert_allclose(acceleration.mean(axis=0), expected, atol=1e-14)
+    # With one agent, the navigation feedback is the entire controller.
+    assert_allclose(m.flocking_control(positions[:1], velocities[:1],
+                                      positions[0], velocities[0]), 0.0)
+
+
+def test_controller_is_invariant_to_agent_order_and_coordinate_frame():
+    rng = np.random.default_rng(4)
+    q, p = rng.normal(size=(2, 6, 2))
+    qr, pr = np.array([5.0, 2.0]), np.array([1.0, 0.5])
+    rotation = np.array([[0.0, -1.0], [1.0, 0.0]])
+    translation, boost = np.array([4.0, -8.0]), np.array([-2.0, 3.0])
+    order = np.array([3, 0, 5, 2, 4, 1])
+    expected = m.flocking_control(q, p, qr, pr)[order] @ rotation
+    transformed = m.flocking_control(q[order] @ rotation + translation,
+                                    p[order] @ rotation + boost,
+                                    qr @ rotation + translation, pr @ rotation + boost)
+    assert_allclose(transformed, expected, atol=1e-13)
+
+
+@pytest.mark.parametrize("seed", [0, 1, 2])
+def test_flock_converges_without_collisions_for_demo_initial_conditions(seed):
+    times, q, p, references = m.simulate(seed=seed)
+    assert np.isfinite(q).all() and np.isfinite(p).all()
+    velocity_error = np.sqrt(np.mean(np.sum((p[-1] - [1.0, 0.5]) ** 2, axis=1)))
+    assert velocity_error < 0.01
+    assert np.linalg.norm(q[-1].mean(axis=0) - references[-1]) < 0.01
+    distance = np.linalg.norm(q[:, :, None, :] - q[:, None, :, :], axis=-1)
+    distance[:, np.arange(25), np.arange(25)] = np.inf
+    assert distance.min() > 3.0
+    adjacency = (distance[-1] < m.INTERACTION_RANGE).astype(float)
+    laplacian = np.diag(adjacency.sum(axis=1)) - adjacency
+    assert np.linalg.eigvalsh(laplacian)[1] > 0.1  # One connected flock.
+
+    # Independent closed-form solution for critically damped centroid motion.
+    omega = np.sqrt(m.POSITION_GAIN)
+    e0 = q[0].mean(axis=0) - references[0]
+    v0 = p[0].mean(axis=0) - [1.0, 0.5]
+    t = times[:, None]
+    expected_error = (e0 + (v0 + omega * e0) * t) * np.exp(-omega * t)
+    assert_allclose(q.mean(axis=1) - references, expected_error, atol=0.025)
+
+
+def test_animation_can_be_rendered(tmp_path):
+    animation = m.create_animation(*m.simulate(simulation_time=0.1))
+    output = tmp_path / "flocking.gif"
+    animation.save(output, writer=PillowWriter(fps=10))
+    assert output.stat().st_size > 0
+    plt.close("all")
+
+
+def test_main_without_animation(monkeypatch):
+    monkeypatch.setattr(m, "show_animation", False)
+    m.main()
+
+
+if __name__ == "__main__":
+    conftest.run_this_test(__file__)