001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.optimization;
006
007import java.util.function.Function;
008import java.util.function.ToDoubleFunction;
009
010/**
011 * An implementation of the Simulated Annealing stochastic nonlinear optimization method.
012 *
013 * <p>Solving optimization problems involves tweaking decision variables to try to minimize some
014 * cost function. Simulated annealing is good for solving optimization problems with many local
015 * minima and a very large search space (it’s a heuristic solver rather than an exact solver like,
016 * say, SQP or interior-point method). Simulated annealing is a popular choice for solving the
017 * traveling salesman problem (see {@link edu.wpi.first.math.path.TravelingSalesman}).
018 *
019 * @see <a
020 *     href="https://en.wikipedia.org/wiki/Simulated_annealing">https://en.wikipedia.org/wiki/Simulated_annealing</a>
021 * @param <State> The type of the state to optimize.
022 */
023public final class SimulatedAnnealing<State> {
024  private final double m_initialTemperature;
025  private final Function<State, State> m_neighbor;
026  private final ToDoubleFunction<State> m_cost;
027
028  /**
029   * Constructor for Simulated Annealing that can be used for the same functions but with different
030   * initial states.
031   *
032   * @param initialTemperature The initial temperature. Higher temperatures make it more likely a
033   *     worse state will be accepted during iteration, helping to avoid local minima. The
034   *     temperature is decreased over time.
035   * @param neighbor Function that generates a random neighbor of the current state.
036   * @param cost Function that returns the scalar cost of a state.
037   */
038  public SimulatedAnnealing(
039      double initialTemperature, Function<State, State> neighbor, ToDoubleFunction<State> cost) {
040    m_initialTemperature = initialTemperature;
041    m_neighbor = neighbor;
042    m_cost = cost;
043  }
044
045  /**
046   * Runs the Simulated Annealing algorithm.
047   *
048   * @param initialGuess The initial state.
049   * @param iterations Number of iterations to run the solver.
050   * @return The optimized stater.
051   */
052  public State solve(State initialGuess, int iterations) {
053    State minState = initialGuess;
054    double minCost = Double.MAX_VALUE;
055
056    State state = initialGuess;
057    double cost = m_cost.applyAsDouble(state);
058
059    for (int i = 0; i < iterations; ++i) {
060      double temperature = m_initialTemperature / i;
061
062      State proposedState = m_neighbor.apply(state);
063      double proposedCost = m_cost.applyAsDouble(proposedState);
064      double deltaCost = proposedCost - cost;
065
066      double acceptanceProbability = Math.exp(-deltaCost / temperature);
067
068      // If cost went down or random number exceeded acceptance probability,
069      // accept the proposed state
070      if (deltaCost < 0 || acceptanceProbability >= Math.random()) {
071        state = proposedState;
072        cost = proposedCost;
073      }
074
075      // If proposed cost is less than minimum, the proposed state becomes the
076      // new minimum
077      if (proposedCost < minCost) {
078        minState = proposedState;
079        minCost = proposedCost;
080      }
081    }
082
083    return minState;
084  }
085}