Simple simple method rough first cut.

CMakeLists.txt

@@ -0,0 +1,51 @@
+cmake_minimum_required(VERSION 3.8)
+project(j7s-optimization)
+
+add_compile_options(-Wall -Wextra -Wpedantic)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
+find_package(rclcpp REQUIRED)
+
+add_library(optimizer src/optimizer.cpp)
+target_compile_features(optimizer PUBLIC cxx_std_17)  # Require C++17
+target_include_directories(optimizer PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+
+add_executable(main src/main.cpp)
+target_compile_features(main PUBLIC cxx_std_17)  # Require C++17
+target_include_directories(main PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(main rclcpp)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+install(
+  TARGETS
+    optimizer
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib/${PROJECT_NAME}
+  LIBRARY DESTINATION lib/${PROJECT_NAME}
+  RUNTIME DESTINATION bin/${PROJECT_NAME}
+)
+install(
+  TARGETS
+    main
+  DESTINATION lib/${PROJECT_NAME})
+
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  optimizer
+)
+ament_export_targets(
+  export_${PROJECT_NAME}
+)
+
+ament_package()

LICENSE

@@ -0,0 +1,9 @@
+Copyright 2022 James Pace
+All Rights Reserved.
+
+For a license to this software contact
+James Pace at jpace121@gmail.com.
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

include/j7s-optimization/optimizer.hpp

@@ -0,0 +1,26 @@
+// Copyright 2022 James Pace
+// All Rights Reserved.
+//
+// For a license to this software contact
+// James Pace at jpace121@gmail.com.
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#ifndef J7S__OPTIMIZER_HPP_
+#define J7S__OPTIMIZER_HPP_
+
+namespace j7s
+{
+
+class Optimizer
+{
+public:
+  Optimizer();
+
+  virtual ~Optimizer();
+};
+
+}  // namespace j7s
+
+#endif  // J7S__OPTIMIZER_HPP_

package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>j7s-optimization</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="jpace121@gmail.com">jimmy</maintainer>
+  <license>Proprietary</license>
+
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/main.cpp

@@ -0,0 +1,234 @@
+// Copyright 2022 James Pace
+// All Rights Reserved.
+//
+// For a license to this software contact
+// James Pace at jpace121@gmail.com.
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#include <cmath>
+#include <iostream>
+#include <vector>
+#include <algorithm>
+#include <stdexcept>
+
+#include "j7s-optimization/optimizer.hpp"
+
+class CostFunction
+{
+public:
+    CostFunction(double a, double b, double c);
+    double eval(double input) const;
+
+    double actualBest() const;
+
+private:
+    double m_a;
+    double m_b;
+    double m_c;
+};
+
+CostFunction::CostFunction(double a, double b, double c) : m_a{a}, m_b{b}, m_c{c} {}
+
+double CostFunction::eval(double input) const
+{
+    return m_a * std::pow(input, 2) + m_b * input + m_c;
+}
+
+double CostFunction::actualBest() const
+{
+    // y = a*x**2 + b*x + c
+    // dy/dx = 2*a*x + b
+    // 0 = 2*a*x + b
+    // -b/2*a = x
+
+    return (-m_b / (2 * m_a));
+}
+
+// A coordinate is pair of a cost input and the resulting the cost.
+// We save them together to minimize cost function evaluations.
+// This isn't the cleanest thing in the world, but it's worth saving
+// evaluations.
+struct Coordinate
+{
+    double input;
+    double cost;
+
+    Coordinate() : input{0.0}, cost{0.0} {};
+    Coordinate(double input, double cost) : input{input}, cost{cost} {};
+
+    // Sort by cost.
+    bool operator<(const Coordinate & other) const { return (cost < other.cost); }
+};
+
+enum class IterationState {
+    OK,
+    CONVERGED
+};
+
+class SimplexSolver
+{
+public:
+    SimplexSolver(const CostFunction & costFunction, const std::vector<double> initSimplex);
+
+    IterationState update();
+
+    Coordinate bestCoord() const;
+
+private:
+    const CostFunction m_costFunction;
+    std::vector<Coordinate> m_currentSimplex;
+
+    // Helper functions.
+    double newPoint();
+    std::vector<Coordinate> contract();
+    double calcVolume();
+};
+
+SimplexSolver::SimplexSolver(const CostFunction & costFunction, const std::vector<double> initSimplex):
+    m_costFunction{costFunction}
+{
+    m_currentSimplex.reserve(initSimplex.size());
+
+    for(const auto val : initSimplex)
+    {
+        m_currentSimplex.emplace_back(val, m_costFunction.eval(val));
+    }
+    std::sort(m_currentSimplex.begin(), m_currentSimplex.end());
+}
+
+Coordinate SimplexSolver::bestCoord() const
+{
+    return m_currentSimplex.front();
+}
+
+double SimplexSolver::newPoint()
+{
+    if (m_currentSimplex.size() == 0)
+    {
+        throw std::runtime_error("Simplex can't be missing.");
+    }
+
+    // Calculate sum.
+    double biggest = m_currentSimplex.back().input;
+    // Calculate volume.
+    double sum = 0.0;
+    // All but the biggest, (would be adding 0...)
+    for (unsigned int index = 0; index < m_currentSimplex.size() - 1; index++)
+    {
+        const double diff = m_currentSimplex[index].input - biggest;
+        sum += diff;
+    }
+
+    const double newPoint = sum * (2.0 / m_currentSimplex.size());
+
+    return newPoint;
+}
+
+std::vector<Coordinate> SimplexSolver::contract()
+{
+    const auto smallest = m_currentSimplex.front();
+
+    std::vector<Coordinate> newVector;
+    newVector.reserve(m_currentSimplex.size());
+    newVector.emplace_back(smallest);
+
+    // TODO: Really check size before I get here...
+    for(auto it = m_currentSimplex.begin() + 1; it != m_currentSimplex.end(); it++)
+    {
+        const auto oldInput = it->input;
+        const auto newInput = 0.5*(oldInput + smallest.input);
+        const auto newCost = m_costFunction.eval(newInput);
+        newVector.emplace_back(newInput, newCost);
+    }
+    std::sort(newVector.begin(), newVector.end());
+
+    return newVector;
+}
+
+double SimplexSolver::calcVolume()
+{
+    // TODO: For reals do something like:
+    // https://math.stackexchange.com/questions/337197/finding-the-volume-of-a-tetrahedron-by-given-vertices
+    // For now:
+    // Sort by input and find the difference squared between the first and last.
+
+    const auto inputLess = [](const Coordinate & first, const Coordinate & second)
+    { return first.input < second.input; };
+
+    // Copy the vector so we don't sort the original.
+    std::vector<Coordinate> simplexCopy = m_currentSimplex;
+    std::sort(simplexCopy.begin(), simplexCopy.end(), inputLess);
+    const auto smallest = simplexCopy.front();
+    const auto biggest = simplexCopy.back();
+    return std::pow(biggest.input - smallest.input, 2.0);
+}
+
+IterationState SimplexSolver::update()
+{
+    if (m_currentSimplex.size() < 3)
+    {
+        throw std::runtime_error("Simplex can't be a line.");
+    }
+    // Check for convergence and potentially early return.
+    // TODO: Make configurable.
+    const auto volume = calcVolume();
+    if(volume < 1e-4)
+    {
+        return IterationState::CONVERGED;
+    }
+
+    // Do update.
+    Coordinate potential;
+    potential.input = newPoint();
+    potential.cost = m_costFunction.eval(potential.input);
+
+    // TODO: Understand why looking at second biggest, not biggest.
+    // Explanation from paper is that it is in order to make
+    // the new guess of the next iteration different the current
+    // biggest.
+    const auto secondBiggest = *(m_currentSimplex.end() - 2);
+    if(potential.cost < secondBiggest.cost)
+    {
+        // Replace the last simplex value with the better one.
+        *(m_currentSimplex.end() - 1) = potential;
+        // TODO: DO I need to sort or is it sorted already?
+        std::sort(m_currentSimplex.begin(), m_currentSimplex.end());
+    }
+    else
+    {
+        // Do a contraction.
+        m_currentSimplex = contract();
+    }
+
+    return IterationState::OK;
+}
+
+int main(int, char **)
+{
+    const CostFunction cost(2.0, 3.0, 4.0);
+    const std::vector<double> init_simplex = {-10, 0, 10};
+    SimplexSolver solver(cost, init_simplex);
+
+    IterationState state = IterationState::OK;
+    for(int cnt = 0; cnt < 1000; cnt++)
+    {
+        state = solver.update();
+        if(state == IterationState::CONVERGED)
+        {
+            break;
+        }
+    }
+    if(state == IterationState::CONVERGED)
+    {
+        const auto best = solver.bestCoord();
+        std::cout << "Converged! Best Input: " << best.input << " Cost: " << best.cost << std::endl;
+        std::cout << "Actual Best: " << cost.actualBest() << " Cost: " << cost.eval(cost.actualBest()) << std::endl;
+    }
+    else
+    {
+        std::cout << "Did not converge." << std::endl;
+    }
+    return 0;
+}

src/optimizer.cpp

@@ -0,0 +1,23 @@
+// Copyright 2022 James Pace
+// All Rights Reserved.
+//
+// For a license to this software contact
+// James Pace at jpace121@gmail.com.
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#include "j7s-optimization/optimizer.hpp"
+
+namespace j7s
+{
+
+Optimizer::Optimizer()
+{
+}
+
+Optimizer::~Optimizer()
+{
+}
+
+}  // namespace j7s