Skip to content

Commit

Permalink
Add flag to disable diagnostics support at compile-time
Browse files Browse the repository at this point in the history
  • Loading branch information
calcmogul committed Jan 19, 2025
1 parent f987dc1 commit 4c5129d
Show file tree
Hide file tree
Showing 6 changed files with 80 additions and 8 deletions.
3 changes: 3 additions & 0 deletions .github/workflows/build-cpp.yml
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,9 @@ jobs:
- name: macOS universal
os: macOS-14
cmake-args: -DCMAKE_OSX_ARCHITECTURES="x86_64;arm64"
- name: Linux x86_64 (disable diagnostics)
os: ubuntu-24.04
cmake-args: -DDISABLE_DIAGNOSTICS=ON

name: ${{ matrix.name }}
runs-on: ${{ matrix.os }}
Expand Down
5 changes: 5 additions & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,7 @@ set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS FALSE)
option(BUILD_BENCHMARKING "Build CasADi and Sleipnir benchmarks" OFF)
option(BUILD_EXAMPLES "Build examples" OFF)
option(BUILD_PYTHON "Build Python module" OFF)
option(DISABLE_DIAGNOSTICS "Disable diagnostics support at compile-time" OFF)

include(CompilerFlags)

Expand Down Expand Up @@ -98,6 +99,10 @@ find_package(Threads REQUIRED)

target_link_libraries(Sleipnir PUBLIC Threads::Threads)

if(DISABLE_DIAGNOSTICS)
target_compile_definitions(Sleipnir PUBLIC SLEIPNIR_DISABLE_DIAGNOSTICS)
endif()

# Eigen dependency
if(NOT USE_SYSTEM_EIGEN)
set(EIGEN_BUILD_CMAKE_PACKAGE TRUE)
Expand Down
11 changes: 9 additions & 2 deletions include/sleipnir/optimization/OptimizationProblem.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
#pragma once

#include <algorithm>
#include <array>
#include <concepts>
#include <functional>
#include <iterator>
Expand All @@ -21,10 +20,14 @@
#include "sleipnir/optimization/solver/InteriorPoint.hpp"
#include "sleipnir/optimization/solver/Newton.hpp"
#include "sleipnir/optimization/solver/SQP.hpp"
#include "sleipnir/util/Print.hpp"
#include "sleipnir/util/SymbolExports.hpp"
#include "sleipnir/util/small_vector.hpp"

#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
#include <array>
#include "sleipnir/util/Print.hpp"
#endif

