commit 6ab8166ffc0c5a3f021622ebbd31ba31b1a60e72 Author: James Pace Date: Mon Jul 4 15:07:22 2022 +0000 Simple simple method rough first cut. diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..c834870 --- /dev/null +++ b/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 + $ + $) + +add_executable(main src/main.cpp) +target_compile_features(main PUBLIC cxx_std_17) # Require C++17 +target_include_directories(main PUBLIC + $ + $) +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() diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..ccd4150 --- /dev/null +++ b/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. \ No newline at end of file diff --git a/include/j7s-optimization/optimizer.hpp b/include/j7s-optimization/optimizer.hpp new file mode 100644 index 0000000..bd25784 --- /dev/null +++ b/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_ diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..06b2e8d --- /dev/null +++ b/package.xml @@ -0,0 +1,18 @@ + + + + j7s-optimization + 0.0.0 + TODO: Package description + jimmy + Proprietary + + ament_cmake_ros + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..4aa5400 --- /dev/null +++ b/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 +#include +#include +#include +#include + +#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 initSimplex); + + IterationState update(); + + Coordinate bestCoord() const; + +private: + const CostFunction m_costFunction; + std::vector m_currentSimplex; + + // Helper functions. + double newPoint(); + std::vector contract(); + double calcVolume(); +}; + +SimplexSolver::SimplexSolver(const CostFunction & costFunction, const std::vector 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 SimplexSolver::contract() +{ + const auto smallest = m_currentSimplex.front(); + + std::vector 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 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 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; +} diff --git a/src/optimizer.cpp b/src/optimizer.cpp new file mode 100644 index 0000000..69eb45a --- /dev/null +++ b/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