robot-rust 0.2.1

The robot-rust project, bindings for wpilib, ctre, and rev in rust.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101

// 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.
 *
 * Solving optimization problems involves tweaking decision variables to try to
 * minimize some cost function. Simulated annealing is good for solving
 * optimization problems with many local minima and a very large search space
 * (it’s a heuristic solver rather than an exact solver like, say, SQP or
 * interior-point method). Simulated annealing is a popular choice for solving
 * the traveling salesman problem (see TravelingSalesman).
 *
 * @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