namespace sleipnir {

/**
Expand Down Expand Up @@ -274,6 +277,7 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
m_f = Variable();
}

#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
if (config.diagnostics) {
constexpr std::array kExprTypeToName{"empty", "constant", "linear",
"quadratic", "nonlinear"};
Expand All @@ -298,6 +302,7 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
sleipnir::println("Number of inequality constraints: {}\n",
m_inequalityConstraints.size());
}
#endif

// If the problem is empty or constant, there's nothing to do
if (status.costFunctionType <= ExpressionType::kConstant &&
Expand All @@ -319,9 +324,11 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
false, x, s, &status);
}

#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
if (config.diagnostics) {
sleipnir::println("Exit condition: {}", ToMessage(status.exitCondition));
}
#endif

// Assign the solution to the original Variable instances
VariableMatrix{m_decisionVariables}.SetValue(x);
Expand Down
27 changes: 25 additions & 2 deletions src/optimization/solver/InteriorPoint.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,12 +21,15 @@
#include "sleipnir/autodiff/Hessian.hpp"
#include "sleipnir/autodiff/Jacobian.hpp"
#include "sleipnir/optimization/SolverExitCondition.hpp"
#include "sleipnir/util/Print.hpp"
#include "sleipnir/util/Spy.hpp"
#include "sleipnir/util/small_vector.hpp"
#include "util/PrintIterationDiagnostics.hpp"
#include "util/ScopeExit.hpp"

#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
#include "sleipnir/util/Print.hpp"
#include "util/PrintIterationDiagnostics.hpp"
#include "util/ToMilliseconds.hpp"
#endif

// See docs/algorithms.md#Works_cited for citation definitions.
//
Expand Down Expand Up @@ -102,6 +105,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,

// Check for overconstrained problem
if (equalityConstraints.size() > decisionVariables.size()) {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
if (config.diagnostics) {
sleipnir::println("The problem has too few degrees of freedom.");
sleipnir::println(
Expand All @@ -112,6 +116,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
}
}
}
#endif

status->exitCondition = SolverExitCondition::kTooFewDOFs;
return;
Expand Down Expand Up @@ -139,9 +144,11 @@ void InteriorPoint(std::span<Variable> decisionVariables,
A_i.rows(), A_i.cols());
}

#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
if (config.diagnostics && !feasibilityRestoration) {
sleipnir::println("Error tolerance: {}\n", config.tolerance);
}
#endif

std::chrono::steady_clock::time_point iterationsStartTime;

Expand All @@ -151,6 +158,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
scope_exit exit{[&] {
status->cost = f.Value();

#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
if (config.diagnostics && !feasibilityRestoration) {
auto solveEndTime = std::chrono::steady_clock::now();

Expand Down Expand Up @@ -187,6 +195,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
jacobianCi.GetProfiler().SolveMeasurements());
sleipnir::println("");
}
#endif
}};

// Barrier parameter minimum
Expand Down Expand Up @@ -246,19 +255,24 @@ void InteriorPoint(std::span<Variable> decisionVariables,
// Error estimate
double E_0 = std::numeric_limits<double>::infinity();

#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
if (config.diagnostics) {
iterationsStartTime = std::chrono::steady_clock::now();
}
#endif

while (E_0 > config.tolerance &&
acceptableIterCounter < config.maxAcceptableIterations) {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
std::chrono::steady_clock::time_point innerIterStartTime;
if (config.diagnostics) {
innerIterStartTime = std::chrono::steady_clock::now();
}
#endif

// Check for local equality constraint infeasibility
if (IsEqualityLocallyInfeasible(A_e, c_e)) {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
if (config.diagnostics) {
sleipnir::println(
"The problem is locally infeasible due to violated equality "
Expand All @@ -271,13 +285,15 @@ void InteriorPoint(std::span<Variable> decisionVariables,
}
}
}
#endif

status->exitCondition = SolverExitCondition::kLocallyInfeasible;
return;
}

// Check for local inequality constraint infeasibility
if (IsInequalityLocallyInfeasible(A_i, c_i)) {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
if (config.diagnostics) {
sleipnir::println(
"The problem is infeasible due to violated inequality "
Expand All @@ -290,6 +306,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
}
}
}
#endif

status->exitCondition = SolverExitCondition::kLocallyInfeasible;
return;
Expand Down Expand Up @@ -464,10 +481,12 @@ void InteriorPoint(std::span<Variable> decisionVariables,
bool stepAcceptable = false;
for (int soc_iteration = 0; soc_iteration < 5 && !stepAcceptable;
++soc_iteration) {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
std::chrono::steady_clock::time_point socIterStartTime;
if (config.diagnostics) {
socIterStartTime = std::chrono::steady_clock::now();
}
#endif

// Rebuild Newton-KKT rhs with updated constraint values.
//
Expand Down Expand Up @@ -517,6 +536,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
stepAcceptable = true;
}

#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
if (config.diagnostics) {
const auto socIterEndTime = std::chrono::steady_clock::now();

Expand All @@ -527,6 +547,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
trial_c_e.lpNorm<1>() + (trial_c_i - trial_s).lpNorm<1>(),
solver.HessianRegularization(), 1.0);
}
#endif
}

if (stepAcceptable) {
Expand Down Expand Up @@ -801,6 +822,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,

const auto innerIterEndTime = std::chrono::steady_clock::now();

#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
if (config.diagnostics) {
PrintIterationDiagnostics(
iterations,
Expand All @@ -810,6 +832,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
c_e.lpNorm<1>() + (c_i - s).lpNorm<1>(),
solver.HessianRegularization(), α);
}
#endif

++iterations;

Expand Down
17 changes: 15 additions & 2 deletions src/optimization/solver/Newton.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,11 +17,14 @@
#include "sleipnir/autodiff/Gradient.hpp"
#include "sleipnir/autodiff/Hessian.hpp"
#include "sleipnir/optimization/SolverExitCondition.hpp"
#include "sleipnir/util/Print.hpp"
#include "sleipnir/util/Spy.hpp"
#include "util/PrintIterationDiagnostics.hpp"
#include "util/ScopeExit.hpp"

#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
#include "sleipnir/util/Print.hpp"
#include "util/PrintIterationDiagnostics.hpp"
#include "util/ToMilliseconds.hpp"
#endif

// See docs/algorithms.md#Works_cited for citation definitions.

Expand Down Expand Up @@ -66,9 +69,11 @@ void Newton(std::span<Variable> decisionVariables, Variable& f,
"Decision variables", H.rows(), H.cols());
}

#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
if (config.diagnostics) {
sleipnir::println("Error tolerance: {}\n", config.tolerance);
}
#endif

std::chrono::steady_clock::time_point iterationsStartTime;

Expand All @@ -78,6 +83,7 @@ void Newton(std::span<Variable> decisionVariables, Variable& f,
scope_exit exit{[&] {
status->cost = f.Value();

#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
if (config.diagnostics) {
auto solveEndTime = std::chrono::steady_clock::now();

Expand Down Expand Up @@ -106,6 +112,7 @@ void Newton(std::span<Variable> decisionVariables, Variable& f,
hessianL.GetProfiler().SolveMeasurements());
sleipnir::println("");
}
#endif
}};

Filter filter{f};
Expand All @@ -119,16 +126,20 @@ void Newton(std::span<Variable> decisionVariables, Variable& f,
// Error estimate
double E_0 = std::numeric_limits<double>::infinity();

#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
if (config.diagnostics) {
iterationsStartTime = std::chrono::steady_clock::now();
}
#endif

while (E_0 > config.tolerance &&
acceptableIterCounter < config.maxAcceptableIterations) {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
std::chrono::steady_clock::time_point innerIterStartTime;
if (config.diagnostics) {
innerIterStartTime = std::chrono::steady_clock::now();
}
#endif

// Check for diverging iterates
if (x.lpNorm<Eigen::Infinity>() > 1e20 || !x.allFinite()) {
Expand Down Expand Up @@ -247,12 +258,14 @@ void Newton(std::span<Variable> decisionVariables, Variable& f,

const auto innerIterEndTime = std::chrono::steady_clock::now();

#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
if (config.diagnostics) {
PrintIterationDiagnostics(iterations, IterationMode::kNormal,
innerIterEndTime - innerIterStartTime, E_0,
f.Value(), 0.0, solver.HessianRegularization(),
α);
}
#endif

++iterations;

Expand Down
Loading

0 comments on commit 4c5129d

Please sign in to comment.