WPILibC++ 2024.3.2
TravelingSalesman.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#pragma once
6
7#include <cmath>
8#include <functional>
9#include <random>
10#include <utility>
11#include <vector>
12
13#include <wpi/array.h>
14
15#include "frc/EigenCore.h"
16#include "frc/geometry/Pose2d.h"
18
19namespace frc {
20
21/**
22 * Given a list of poses, this class finds the shortest possible route that
23 * visits each pose exactly once and returns to the origin pose.
24 *
25 * @see <a
26 * href="https://en.wikipedia.org/wiki/Travelling_salesman_problem">https://en.wikipedia.org/wiki/Travelling_salesman_problem</a>
27 */
29 public:
30 /**
31 * Constructs a traveling salesman problem solver with a cost function defined
32 * as the 2D distance between poses.
33 */
34 constexpr TravelingSalesman() = default;
35
36 /**
37 * Constructs a traveling salesman problem solver with a user-provided cost
38 * function.
39 *
40 * @param cost Function that returns the cost between two poses. The sum of
41 * the costs for every pair of poses is minimized.
42 */
43 explicit TravelingSalesman(std::function<double(Pose2d, Pose2d)> cost)
44 : m_cost{std::move(cost)} {}
45
46 /**
47 * Finds the path through every pose that minimizes the cost. The first pose
48 * in the returned array is the first pose that was passed in.
49 *
50 * This overload supports a statically-sized list of poses.
51 *
52 * @tparam Poses The length of the path and the number of poses.
53 * @param poses An array of Pose2ds the path must pass through.
54 * @param iterations The number of times the solver attempts to find a better
55 * random neighbor.
56 * @return The optimized path as an array of Pose2ds.
57 */
58 template <size_t Poses>
60 int iterations) {
62 1, &Neighbor<Poses>, [&](const Vectord<Poses>& state) {
63 // Total cost is sum of all costs between adjacent pairs in path
64 double sum = 0.0;
65 for (int i = 0; i < state.rows(); ++i) {
66 sum +=
67 m_cost(poses[static_cast<int>(state(i))],
68 poses[static_cast<int>(state((i + 1) % poses.size()))]);
69 }
70 return sum;
71 }};
72
73 Eigen::Vector<double, Poses> initial;
74 for (int i = 0; i < initial.rows(); ++i) {
75 initial(i) = i;
76 }
77
78 auto indices = solver.Solve(initial, iterations);
79
81 for (size_t i = 0; i < poses.size(); ++i) {
82 solution[i] = poses[static_cast<int>(indices[i])];
83 }
84
85 // Rotate solution list until solution[0] = poses[0]
86 std::rotate(solution.begin(),
87 std::find(solution.begin(), solution.end(), poses[0]),
88 solution.end());
89
90 return solution;
91 }
92
93 /**
94 * Finds the path through every pose that minimizes the cost. The first pose
95 * in the returned array is the first pose that was passed in.
96 *
97 * This overload supports a dynamically-sized list of poses for Python to use.
98 *
99 * @param poses An array of Pose2ds the path must pass through.
100 * @param iterations The number of times the solver attempts to find a better
101 * random neighbor.
102 * @return The optimized path as an array of Pose2ds.
103 */
104 std::vector<Pose2d> Solve(std::span<const Pose2d> poses, int iterations) {
106 1.0, &Neighbor<Eigen::Dynamic>, [&](const Eigen::VectorXd& state) {
107 // Total cost is sum of all costs between adjacent pairs in path
108 double sum = 0.0;
109 for (int i = 0; i < state.rows(); ++i) {
110 sum +=
111 m_cost(poses[static_cast<int>(state(i))],
112 poses[static_cast<int>(state((i + 1) % poses.size()))]);
113 }
114 return sum;
115 }};
116
117 Eigen::VectorXd initial{poses.size()};
118 for (int i = 0; i < initial.rows(); ++i) {
119 initial(i) = i;
120 }
121
122 auto indices = solver.Solve(initial, iterations);
123
124 std::vector<Pose2d> solution;
125 for (size_t i = 0; i < poses.size(); ++i) {
126 solution.emplace_back(poses[static_cast<int>(indices[i])]);
127 }
128
129 // Rotate solution list until solution[0] = poses[0]
130 std::rotate(solution.begin(),
131 std::find(solution.begin(), solution.end(), poses[0]),
132 solution.end());
133
134 return solution;
135 }
136
137 private:
138 // Default cost is distance between poses
139 std::function<double(const Pose2d&, const Pose2d&)> m_cost =
140 [](const Pose2d& a, const Pose2d& b) -> double {
141 return units::math::hypot(a.X() - b.X(), a.Y() - b.Y()).value();
142 };
143
144 /**
145 * A random neighbor is generated to try to replace the current one.
146 *
147 * @tparam Poses The length of the path and the number of poses.
148 * @param state A vector that is a list of indices that defines the path
149 * through the path array.
150 * @return Generates a random neighbor of the current state by flipping a
151 * random range in the path array.
152 */
153 template <int Poses>
154 static Eigen::Vector<double, Poses> Neighbor(
155 const Eigen::Vector<double, Poses>& state) {
156 Eigen::Vector<double, Poses> proposedState = state;
157
158 std::random_device rd;
159 std::mt19937 gen{rd()};
160 std::uniform_int_distribution<> distr{0,
161 static_cast<int>(state.rows()) - 1};
162
163 int rangeStart = distr(gen);
164 int rangeEnd = distr(gen);
165 if (rangeEnd < rangeStart) {
166 std::swap(rangeStart, rangeEnd);
167 }
168
169 for (int i = rangeStart; i <= (rangeStart + rangeEnd) / 2; ++i) {
170 double temp = proposedState(i, 0);
171 proposedState(i, 0) = state(rangeEnd - (i - rangeStart), 0);
172 proposedState(rangeEnd - (i - rangeStart), 0) = temp;
173 }
174
175 return proposedState;
176 }
177};
178
179} // namespace frc
Represents a 2D pose containing translational and rotational elements.
Definition: Pose2d.h:23
constexpr units::meter_t Y() const
Returns the Y component of the pose's translation.
Definition: Pose2d.h:96
constexpr units::meter_t X() const
Returns the X component of the pose's translation.
Definition: Pose2d.h:89
An implementation of the Simulated Annealing stochastic nonlinear optimization method.
Definition: SimulatedAnnealing.h:30
Given a list of poses, this class finds the shortest possible route that visits each pose exactly onc...
Definition: TravelingSalesman.h:28
TravelingSalesman(std::function< double(Pose2d, Pose2d)> cost)
Constructs a traveling salesman problem solver with a user-provided cost function.
Definition: TravelingSalesman.h:43
wpi::array< Pose2d, Poses > Solve(const wpi::array< Pose2d, Poses > &poses, int iterations)
Finds the path through every pose that minimizes the cost.
Definition: TravelingSalesman.h:59
std::vector< Pose2d > Solve(std::span< const Pose2d > poses, int iterations)
Finds the path through every pose that minimizes the cost.
Definition: TravelingSalesman.h:104
constexpr TravelingSalesman()=default
Constructs a traveling salesman problem solver with a cost function defined as the 2D distance betwee...
This class is a wrapper around std::array that does compile time size checking.
Definition: array.h:26
UnitTypeLhs hypot(const UnitTypeLhs &x, const UnitTypeRhs &y)
Computes the square root of the sum-of-squares of x and y.
Definition: math.h:505
FMT_CONSTEXPR auto find(Ptr first, Ptr last, T value, Ptr &out) -> bool
Definition: core.h:2120
state
Definition: core.h:2271
Definition: AprilTagPoseEstimator.h:15
Eigen::Vector< double, Size > Vectord
Definition: EigenCore.h:12
Definition: array.h:89
WPI_BASIC_JSON_TPL_DECLARATION void swap(wpi::WPI_BASIC_JSON_TPL &j1, wpi::WPI_BASIC_JSON_TPL &j2) noexcept(//NOLINT(readability-inconsistent-declaration-parameter-name) is_nothrow_move_constructible< wpi::WPI_BASIC_JSON_TPL >::value &&//NOLINT(misc-redundant-expression) is_nothrow_move_assignable< wpi::WPI_BASIC_JSON_TPL >::value)
exchanges the values of two JSON objects
Definition: json.h:5219
compound_unit< energy::joules, inverse< mass::kilogram > > rd