[wpimath] Add simulated annealing (#5961)

Co-authored-by: Ashray._.g <ashray.gupta@gmail.com>

wpimath/src/main/java/edu/wpi/first/math/optimization/SimulatedAnnealing.java

@@ -0,0 +1,79 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.optimization;
+
+import java.util.function.Function;
+import java.util.function.ToDoubleFunction;
+
+/**
+ * An implementation of the Simulated Annealing stochastic nonlinear optimization method.
+ *
+ * @see <a
+ *     href="https://en.wikipedia.org/wiki/Simulated_annealing">https://en.wikipedia.org/wiki/Simulated_annealing</a>
+ * @param <State> The type of the state to optimize.
+ */
+public final class SimulatedAnnealing<State> {
+  private final double m_initialTemperature;
+  private final Function<State, State> m_neighbor;
+  private final ToDoubleFunction<State> m_cost;
+
+  /**
+   * Constructor for Simulated Annealing that can be used for the same functions but with different
+   * initial states.
+   *
+   * @param initialTemperature The initial temperature. Higher temperatures make it more likely a
+   *     worse state will be accepted during iteration, helping to avoid local minima. The
+   *     temperature is decreased over time.
+   * @param neighbor Function that generates a random neighbor of the current state.
+   * @param cost Function that returns the scalar cost of a state.
+   */
+  public SimulatedAnnealing(
+      double initialTemperature, Function<State, State> neighbor, ToDoubleFunction<State> cost) {
+    m_initialTemperature = initialTemperature;
+    m_neighbor = neighbor;
+    m_cost = cost;
+  }
+
+  /**
+   * Runs the Simulated Annealing algorithm.
+   *
+   * @param initialGuess The initial state.
+   * @param iterations Number of iterations to run the solver.
+   * @return The optimized stater.
+   */
+  public State solve(State initialGuess, int iterations) {
+    State minState = initialGuess;
+    double minCost = Double.MAX_VALUE;
+
+    State state = initialGuess;
+    double cost = m_cost.applyAsDouble(state);
+
+    for (int i = 0; i < iterations; ++i) {
+      double temperature = m_initialTemperature / i;
+
+      State proposedState = m_neighbor.apply(state);
+      double proposedCost = m_cost.applyAsDouble(proposedState);
+      double deltaCost = proposedCost - cost;
+
+      double acceptanceProbability = Math.exp(-deltaCost / temperature);
+
+      // If cost went down or random number exceeded acceptance probability,
+      // accept the proposed state
+      if (deltaCost < 0 || acceptanceProbability >= Math.random()) {
+        state = proposedState;
+        cost = proposedCost;
+      }
+
+      // If proposed cost is less than minimum, the proposed state becomes the
+      // new minimum
+      if (proposedCost < minCost) {
+        minState = proposedState;
+        minCost = proposedCost;
+      }
+    }
+
+    return minState;
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/path/TravelingSalesman.java

@@ -0,0 +1,108 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.path;
+
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.optimization.SimulatedAnnealing;
+import java.util.function.ToDoubleBiFunction;
+
+/**
+ * Given a list of poses, this class finds the shortest possible route that visits each pose exactly
+ * once and returns to the origin pose.
+ *
+ * @see <a
+ *     href="https://en.wikipedia.org/wiki/Travelling_salesman_problem">https://en.wikipedia.org/wiki/Travelling_salesman_problem</a>
+ */
+public class TravelingSalesman {
+  // Default cost is 2D distance between poses
+  private final ToDoubleBiFunction<Pose2d, Pose2d> m_cost;
+
+  /**
+   * Constructs a traveling salesman problem solver with a cost function defined as the 2D distance
+   * between poses.
+   */
+  public TravelingSalesman() {
+    this((Pose2d a, Pose2d b) -> Math.hypot(a.getX() - b.getX(), a.getY() - b.getY()));
+  }
+
+  /**
+   * Constructs a traveling salesman problem solver with a user-provided cost function.
+   *
+   * @param cost Function that returns the cost between two poses. The sum of the costs for every
+   *     pair of poses is minimized.
+   */
+  public TravelingSalesman(ToDoubleBiFunction<Pose2d, Pose2d> cost) {
+    m_cost = cost;
+  }
+
+  /**
+   * Finds the path through every pose that minimizes the cost.
+   *
+   * @param <Poses> A Num defining the length of the path and the number of poses.
+   * @param poses An array of Pose2ds the path must pass through.
+   * @param iterations The number of times the solver attempts to find a better random neighbor.
+   * @return The optimized path as an array of Pose2ds.
+   */
+  public <Poses extends Num> Pose2d[] solve(Pose2d[] poses, int iterations) {
+    var solver =
+        new SimulatedAnnealing<>(
+            1.0,
+            this::neighbor,
+            // Total cost is sum of all costs between adjacent pose pairs in path
+            (Vector<Poses> state) -> {
+              double sum = 0.0;
+              for (int i = 0; i < state.getNumRows(); ++i) {
+                sum +=
+                    m_cost.applyAsDouble(
+                        poses[(int) state.get(i, 0)],
+                        poses[(int) (state.get((i + 1) % poses.length, 0))]);
+              }
+              return sum;
+            });
+
+    var initial = new Vector<Poses>(() -> poses.length);
+    for (int i = 0; i < poses.length; ++i) {
+      initial.set(i, 0, i);
+    }
+
+    var indices = solver.solve(initial, iterations);
+
+    var solution = new Pose2d[poses.length];
+    for (int i = 0; i < poses.length; ++i) {
+      solution[i] = poses[(int) indices.get(i, 0)];
+    }
+
+    return solution;
+  }
+
+  /**
+   * A random neighbor is generated to try to replace the current one.
+   *
+   * @param state A vector that is a list of indices that defines the path through the path array.
+   * @return Generates a random neighbor of the current state by flipping a random range in the path
+   *     array.
+   */
+  private <Poses extends Num> Vector<Poses> neighbor(Vector<Poses> state) {
+    var proposedState = new Vector<Poses>(state);
+
+    int rangeStart = (int) (Math.random() * (state.getNumRows() - 1));
+    int rangeEnd = (int) (Math.random() * (state.getNumRows() - 1));
+    if (rangeEnd < rangeStart) {
+      int temp = rangeEnd;
+      rangeEnd = rangeStart;
+      rangeStart = temp;
+    }
+
+    for (int i = rangeStart; i <= (rangeStart + rangeEnd) / 2; ++i) {
+      double temp = proposedState.get(i, 0);
+      proposedState.set(i, 0, state.get(rangeEnd - (i - rangeStart), 0));
+      proposedState.set(rangeEnd - (i - rangeStart), 0, temp);
+    }
+
+    return proposedState;
+  }
+}

wpimath/src/main/native/include/frc/optimization/SimulatedAnnealing.h

@@ -0,0 +1,94 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <cmath>
+#include <functional>
+#include <limits>
+#include <random>
+
+namespace frc {
+
+/**
+ * An implementation of the Simulated Annealing stochastic nonlinear
+ * optimization method.
+ *
+ * @see <a
+ * href="https://en.wikipedia.org/wiki/Simulated_annealing">https://en.wikipedia.org/wiki/Simulated_annealing</a>
+ * @tparam State The type of the state to optimize.
+ */
+template <typename State>
+class SimulatedAnnealing {
+ public:
+  /**
+   * Constructor for Simulated Annealing that can be used for the same functions
+   * but with different initial states.
+   *
+   * @param initialTemperature The initial temperature. Higher temperatures make
+   *     it more likely a worse state will be accepted during iteration, helping
+   *     to avoid local minima. The temperature is decreased over time.
+   * @param neighbor Function that generates a random neighbor of the current
+   *     state.
+   * @param cost Function that returns the scalar cost of a state.
+   */
+  constexpr SimulatedAnnealing(double initialTemperature,
+                               std::function<State(const State&)> neighbor,
+                               std::function<double(const State&)> cost)
+      : m_initialTemperature{initialTemperature},
+        m_neighbor{neighbor},
+        m_cost{cost} {}
+
+  /**
+   * Runs the Simulated Annealing algorithm.
+   *
+   * @param initialGuess The initial state.
+   * @param iterations Number of iterations to run the solver.
+   * @return The optimized state.
+   */
+  State Solve(const State& initialGuess, int iterations) {
+    State minState = initialGuess;
+    double minCost = std::numeric_limits<double>::infinity();
+
+    std::random_device rd;
+    std::mt19937 gen{rd()};
+    std::uniform_real_distribution<> distr{0.0, 1.0};
+
+    State state = initialGuess;
+    double cost = m_cost(state);
+
+    for (int i = 0; i < iterations; ++i) {
+      double temperature = m_initialTemperature / i;
+
+      State proposedState = m_neighbor(state);
+      double proposedCost = m_cost(proposedState);
+      double deltaCost = proposedCost - cost;
+
+      double acceptanceProbability = std::exp(-deltaCost / temperature);
+
+      // If cost went down or random number exceeded acceptance probability,
+      // accept the proposed state
+      if (deltaCost < 0 || acceptanceProbability >= distr(gen)) {
+        state = proposedState;
+        cost = proposedCost;
+      }
+
+      // If proposed cost is less than minimum, the proposed state becomes the
+      // new minimum
+      if (proposedCost < minCost) {
+        minState = proposedState;
+        minCost = proposedCost;
+      }
+    }
+
+    return minState;
+  }
+
+ private:
+  double m_initialTemperature;
+  std::function<State(const State&)> m_neighbor;
+  std::function<double(const State&)> m_cost;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/frc/path/TravelingSalesman.h

@@ -0,0 +1,167 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <cmath>
+#include <functional>
+#include <random>
+#include <utility>
+#include <vector>
+
+#include <wpi/array.h>
+
+#include "frc/EigenCore.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/optimization/SimulatedAnnealing.h"
+
+namespace frc {
+
+/**
+ * Given a list of poses, this class finds the shortest possible route that
+ * visits each pose exactly once and returns to the origin pose.
+ *
+ * @see <a
+ * href="https://en.wikipedia.org/wiki/Travelling_salesman_problem">https://en.wikipedia.org/wiki/Travelling_salesman_problem</a>
+ */
+class TravelingSalesman {
+ public:
+  /**
+   * Constructs a traveling salesman problem solver with a cost function defined
+   * as the 2D distance between poses.
+   */
+  constexpr TravelingSalesman() = default;
+
+  /**
+   * Constructs a traveling salesman problem solver with a user-provided cost
+   * function.
+   *
+   * @param cost Function that returns the cost between two poses. The sum of
+   *     the costs for every pair of poses is minimized.
+   */
+  explicit TravelingSalesman(std::function<double(Pose2d, Pose2d)> cost)
+      : m_cost{std::move(cost)} {}
+
+  /**
+   * Finds the path through every pose that minimizes the cost.
+   *
+   * This overload supports a statically-sized list of poses.
+   *
+   * @tparam Poses The length of the path and the number of poses.
+   * @param poses An array of Pose2ds the path must pass through.
+   * @param iterations The number of times the solver attempts to find a better
+   *     random neighbor.
+   * @return The optimized path as an array of Pose2ds.
+   */
+  template <size_t Poses>
+  wpi::array<Pose2d, Poses> Solve(const wpi::array<Pose2d, Poses>& poses,
+                                  int iterations) {
+    SimulatedAnnealing<Vectord<Poses>> solver{
+        1, &Neighbor<Poses>, [&](const Vectord<Poses>& state) {
+          // Total cost is sum of all costs between adjacent pairs in path
+          double sum = 0.0;
+          for (int i = 0; i < state.rows(); ++i) {
+            sum +=
+                m_cost(poses[static_cast<int>(state(i))],
+                       poses[static_cast<int>(state((i + 1) % poses.size()))]);
+          }
+          return sum;
+        }};
+
+    Eigen::Vector<double, Poses> initial;
+    for (int i = 0; i < initial.rows(); ++i) {
+      initial(i) = i;
+    }
+
+    auto indices = solver.Solve(initial, iterations);
+
+    wpi::array<Pose2d, Poses> solution{wpi::empty_array};
+    for (size_t i = 0; i < poses.size(); ++i) {
+      solution[i] = poses[static_cast<int>(indices[i])];
+    }
+
+    return solution;
+  }
+
+  /**
+   * Finds the path through every pose that minimizes the cost.
+   *
+   * This overload supports a dynamically-sized list of poses for Python to use.
+   *
+   * @param poses An array of Pose2ds the path must pass through.
+   * @param iterations The number of times the solver attempts to find a better
+   *     random neighbor.
+   * @return The optimized path as an array of Pose2ds.
+   */
+  std::vector<Pose2d> Solve(std::span<const Pose2d> poses, int iterations) {
+    SimulatedAnnealing<Eigen::VectorXd> solver{
+        1.0, &Neighbor<Eigen::Dynamic>, [&](const Eigen::VectorXd& state) {
+          // Total cost is sum of all costs between adjacent pairs in path
+          double sum = 0.0;
+          for (int i = 0; i < state.rows(); ++i) {
+            sum +=
+                m_cost(poses[static_cast<int>(state(i))],
+                       poses[static_cast<int>(state((i + 1) % poses.size()))]);
+          }
+          return sum;
+        }};
+
+    Eigen::VectorXd initial{poses.size()};
+    for (int i = 0; i < initial.rows(); ++i) {
+      initial(i) = i;
+    }
+
+    auto indices = solver.Solve(initial, iterations);
+
+    std::vector<Pose2d> solution;
+    for (size_t i = 0; i < poses.size(); ++i) {
+      solution.emplace_back(poses[static_cast<int>(indices[i])]);
+    }
+
+    return solution;
+  }
+
+ private:
+  // Default cost is distance between poses
+  std::function<double(const Pose2d&, const Pose2d&)> m_cost =
+      [](const Pose2d& a, const Pose2d& b) -> double {
+    return units::math::hypot(a.X() - b.X(), a.Y() - b.Y()).value();
+  };
+
+  /**
+   * A random neighbor is generated to try to replace the current one.
+   *
+   * @tparam Poses The length of the path and the number of poses.
+   * @param state A vector that is a list of indices that defines the path
+   *     through the path array.
+   * @return Generates a random neighbor of the current state by flipping a
+   *     random range in the path array.
+   */
+  template <int Poses>
+  static Eigen::Vector<double, Poses> Neighbor(
+      const Eigen::Vector<double, Poses>& state) {
+    Eigen::Vector<double, Poses> proposedState = state;
+
+    std::random_device rd;
+    std::mt19937 gen{rd()};
+    std::uniform_int_distribution<> distr{0,
+                                          static_cast<int>(state.rows()) - 1};
+
+    int rangeStart = distr(gen);
+    int rangeEnd = distr(gen);
+    if (rangeEnd < rangeStart) {
+      std::swap(rangeStart, rangeEnd);
+    }
+
+    for (int i = rangeStart; i <= (rangeStart + rangeEnd) / 2; ++i) {
+      double temp = proposedState(i, 0);
+      proposedState(i, 0) = state(rangeEnd - (i - rangeStart), 0);
+      proposedState(rangeEnd - (i - rangeStart), 0) = temp;
+    }
+
+    return proposedState;
+  }
+};
+
+}  // namespace frc

wpimath/src/test/java/edu/wpi/first/math/optimization/SimulatedAnnealingTest.java

@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.optimization;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MathUtil;
+import java.util.function.DoubleUnaryOperator;
+import org.junit.jupiter.api.Test;
+
+class SimulatedAnnealingTest {
+  @Test
+  void testDoubleFunctionOptimizationHeartBeat() {
+    DoubleUnaryOperator function = x -> -(x + Math.sin(x)) * Math.exp(-x * x) + 1;
+
+    double stepSize = 10.0;
+
+    var simulatedAnnealing =
+        new SimulatedAnnealing<Double>(
+            2.0,
+            x -> MathUtil.clamp(x + (Math.random() - 0.5) * stepSize, -3, 3),
+            x -> function.applyAsDouble(x));
+
+    double solution = simulatedAnnealing.solve(-1.0, 5000);
+
+    assertEquals(0.68, solution, 1e-1);
+  }
+
+  @Test
+  void testDoubleFunctionOptimizationMultimodal() {
+    DoubleUnaryOperator function = x -> Math.sin(x) + Math.sin((10.0 / 3.0) * x);
+
+    double stepSize = 10.0;
+
+    var simulatedAnnealing =
+        new SimulatedAnnealing<Double>(
+            2.0,
+            x -> MathUtil.clamp(x + (Math.random() - 0.5) * stepSize, 0, 7),
+            x -> function.applyAsDouble(x));
+
+    double solution = simulatedAnnealing.solve(-1.0, 5000);
+
+    assertEquals(5.146, solution, 1e-1);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/path/TravelingSalesmanTest.java

@@ -0,0 +1,114 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.path;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.util.CircularBuffer;
+import org.junit.jupiter.api.Test;
+
+class TravelingSalesmanTest {
+  /**
+   * Returns true if the cycles represented by the two lists match.
+   *
+   * @param expected The expected cycle.
+   * @param actual The actual cycle.
+   */
+  private boolean isMatchingCycle(Pose2d[] expected, Pose2d[] actual) {
+    assertEquals(expected.length, actual.length);
+
+    // Find first element in actual that matches expected
+    int actualStart = 0;
+    while (!actual[actualStart].equals(expected[0])) {
+      ++actualStart;
+    }
+
+    // Check actual has expected cycle (forward)
+    var actualBufferForward = new CircularBuffer<Pose2d>(actual.length);
+    for (int i = 0; i < actual.length; ++i) {
+      actualBufferForward.addLast(actual[(actualStart + i) % actual.length]);
+    }
+    boolean matchesExpectedForward = true;
+    for (int i = 0; i < expected.length; ++i) {
+      matchesExpectedForward &= expected[i].equals(actualBufferForward.get(i));
+    }
+
+    // Check actual has expected cycle (reverse)
+    var actualBufferReverse = new CircularBuffer<Pose2d>(actual.length);
+    for (int i = 0; i < actual.length; ++i) {
+      actualBufferReverse.addFirst(actual[(actualStart + 1 + i) % actual.length]);
+    }
+
+    boolean matchesExpectedReverse = true;
+    for (int i = 0; i < expected.length; ++i) {
+      matchesExpectedReverse &= expected[i].equals(actualBufferReverse.get(i));
+    }
+
+    // Actual may be reversed from expected, but that's still valid
+    return matchesExpectedForward || matchesExpectedReverse;
+  }
+
+  @Test
+  void testFiveLengthPathWithDistanceCost() {
+    // ...................
+    // ........2..........
+    // ..0..........4.....
+    // ...................
+    // ....3.....1........
+    // ...................
+    var poses =
+        new Pose2d[] {
+          new Pose2d(3, 3, new Rotation2d(0)),
+          new Pose2d(11, 5, new Rotation2d(0)),
+          new Pose2d(9, 2, new Rotation2d(0)),
+          new Pose2d(5, 5, new Rotation2d(0)),
+          new Pose2d(14, 3, new Rotation2d(0))
+        };
+
+    var traveler = new TravelingSalesman();
+    var solution = traveler.solve(poses, 500);
+
+    assertEquals(5, solution.length);
+    var expected = new Pose2d[] {poses[0], poses[2], poses[4], poses[1], poses[3]};
+    assertTrue(isMatchingCycle(expected, solution));
+  }
+
+  @Test
+  void testTenLengthPathWithDistanceCost() {
+    // ....6.3..1.2.......
+    // ..4................
+    // .............9.....
+    // .0.................
+    // .....7..5...8......
+    // ...................
+    var poses =
+        new Pose2d[] {
+          new Pose2d(2, 4, new Rotation2d(0)),
+          new Pose2d(10, 1, new Rotation2d(0)),
+          new Pose2d(12, 1, new Rotation2d(0)),
+          new Pose2d(7, 1, new Rotation2d(0)),
+          new Pose2d(3, 2, new Rotation2d(0)),
+          new Pose2d(9, 5, new Rotation2d(0)),
+          new Pose2d(5, 1, new Rotation2d(0)),
+          new Pose2d(6, 5, new Rotation2d(0)),
+          new Pose2d(13, 5, new Rotation2d(0)),
+          new Pose2d(14, 3, new Rotation2d(0))
+        };
+
+    var traveler = new TravelingSalesman();
+    var solution = traveler.solve(poses, 500);
+
+    assertEquals(10, solution.length);
+    var expected =
+        new Pose2d[] {
+          poses[0], poses[4], poses[6], poses[3], poses[1], poses[2], poses[9], poses[8], poses[5],
+          poses[7]
+        };
+    assertTrue(isMatchingCycle(expected, solution));
+  }
+}

wpimath/src/test/native/cpp/optimization/SimulatedAnnealingTest.cpp

@@ -0,0 +1,57 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <algorithm>
+#include <cmath>
+#include <random>
+
+#include <gtest/gtest.h>
+
+#include "frc/optimization/SimulatedAnnealing.h"
+
+TEST(SimulatedAnnealingTest, DoubleFunctionOptimizationHeartBeat) {
+  auto function = [](double x) {
+    return -(x + std::sin(x)) * std::exp(-x * x) + 1;
+  };
+
+  constexpr double stepSize = 10.0;
+
+  std::random_device rd;
+  std::mt19937 gen{rd()};
+  std::uniform_real_distribution<> distr{0.0, 1.0};
+
+  frc::SimulatedAnnealing<double> simulatedAnnealing{
+      2.0,
+      [&](const double& x) {
+        return std::clamp(x + (distr(gen) - 0.5) * stepSize, -3.0, 3.0);
+      },
+      [&](const double& x) { return function(x); }};
+
+  double solution = simulatedAnnealing.Solve(-1.0, 5000);
+
+  EXPECT_NEAR(0.68, solution, 1e-1);
+}
+
+TEST(SimulatedAnnealingTest, DoubleFunctionOptimizationMultimodal) {
+  auto function = [](double x) {
+    return std::sin(x) + std::sin((10.0 / 3.0) * x);
+  };
+
+  constexpr double stepSize = 10.0;
+
+  std::random_device rd;
+  std::mt19937 gen{rd()};
+  std::uniform_real_distribution<> distr{0.0, 1.0};
+
+  frc::SimulatedAnnealing<double> simulatedAnnealing{
+      2.0,
+      [&](const double& x) {
+        return std::clamp(x + (distr(gen) - 0.5) * stepSize, 0.0, 7.0);
+      },
+      [&](const double& x) { return function(x); }};
+
+  double solution = simulatedAnnealing.Solve(-1.0, 5000);
+
+  EXPECT_NEAR(5.146, solution, 1e-1);
+}

wpimath/src/test/native/cpp/path/TravelingSalesmanTest.cpp

@@ -0,0 +1,151 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <cassert>
+#include <span>
+#include <vector>
+
+#include <gtest/gtest.h>
+#include <wpi/array.h>
+#include <wpi/circular_buffer.h>
+
+#include "frc/EigenCore.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/path/TravelingSalesman.h"
+
+/**
+ * Returns true if the cycles represented by the two lists match.
+ *
+ * @param expected The expected cycle.
+ * @param actual The actual cycle.
+ */
+bool IsMatchingCycle(std::span<const frc::Pose2d> expected,
+                     std::span<const frc::Pose2d> actual) {
+  assert(expected.size() == actual.size());
+
+  // Find first element in actual that matches expected
+  size_t actualStart = 0;
+  while (actual[actualStart] != expected[0]) {
+    ++actualStart;
+  }
+
+  // Check actual has expected cycle (forward)
+  wpi::circular_buffer<frc::Pose2d> actualBufferForward{expected.size()};
+  for (size_t i = 0; i < actual.size(); ++i) {
+    actualBufferForward.push_back(actual[(actualStart + i) % actual.size()]);
+  }
+  bool matchesExpectedForward = true;
+  for (size_t i = 0; i < expected.size(); ++i) {
+    matchesExpectedForward &= (expected[i] == actualBufferForward[i]);
+  }
+
+  // Check actual has expected cycle (reverse)
+  wpi::circular_buffer<frc::Pose2d> actualBufferReverse{expected.size()};
+  for (size_t i = 0; i < actual.size(); ++i) {
+    actualBufferReverse.push_front(
+        actual[(actualStart + 1 + i) % actual.size()]);
+  }
+  bool matchesExpectedReverse = true;
+  for (size_t i = 0; i < expected.size(); ++i) {
+    matchesExpectedReverse &= (expected[i] == actualBufferReverse[i]);
+  }
+
+  // Actual may be reversed from expected, but that's still valid
+  return matchesExpectedForward || matchesExpectedReverse;
+}
+
+TEST(TravelingSalesmanTest, FiveLengthStaticPathWithDistanceCost) {
+  // ...................
+  // ........2..........
+  // ..0..........4.....
+  // ...................
+  // ....3.....1........
+  // ...................
+  wpi::array<frc::Pose2d, 5> poses{
+      frc::Pose2d{3_m, 3_m, 0_rad}, frc::Pose2d{11_m, 5_m, 0_rad},
+      frc::Pose2d{9_m, 2_m, 0_rad}, frc::Pose2d{5_m, 5_m, 0_rad},
+      frc::Pose2d{14_m, 3_m, 0_rad}};
+
+  frc::TravelingSalesman traveler;
+  wpi::array<frc::Pose2d, 5> solution = traveler.Solve(poses, 500);
+
+  wpi::array<frc::Pose2d, 5> expected{poses[0], poses[2], poses[4], poses[1],
+                                      poses[3]};
+
+  EXPECT_TRUE(IsMatchingCycle(expected, solution));
+}
+
+TEST(TravelingSalesmanTest, FiveLengthDynamicPathWithDistanceCost) {
+  // ...................
+  // ........2..........
+  // ..0..........4.....
+  // ...................
+  // ....3.....1........
+  // ...................
+  wpi::array<frc::Pose2d, 5> poses{
+      frc::Pose2d{3_m, 3_m, 0_rad}, frc::Pose2d{11_m, 5_m, 0_rad},
+      frc::Pose2d{9_m, 2_m, 0_rad}, frc::Pose2d{5_m, 5_m, 0_rad},
+      frc::Pose2d{14_m, 3_m, 0_rad}};
+
+  frc::TravelingSalesman traveler;
+  std::vector<frc::Pose2d> solution =
+      traveler.Solve(std::span<const frc::Pose2d>{poses}, 500);
+
+  ASSERT_EQ(5u, solution.size());
+  wpi::array<frc::Pose2d, 5> expected{poses[0], poses[2], poses[4], poses[1],
+                                      poses[3]};
+
+  EXPECT_TRUE(IsMatchingCycle(expected, solution));
+}
+
+TEST(TravelingSalesmanTest, TenLengthStaticPathWithDistanceCost) {
+  // ....6.3..1.2.......
+  // ..4................
+  // .............9.....
+  // .0.................
+  // .....7..5...8......
+  // ...................
+  wpi::array<frc::Pose2d, 10> poses{
+      frc::Pose2d{2_m, 4_m, 0_rad},  frc::Pose2d{10_m, 1_m, 0_rad},
+      frc::Pose2d{12_m, 1_m, 0_rad}, frc::Pose2d{7_m, 1_m, 0_rad},
+      frc::Pose2d{3_m, 2_m, 0_rad},  frc::Pose2d{9_m, 5_m, 0_rad},
+      frc::Pose2d{5_m, 1_m, 0_rad},  frc::Pose2d{6_m, 5_m, 0_rad},
+      frc::Pose2d{13_m, 5_m, 0_rad}, frc::Pose2d{14_m, 3_m, 0_rad}};
+
+  frc::TravelingSalesman traveler;
+  wpi::array<frc::Pose2d, 10> solution = traveler.Solve(poses, 500);
+
+  wpi::array<frc::Pose2d, 10> expected{poses[0], poses[4], poses[6], poses[3],
+                                       poses[1], poses[2], poses[9], poses[8],
+                                       poses[5], poses[7]};
+
+  EXPECT_TRUE(IsMatchingCycle(expected, solution));
+}
+
+TEST(TravelingSalesmanTest, TenLengthDynamicPathWithDistanceCost) {
+  // ....6.3..1.2.......
+  // ..4................
+  // .............9.....
+  // .0.................
+  // .....7..5...8......
+  // ...................
+  wpi::array<frc::Pose2d, 10> poses{
+      frc::Pose2d{2_m, 4_m, 0_rad},  frc::Pose2d{10_m, 1_m, 0_rad},
+      frc::Pose2d{12_m, 1_m, 0_rad}, frc::Pose2d{7_m, 1_m, 0_rad},
+      frc::Pose2d{3_m, 2_m, 0_rad},  frc::Pose2d{9_m, 5_m, 0_rad},
+      frc::Pose2d{5_m, 1_m, 0_rad},  frc::Pose2d{6_m, 5_m, 0_rad},
+      frc::Pose2d{13_m, 5_m, 0_rad}, frc::Pose2d{14_m, 3_m, 0_rad}};
+
+  frc::TravelingSalesman traveler;
+  std::vector<frc::Pose2d> solution =
+      traveler.Solve(std::span<const frc::Pose2d>{poses}, 500);
+
+  ASSERT_EQ(10u, solution.size());
+  wpi::array<frc::Pose2d, 10> expected{poses[0], poses[4], poses[6], poses[3],
+                                       poses[1], poses[2], poses[9], poses[8],
+                                       poses[5], poses[7]};
+
+  EXPECT_TRUE(IsMatchingCycle(expected, solution));
+}