Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Thad House
2024-12-08 12:04:23 -08:00
parent f1e4eafaa0 d5edb4060d
commit b6ae9e9cc9
22 changed files with 1014 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
developerRobot/build.gradle
View File

@@ -54,6 +54,9 @@ dependencies {
implementation project(':cameraserver')
implementation project(':wpilibNewCommands')
implementation project(':apriltag')
implementation project(':wpiunits')
implementation project(':epilogue-runtime')
annotationProcessor project(':epilogue-processor')
}
tasks.withType(com.github.spotbugs.snom.SpotBugsTask).configureEach {

4
upstream_utils/sleipnir.py
View File

@@ -48,8 +48,8 @@ def copy_upstream_src(wpilib_root):
def main():
name = "sleipnir"
url = "https://github.com/SleipnirGroup/Sleipnir"
# main on 2024-09-18
tag = "8bbce85252bc351c5aacb0de9f50fa31b8b9e1ae"
# main on 2024-12-07
tag = "01206ab17d741f4c45a7faeb56b8a5442df1681c"
sleipnir = Lib(name, url, tag, copy_upstream_src)
sleipnir.main()

132
upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch
View File

@@ -4,22 +4,23 @@ Date: Sun, 16 Jun 2024 12:08:49 -0700
Subject: [PATCH 2/3] Use wpi::SmallVector
---
include/.styleguide | 1 +
include/sleipnir/autodiff/Expression.hpp | 13 +++++++------
include/sleipnir/autodiff/ExpressionGraph.hpp | 15 ++++++++-------
include/sleipnir/autodiff/Hessian.hpp | 4 ++--
include/sleipnir/autodiff/Jacobian.hpp | 10 +++++-----
include/sleipnir/autodiff/Variable.hpp | 10 +++++-----
include/sleipnir/autodiff/VariableMatrix.hpp | 4 ++--
include/sleipnir/optimization/Multistart.hpp | 7 ++++---
.../sleipnir/optimization/OptimizationProblem.hpp | 8 ++++----
include/sleipnir/util/Pool.hpp | 7 ++++---
include/sleipnir/util/Spy.hpp | 4 ++--
src/.styleguide | 1 +
src/optimization/solver/InteriorPoint.cpp | 4 ++--
.../solver/util/FeasibilityRestoration.hpp | 10 +++++-----
src/optimization/solver/util/Filter.hpp | 4 ++--
15 files changed, 54 insertions(+), 48 deletions(-)
include/.styleguide | 1 +
include/sleipnir/autodiff/Expression.hpp | 13 +++++++------
include/sleipnir/autodiff/ExpressionGraph.hpp | 15 ++++++++-------
include/sleipnir/autodiff/Hessian.hpp | 4 ++--
include/sleipnir/autodiff/Jacobian.hpp | 10 +++++-----
include/sleipnir/autodiff/Variable.hpp | 10 +++++-----
include/sleipnir/autodiff/VariableMatrix.hpp | 4 ++--
include/sleipnir/optimization/Multistart.hpp | 7 ++++---
.../optimization/OptimizationProblem.hpp | 8 ++++----
include/sleipnir/util/Pool.hpp | 7 ++++---
include/sleipnir/util/Spy.hpp | 4 ++--
src/.styleguide | 1 +
src/optimization/solver/InteriorPoint.cpp | 4 ++--
src/optimization/solver/SQP.cpp | 4 ++--
.../solver/util/FeasibilityRestoration.hpp | 18 +++++++++---------
src/optimization/solver/util/Filter.hpp | 4 ++--
16 files changed, 60 insertions(+), 54 deletions(-)
diff --git a/include/.styleguide b/include/.styleguide
index 6a7f8ed28f9cb037c9746a7e0ef5e110481d9825..efa36cee1fb593ae810032340c64f1854fbbc523 100644
@@ -334,7 +335,7 @@ index 8055713a2492a9c8473f047a2fb9fe7ca57753c3..09b1b2f3bf33c35ae0aeddb9b5d47c0d
for (auto& future : futures) {
diff --git a/include/sleipnir/optimization/OptimizationProblem.hpp b/include/sleipnir/optimization/OptimizationProblem.hpp
index 28b2943f5842229335e79eae14abbda6ff5b7000..e812fa5e3454903d4dfb8572539ebf1b506bdf11 100644
index 569dcdee507881ceb412585ca811927072552c15..66883fed98ad087010fb153bd91effce6e047928 100644
--- a/include/sleipnir/optimization/OptimizationProblem.hpp
+++ b/include/sleipnir/optimization/OptimizationProblem.hpp
@@ -11,6 +11,7 @@
@@ -345,15 +346,15 @@ index 28b2943f5842229335e79eae14abbda6ff5b7000..e812fa5e3454903d4dfb8572539ebf1b
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/autodiff/VariableMatrix.hpp"
@@ -21,7 +22,6 @@
#include "sleipnir/optimization/solver/InteriorPoint.hpp"
@@ -22,7 +23,6 @@
#include "sleipnir/optimization/solver/SQP.hpp"
#include "sleipnir/util/Print.hpp"
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -358,16 +358,16 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
@@ -364,16 +364,16 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
private:
// The list of decision variables, which are the root of the problem's
// expression tree
@@ -434,7 +435,7 @@ index f3b2f0cf9e60b3a86b9654ff2b381f9c48734ff6..ad739cea6dce6f6cb586f538d1d30b92
+ ^wpi/
}
diff --git a/src/optimization/solver/InteriorPoint.cpp b/src/optimization/solver/InteriorPoint.cpp
index dcd8b56dc08516b80f89550c43cb7002745b93d8..892d2dd20f7fe92c2c91518a4ecb487425643585 100644
index a09d9866d05731c8ce53122b3d1a850803883df4..d3981c59d163927e3e5ba602c3323f6e1429c475 100644
--- a/src/optimization/solver/InteriorPoint.cpp
+++ b/src/optimization/solver/InteriorPoint.cpp
@@ -9,6 +9,7 @@
@@ -462,8 +463,37 @@ index dcd8b56dc08516b80f89550c43cb7002745b93d8..892d2dd20f7fe92c2c91518a4ecb4874
RegularizedLDLT solver;
diff --git a/src/optimization/solver/SQP.cpp b/src/optimization/solver/SQP.cpp
index 77b9632e1da37361c995a8579d1d83a2756d6d88..662abc2fb6e311767b0fbb3a61121ce78549a3f6 100644
--- a/src/optimization/solver/SQP.cpp
+++ b/src/optimization/solver/SQP.cpp
@@ -9,6 +9,7 @@
#include <limits>
#include <Eigen/SparseCholesky>
+#include <wpi/SmallVector.h>
#include "optimization/RegularizedLDLT.hpp"
#include "optimization/solver/util/ErrorEstimate.hpp"
@@ -22,7 +23,6 @@
#include "sleipnir/optimization/SolverExitCondition.hpp"
#include "sleipnir/util/Print.hpp"
#include "sleipnir/util/Spy.hpp"
-#include "sleipnir/util/small_vector.hpp"
#include "util/ScopeExit.hpp"
#include "util/ToMilliseconds.hpp"
@@ -155,7 +155,7 @@ void SQP(std::span<Variable> decisionVariables,
Filter filter{f};
// Kept outside the loop so its storage can be reused
- small_vector<Eigen::Triplet<double>> triplets;
+ wpi::SmallVector<Eigen::Triplet<double>> triplets;
RegularizedLDLT solver;
diff --git a/src/optimization/solver/util/FeasibilityRestoration.hpp b/src/optimization/solver/util/FeasibilityRestoration.hpp
index c324ef093cc7dc8ce93af2cba337042a65b28475..0f13676aea0e80549ef1ef43e4972d5498acaa18 100644
index feefe137adf0832b094a36d61201b15962138ded..79b5d99ae27de6049ba098888a965291e6b677fa 100644
--- a/src/optimization/solver/util/FeasibilityRestoration.hpp
+++ b/src/optimization/solver/util/FeasibilityRestoration.hpp
@@ -8,6 +8,7 @@
@@ -482,16 +512,16 @@ index c324ef093cc7dc8ce93af2cba337042a65b28475..0f13676aea0e80549ef1ef43e4972d54
namespace sleipnir {
@@ -65,7 +65,7 @@ inline void FeasibilityRestoration(
@@ -57,7 +57,7 @@ inline void FeasibilityRestoration(
constexpr double ρ = 1000.0;
double μ = config.tolerance / 10.0;
- small_vector<Variable> fr_decisionVariables;
+ wpi::SmallVector<Variable> fr_decisionVariables;
fr_decisionVariables.reserve(decisionVariables.size() +
2 * equalityConstraints.size() +
2 * inequalityConstraints.size());
@@ -81,7 +81,7 @@ inline void FeasibilityRestoration(
2 * equalityConstraints.size());
@@ -70,7 +70,7 @@ inline void FeasibilityRestoration(
fr_decisionVariables.emplace_back();
}
@@ -500,7 +530,7 @@ index c324ef093cc7dc8ce93af2cba337042a65b28475..0f13676aea0e80549ef1ef43e4972d54
VariableMatrix xAD{std::span{it, it + decisionVariables.size()}};
it += decisionVariables.size();
@@ -157,7 +157,7 @@ inline void FeasibilityRestoration(
@@ -128,7 +128,7 @@ inline void FeasibilityRestoration(
}
// cₑ(x) - pₑ + nₑ = 0
@@ -509,7 +539,43 @@ index c324ef093cc7dc8ce93af2cba337042a65b28475..0f13676aea0e80549ef1ef43e4972d54
fr_equalityConstraints.assign(equalityConstraints.begin(),
equalityConstraints.end());
for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) {
@@ -166,7 +166,7 @@ inline void FeasibilityRestoration(
@@ -137,7 +137,7 @@ inline void FeasibilityRestoration(
}
// cᵢ(x) - s - pᵢ + nᵢ = 0
- small_vector<Variable> fr_inequalityConstraints;
+ wpi::SmallVector<Variable> fr_inequalityConstraints;
// pₑ ≥ 0
std::copy(p_e.begin(), p_e.end(),
@@ -227,7 +227,7 @@ inline void FeasibilityRestoration(
constexpr double ρ = 1000.0;
- small_vector<Variable> fr_decisionVariables;
+ wpi::SmallVector<Variable> fr_decisionVariables;
fr_decisionVariables.reserve(decisionVariables.size() +
2 * equalityConstraints.size() +
2 * inequalityConstraints.size());
@@ -243,7 +243,7 @@ inline void FeasibilityRestoration(
fr_decisionVariables.emplace_back();
}
- auto it = fr_decisionVariables.cbegin();
+ auto it = fr_decisionVariables.begin();
VariableMatrix xAD{std::span{it, it + decisionVariables.size()}};
it += decisionVariables.size();
@@ -319,7 +319,7 @@ inline void FeasibilityRestoration(
}
// cₑ(x) - pₑ + nₑ = 0
- small_vector<Variable> fr_equalityConstraints;
+ wpi::SmallVector<Variable> fr_equalityConstraints;
fr_equalityConstraints.assign(equalityConstraints.begin(),
equalityConstraints.end());
for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) {
@@ -328,7 +328,7 @@ inline void FeasibilityRestoration(
}
// cᵢ(x) - s - pᵢ + nᵢ = 0
@@ -519,7 +585,7 @@ index c324ef093cc7dc8ce93af2cba337042a65b28475..0f13676aea0e80549ef1ef43e4972d54
inequalityConstraints.end());
for (size_t row = 0; row < fr_inequalityConstraints.size(); ++row) {
diff --git a/src/optimization/solver/util/Filter.hpp b/src/optimization/solver/util/Filter.hpp
index 3fbb849edc4a6b3336f94b5af9e018a37b07b123..02bdb5a8db5c80dd86d17ea4421ec564d7e0a2c7 100644
index f19236928c59623bc0a3ce87b659f0756997f821..0c14efd7b8afa6cef398f5a7d383c54dbf64ec68 100644
--- a/src/optimization/solver/util/Filter.hpp
+++ b/src/optimization/solver/util/Filter.hpp
@@ -8,9 +8,9 @@
@@ -531,12 +597,12 @@ index 3fbb849edc4a6b3336f94b5af9e018a37b07b123..02bdb5a8db5c80dd86d17ea4421ec564
#include "sleipnir/autodiff/Variable.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
// See docs/algorithms.md#Works_cited for citation definitions.
@@ -177,7 +177,7 @@ class Filter {
@@ -182,7 +182,7 @@ class Filter {
private:
Variable* m_f = nullptr;
double m_μ = 0.0;
- small_vector<FilterEntry> m_filter;
+ wpi::SmallVector<FilterEntry> m_filter;
};

6
wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java
View File

@@ -199,7 +199,7 @@ public class Ellipse2d implements ProtobufSerializable, StructSerializable {
* @return The distance (0, if the point is contained by the ellipse)
*/
public double getDistance(Translation2d point) {
return findNearestPoint(point).getDistance(point);
return nearest(point).getDistance(point);
}
/**
@@ -218,7 +218,7 @@ public class Ellipse2d implements ProtobufSerializable, StructSerializable {
* @param point The point that this will find the nearest point to.
* @return A new point that is nearest to {@code point} and contained in the ellipse.
*/
public Translation2d findNearestPoint(Translation2d point) {
public Translation2d nearest(Translation2d point) {
// Check if already in ellipse
if (contains(point)) {
return point;
@@ -226,7 +226,7 @@ public class Ellipse2d implements ProtobufSerializable, StructSerializable {
// Find nearest point
var nearestPoint = new double[2];
Ellipse2dJNI.findNearestPoint(
Ellipse2dJNI.nearest(
m_center.getX(),
m_center.getY(),
m_center.getRotation().getRadians(),

4
wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java
View File

@@ -186,7 +186,7 @@ public class Rectangle2d implements ProtobufSerializable, StructSerializable {
* @return The distance (0, if the point is contained by the rectangle)
*/
public double getDistance(Translation2d point) {
return findNearestPoint(point).getDistance(point);
return nearest(point).getDistance(point);
}
/**
@@ -205,7 +205,7 @@ public class Rectangle2d implements ProtobufSerializable, StructSerializable {
* @param point The point that this will find the nearest point to.
* @return A new point that is nearest to {@code point} and contained in the rectangle.
*/
public Translation2d findNearestPoint(Translation2d point) {
public Translation2d nearest(Translation2d point) {
// Check if already in rectangle
if (contains(point)) {
return point;

4
wpimath/src/main/java/edu/wpi/first/math/jni/Ellipse2dJNI.java
View File

@@ -9,7 +9,7 @@ public final class Ellipse2dJNI extends WPIMathJNI {
/**
* Returns the nearest point that is contained within the ellipse.
*
* <p>Constructs an Ellipse2d object and runs its FindNearestPoint() method.
* <p>Constructs an Ellipse2d object and runs its nearest() method.
*
* @param centerX The x coordinate of the center of the ellipse in meters.
* @param centerY The y coordinate of the center of the ellipse in meters.
@@ -20,7 +20,7 @@ public final class Ellipse2dJNI extends WPIMathJNI {
* @param pointY The y coordinate of the point that this will find the nearest point to.
* @param nearestPoint Array to store nearest point into.
*/
public static native void findNearestPoint(
public static native void nearest(
double centerX,
double centerY,
double centerHeading,

2
wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp
View File

@@ -8,7 +8,7 @@
using namespace frc;
Translation2d Ellipse2d::FindNearestPoint(const Translation2d& point) const {
Translation2d Ellipse2d::Nearest(const Translation2d& point) const {
// Check if already in ellipse
if (Contains(point)) {
return point;

6
wpimath/src/main/native/cpp/jni/Ellipse2dJNI.cpp
View File

@@ -16,11 +16,11 @@ extern "C" {
/*
* Class: edu_wpi_first_math_jni_Ellipse2dJNI
* Method: findNearestPoint
* Method: nearest
* Signature: (DDDDDDD[D)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_math_jni_Ellipse2dJNI_findNearestPoint
Java_edu_wpi_first_math_jni_Ellipse2dJNI_nearest
(JNIEnv* env, jclass, jdouble centerX, jdouble centerY, jdouble centerHeading,
jdouble xSemiAxis, jdouble ySemiAxis, jdouble pointX, jdouble pointY,
jdoubleArray nearestPoint)
@@ -30,7 +30,7 @@ Java_edu_wpi_first_math_jni_Ellipse2dJNI_findNearestPoint
frc::Pose2d{units::meter_t{centerX}, units::meter_t{centerY},
units::radian_t{centerHeading}},
units::meter_t{xSemiAxis}, units::meter_t{ySemiAxis}}
.FindNearestPoint({units::meter_t{pointX}, units::meter_t{pointY}});
.Nearest({units::meter_t{pointX}, units::meter_t{pointY}});
wpi::array buf{point.X().value(), point.Y().value()};
env->SetDoubleArrayRegion(nearestPoint, 0, 2, buf.data());

4
wpimath/src/main/native/include/frc/geometry/Ellipse2d.h
View File

@@ -154,7 +154,7 @@ class WPILIB_DLLEXPORT Ellipse2d {
* @return The distance (0, if the point is contained by the ellipse)
*/
units::meter_t Distance(const Translation2d& point) const {
return FindNearestPoint(point).Distance(point);
return Nearest(point).Distance(point);
}
/**
@@ -164,7 +164,7 @@ class WPILIB_DLLEXPORT Ellipse2d {
* @return A new point that is nearest to {@code point} and contained in the
* ellipse.
*/
Translation2d FindNearestPoint(const Translation2d& point) const;
Translation2d Nearest(const Translation2d& point) const;
/**
* Checks equality between this Ellipse2d and another object.

4
wpimath/src/main/native/include/frc/geometry/Rectangle2d.h
View File

@@ -155,7 +155,7 @@ class WPILIB_DLLEXPORT Rectangle2d {
* @return The distance (0, if the point is contained by the rectangle)
*/
constexpr units::meter_t Distance(const Translation2d& point) const {
return FindNearestPoint(point).Distance(point);
return Nearest(point).Distance(point);
}
/**
@@ -165,7 +165,7 @@ class WPILIB_DLLEXPORT Rectangle2d {
* @return A new point that is nearest to {@code point} and contained in the
* rectangle.
*/
constexpr Translation2d FindNearestPoint(const Translation2d& point) const {
constexpr Translation2d Nearest(const Translation2d& point) const {
// Check if already in rectangle
if (Contains(point)) {
return point;

14
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp vendored
View File

@@ -20,6 +20,7 @@
#include "sleipnir/optimization/SolverIterationInfo.hpp"
#include "sleipnir/optimization/SolverStatus.hpp"
#include "sleipnir/optimization/solver/InteriorPoint.hpp"
#include "sleipnir/optimization/solver/SQP.hpp"
#include "sleipnir/util/Print.hpp"
#include "sleipnir/util/SymbolExports.hpp"
@@ -305,10 +306,15 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
}
// Solve the optimization problem
Eigen::VectorXd s = Eigen::VectorXd::Ones(m_inequalityConstraints.size());
InteriorPoint(m_decisionVariables, m_equalityConstraints,
m_inequalityConstraints, m_f.value(), m_callback, config,
false, x, s, &status);
if (m_inequalityConstraints.empty()) {
SQP(m_decisionVariables, m_equalityConstraints, m_f.value(), m_callback,
config, x, &status);
} else {
Eigen::VectorXd s = Eigen::VectorXd::Ones(m_inequalityConstraints.size());
InteriorPoint(m_decisionVariables, m_equalityConstraints,
m_inequalityConstraints, m_f.value(), m_callback, config,
false, x, s, &status);
}
if (config.diagnostics) {
sleipnir::println("Exit condition: {}", ToMessage(status.exitCondition));

46
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/SQP.hpp vendored Normal file
View File

@@ -0,0 +1,46 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <span>
#include <Eigen/Core>
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/optimization/SolverConfig.hpp"
#include "sleipnir/optimization/SolverIterationInfo.hpp"
#include "sleipnir/optimization/SolverStatus.hpp"
#include "sleipnir/util/FunctionRef.hpp"
#include "sleipnir/util/SymbolExports.hpp"
namespace sleipnir {
/**
Finds the optimal solution to a nonlinear program using Sequential Quadratic
Programming (SQP).
A nonlinear program has the form:
@verbatim
min_x f(x)
subject to cₑ(x) = 0
@endverbatim
where f(x) is the cost function and cₑ(x) are the equality constraints.
@param[in] decisionVariables The list of decision variables.
@param[in] equalityConstraints The list of equality constraints.
@param[in] f The cost function.
@param[in] callback The user callback.
@param[in] config Configuration options for the solver.
@param[in,out] x The initial guess and output location for the decision
variables.
@param[out] status The solver status.
*/
SLEIPNIR_DLLEXPORT void SQP(
std::span<Variable> decisionVariables,
std::span<Variable> equalityConstraints, Variable& f,
function_ref<bool(const SolverIterationInfo& info)> callback,
const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status);
} // namespace sleipnir

17
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp vendored
View File

@@ -195,7 +195,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
// Fraction-to-the-boundary rule scale factor τ
double τ = τ_min;
Filter filter{f, μ};
Filter filter{f};
// This should be run when the error estimate is below a desired threshold for
// the current barrier parameter
@@ -222,7 +222,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
τ = std::max(τ_min, 1.0 - μ);
// Reset the filter when the barrier parameter is updated
filter.Reset(μ);
filter.Reset();
};
// Kept outside the loop so its storage can be reused
@@ -372,6 +372,9 @@ void InteriorPoint(std::span<Variable> decisionVariables,
rhs.segment(x.rows(), y.rows()) = -c_e;
// Solve the Newton-KKT system
//
// [H + AᵢᵀΣAᵢ Aₑᵀ][ pₖˣ] = −[∇f − Aₑᵀy + Aᵢᵀ(S⁻¹(Zcᵢ − μe) − z)]
// [ Aₑ 0 ][−pₖʸ] [ cₑ ]
solver.Compute(lhs, equalityConstraints.size(), μ);
Eigen::VectorXd step{x.rows() + y.rows()};
if (solver.Info() == Eigen::Success) {
@@ -435,7 +438,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
}
// Check whether filter accepts trial iterate
auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i);
auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i, μ);
if (filter.TryAdd(entry)) {
// Accept step
break;
@@ -499,7 +502,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
trial_c_i = c_iAD.Value();
// Check whether filter accepts trial iterate
entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i);
entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i, μ);
if (filter.TryAdd(entry)) {
p_x = p_x_cor;
p_y = p_y_soc;
@@ -531,7 +534,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
if (fullStepRejectedCounter >= 4 &&
filter.maxConstraintViolation > entry.constraintViolation / 10.0) {
filter.maxConstraintViolation *= 0.1;
filter.Reset(μ);
filter.Reset();
continue;
}
@@ -580,7 +583,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
return;
}
auto initialEntry = filter.MakeEntry(s, c_e, c_i);
auto initialEntry = filter.MakeEntry(s, c_e, c_i, μ);
// Feasibility restoration phase
Eigen::VectorXd fr_x = x;
@@ -603,7 +606,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
// If current iterate is acceptable to normal filter and
// constraint violation has sufficiently reduced, stop
// feasibility restoration
auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i);
auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i, μ);
if (filter.IsAcceptable(entry) &&
entry.constraintViolation <
0.9 * initialEntry.constraintViolation) {

559
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp vendored Normal file
View File

@@ -0,0 +1,559 @@
// Copyright (c) Sleipnir contributors
#include "sleipnir/optimization/solver/SQP.hpp"
#include <algorithm>
#include <chrono>
#include <cmath>
#include <fstream>
#include <limits>
#include <Eigen/SparseCholesky>
#include <wpi/SmallVector.h>
#include "optimization/RegularizedLDLT.hpp"
#include "optimization/solver/util/ErrorEstimate.hpp"
#include "optimization/solver/util/FeasibilityRestoration.hpp"
#include "optimization/solver/util/Filter.hpp"
#include "optimization/solver/util/IsLocallyInfeasible.hpp"
#include "optimization/solver/util/KKTError.hpp"
#include "sleipnir/autodiff/Gradient.hpp"
#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 "util/ScopeExit.hpp"
#include "util/ToMilliseconds.hpp"
// See docs/algorithms.md#Works_cited for citation definitions.
namespace sleipnir {
void SQP(std::span<Variable> decisionVariables,
std::span<Variable> equalityConstraints, Variable& f,
function_ref<bool(const SolverIterationInfo& info)> callback,
const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status) {
const auto solveStartTime = std::chrono::system_clock::now();
// Map decision variables and constraints to VariableMatrices for Lagrangian
VariableMatrix xAD{decisionVariables};
xAD.SetValue(x);
VariableMatrix c_eAD{equalityConstraints};
// Create autodiff variables for y for Lagrangian
VariableMatrix yAD(equalityConstraints.size());
for (auto& y : yAD) {
y.SetValue(0.0);
}
// Lagrangian L
//
// L(xₖ, yₖ) = f(xₖ) − yₖᵀcₑ(xₖ)
auto L = f - (yAD.T() * c_eAD)(0);
// Equality constraint Jacobian Aₑ
//
// [∇ᵀcₑ₁(xₖ)]
// Aₑ(x) = [∇ᵀcₑ₂(xₖ)]
// [ ⋮ ]
// [∇ᵀcₑₘ(xₖ)]
Jacobian jacobianCe{c_eAD, xAD};
Eigen::SparseMatrix<double> A_e = jacobianCe.Value();
// Gradient of f ∇f
Gradient gradientF{f, xAD};
Eigen::SparseVector<double> g = gradientF.Value();
// Hessian of the Lagrangian H
//
// Hₖ = ∇²ₓₓL(xₖ, yₖ)
Hessian hessianL{L, xAD};
Eigen::SparseMatrix<double> H = hessianL.Value();
Eigen::VectorXd y = yAD.Value();
Eigen::VectorXd c_e = c_eAD.Value();
// Check for overconstrained problem
if (equalityConstraints.size() > decisionVariables.size()) {
if (config.diagnostics) {
sleipnir::println("The problem has too few degrees of freedom.");
sleipnir::println(
"Violated constraints (cₑ(x) = 0) in order of declaration:");
for (int row = 0; row < c_e.rows(); ++row) {
if (c_e(row) < 0.0) {
sleipnir::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e(row));
}
}
}
status->exitCondition = SolverExitCondition::kTooFewDOFs;
return;
}
// Check whether initial guess has finite f(xₖ) and cₑ(xₖ)
if (!std::isfinite(f.Value()) || !c_e.allFinite()) {
status->exitCondition =
SolverExitCondition::kNonfiniteInitialCostOrConstraints;
return;
}
// Sparsity pattern files written when spy flag is set in SolverConfig
std::ofstream H_spy;
std::ofstream A_e_spy;
if (config.spy) {
A_e_spy.open("A_e.spy");
H_spy.open("H.spy");
}
if (config.diagnostics) {
sleipnir::println("Error tolerance: {}\n", config.tolerance);
}
std::chrono::system_clock::time_point iterationsStartTime;
int iterations = 0;
// Prints final diagnostics when the solver exits
scope_exit exit{[&] {
status->cost = f.Value();
if (config.diagnostics) {
auto solveEndTime = std::chrono::system_clock::now();
sleipnir::println("\nSolve time: {:.3f} ms",
ToMilliseconds(solveEndTime - solveStartTime));
sleipnir::println(" ↳ {:.3f} ms (solver setup)",
ToMilliseconds(iterationsStartTime - solveStartTime));
if (iterations > 0) {
sleipnir::println(
" ↳ {:.3f} ms ({} solver iterations; {:.3f} ms average)",
ToMilliseconds(solveEndTime - iterationsStartTime), iterations,
ToMilliseconds((solveEndTime - iterationsStartTime) / iterations));
}
sleipnir::println("");
sleipnir::println("{:^8} {:^10} {:^14} {:^6}", "autodiff",
"setup (ms)", "avg solve (ms)", "solves");
sleipnir::println("{:=^47}", "");
constexpr auto format = "{:^8} {:10.3f} {:14.3f} {:6}";
sleipnir::println(format, "∇f(x)",
gradientF.GetProfiler().SetupDuration(),
gradientF.GetProfiler().AverageSolveDuration(),
gradientF.GetProfiler().SolveMeasurements());
sleipnir::println(format, "∇²ₓₓL", hessianL.GetProfiler().SetupDuration(),
hessianL.GetProfiler().AverageSolveDuration(),
hessianL.GetProfiler().SolveMeasurements());
sleipnir::println(format, "∂cₑ/∂x",
jacobianCe.GetProfiler().SetupDuration(),
jacobianCe.GetProfiler().AverageSolveDuration(),
jacobianCe.GetProfiler().SolveMeasurements());
sleipnir::println("");
}
}};
Filter filter{f};
// Kept outside the loop so its storage can be reused
wpi::SmallVector<Eigen::Triplet<double>> triplets;
RegularizedLDLT solver;
// Variables for determining when a step is acceptable
constexpr double α_red_factor = 0.5;
int acceptableIterCounter = 0;
int fullStepRejectedCounter = 0;
// Error estimate
double E_0 = std::numeric_limits<double>::infinity();
if (config.diagnostics) {
iterationsStartTime = std::chrono::system_clock::now();
}
while (E_0 > config.tolerance &&
acceptableIterCounter < config.maxAcceptableIterations) {
std::chrono::system_clock::time_point innerIterStartTime;
if (config.diagnostics) {
innerIterStartTime = std::chrono::system_clock::now();
}
// Check for local equality constraint infeasibility
if (IsEqualityLocallyInfeasible(A_e, c_e)) {
if (config.diagnostics) {
sleipnir::println(
"The problem is locally infeasible due to violated equality "
"constraints.");
sleipnir::println(
"Violated constraints (cₑ(x) = 0) in order of declaration:");
for (int row = 0; row < c_e.rows(); ++row) {
if (c_e(row) < 0.0) {
sleipnir::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e(row));
}
}
}
status->exitCondition = SolverExitCondition::kLocallyInfeasible;
return;
}
// Check for diverging iterates
if (x.lpNorm<Eigen::Infinity>() > 1e20 || !x.allFinite()) {
status->exitCondition = SolverExitCondition::kDivergingIterates;
return;
}
// Write out spy file contents if that's enabled
if (config.spy) {
// Gap between sparsity patterns
if (iterations > 0) {
A_e_spy << "\n";
H_spy << "\n";
}
Spy(H_spy, H);
Spy(A_e_spy, A_e);
}
// Call user callback
if (callback({iterations, x, Eigen::VectorXd::Zero(0), g, H, A_e,
Eigen::SparseMatrix<double>{}})) {
status->exitCondition = SolverExitCondition::kCallbackRequestedStop;
return;
}
// lhs = [H Aₑᵀ]
// [Aₑ 0 ]
//
// Don't assign upper triangle because solver only uses lower triangle.
const Eigen::SparseMatrix<double> topLeft =
H.triangularView<Eigen::Lower>();
triplets.clear();
triplets.reserve(topLeft.nonZeros() + A_e.nonZeros());
for (int col = 0; col < H.cols(); ++col) {
// Append column of H + AᵢᵀΣAᵢ lower triangle in top-left quadrant
for (Eigen::SparseMatrix<double>::InnerIterator it{topLeft, col}; it;
++it) {
triplets.emplace_back(it.row(), it.col(), it.value());
}
// Append column of Aₑ in bottom-left quadrant
for (Eigen::SparseMatrix<double>::InnerIterator it{A_e, col}; it; ++it) {
triplets.emplace_back(H.rows() + it.row(), it.col(), it.value());
}
}
Eigen::SparseMatrix<double> lhs(
decisionVariables.size() + equalityConstraints.size(),
decisionVariables.size() + equalityConstraints.size());
lhs.setFromSortedTriplets(triplets.begin(), triplets.end(),
[](const auto&, const auto& b) { return b; });
// rhs = −[∇f − Aₑᵀy]
// [ cₑ ]
Eigen::VectorXd rhs{x.rows() + y.rows()};
rhs.segment(0, x.rows()) = -(g - A_e.transpose() * y);
rhs.segment(x.rows(), y.rows()) = -c_e;
// Solve the Newton-KKT system
//
// [H Aₑᵀ][ pₖˣ] = −[∇f − Aₑᵀy]
// [Aₑ 0 ][−pₖʸ] [ cₑ ]
solver.Compute(lhs, equalityConstraints.size(), config.tolerance / 10.0);
Eigen::VectorXd step{x.rows() + y.rows()};
if (solver.Info() == Eigen::Success) {
step = solver.Solve(rhs);
} else {
// The regularization procedure failed due to a rank-deficient equality
// constraint Jacobian with linearly dependent constraints
status->exitCondition = SolverExitCondition::kLocallyInfeasible;
return;
}
// step = [ pₖˣ]
// [−pₖʸ]
Eigen::VectorXd p_x = step.segment(0, x.rows());
Eigen::VectorXd p_y = -step.segment(x.rows(), y.rows());
constexpr double α_max = 1.0;
double α = α_max;
// Loop until a step is accepted. If a step becomes acceptable, the loop
// will exit early.
while (1) {
Eigen::VectorXd trial_x = x + α * p_x;
Eigen::VectorXd trial_y = y + α * p_y;
xAD.SetValue(trial_x);
Eigen::VectorXd trial_c_e = c_eAD.Value();
// If f(xₖ + αpₖˣ) or cₑ(xₖ + αpₖˣ) aren't finite, reduce step size
// immediately
if (!std::isfinite(f.Value()) || !trial_c_e.allFinite()) {
// Reduce step size
α *= α_red_factor;
continue;
}
// Check whether filter accepts trial iterate
auto entry = filter.MakeEntry(trial_c_e);
if (filter.TryAdd(entry)) {
// Accept step
break;
}
double prevConstraintViolation = c_e.lpNorm<1>();
double nextConstraintViolation = trial_c_e.lpNorm<1>();
// Second-order corrections
//
// If first trial point was rejected and constraint violation stayed the
// same or went up, apply second-order corrections
if (nextConstraintViolation >= prevConstraintViolation) {
// Apply second-order corrections. See section 2.4 of [2].
Eigen::VectorXd p_x_cor = p_x;
Eigen::VectorXd p_y_soc = p_y;
double α_soc = α;
Eigen::VectorXd c_e_soc = c_e;
bool stepAcceptable = false;
for (int soc_iteration = 0; soc_iteration < 5 && !stepAcceptable;
++soc_iteration) {
// Rebuild Newton-KKT rhs with updated constraint values.
//
// rhs = −[∇f − Aₑᵀy]
// [ cₑˢᵒᶜ ]
//
// where cₑˢᵒᶜ = αc(xₖ) + c(xₖ + αpₖˣ)
c_e_soc = α_soc * c_e_soc + trial_c_e;
rhs.bottomRows(y.rows()) = -c_e_soc;
// Solve the Newton-KKT system
step = solver.Solve(rhs);
p_x_cor = step.segment(0, x.rows());
p_y_soc = -step.segment(x.rows(), y.rows());
trial_x = x + α_soc * p_x_cor;
trial_y = y + α_soc * p_y_soc;
xAD.SetValue(trial_x);
trial_c_e = c_eAD.Value();
// Check whether filter accepts trial iterate
entry = filter.MakeEntry(trial_c_e);
if (filter.TryAdd(entry)) {
p_x = p_x_cor;
p_y = p_y_soc;
α = α_soc;
stepAcceptable = true;
}
}
if (stepAcceptable) {
// Accept step
break;
}
}
// If we got here and α is the full step, the full step was rejected.
// Increment the full-step rejected counter to keep track of how many full
// steps have been rejected in a row.
if (α == α_max) {
++fullStepRejectedCounter;
}
// If the full step was rejected enough times in a row, reset the filter
// because it may be impeding progress.
//
// See section 3.2 case I of [2].
if (fullStepRejectedCounter >= 4 &&
filter.maxConstraintViolation > entry.constraintViolation / 10.0) {
filter.maxConstraintViolation *= 0.1;
filter.Reset();
continue;
}
// Reduce step size
α *= α_red_factor;
// Safety factor for the minimal step size
constexpr double α_min_frac = 0.05;
// If step size hit a minimum, check if the KKT error was reduced. If it
// wasn't, report infeasible.
if (α < α_min_frac * Filter::γConstraint) {
double currentKKTError = KKTError(g, A_e, c_e, y);
Eigen::VectorXd trial_x = x + α_max * p_x;
Eigen::VectorXd trial_y = y + α_max * p_y;
// Upate autodiff
xAD.SetValue(trial_x);
yAD.SetValue(trial_y);
Eigen::VectorXd trial_c_e = c_eAD.Value();
double nextKKTError =
KKTError(gradientF.Value(), jacobianCe.Value(), trial_c_e, trial_y);
// If the step using αᵐᵃˣ reduced the KKT error, accept it anyway
if (nextKKTError <= 0.999 * currentKKTError) {
α = α_max;
// Accept step
break;
}
auto initialEntry = filter.MakeEntry(c_e);
// Feasibility restoration phase
Eigen::VectorXd fr_x = x;
SolverStatus fr_status;
FeasibilityRestoration(
decisionVariables, equalityConstraints,
[&](const SolverIterationInfo& info) {
trial_x = info.x.segment(0, decisionVariables.size());
xAD.SetValue(trial_x);
trial_c_e = c_eAD.Value();
// If current iterate is acceptable to normal filter and
// constraint violation has sufficiently reduced, stop
// feasibility restoration
entry = filter.MakeEntry(trial_c_e);
if (filter.IsAcceptable(entry) &&
entry.constraintViolation <
0.9 * initialEntry.constraintViolation) {
return true;
}
return false;
},
config, fr_x, &fr_status);
if (fr_status.exitCondition ==
SolverExitCondition::kCallbackRequestedStop) {
p_x = fr_x - x;
// Lagrange mutliplier estimates
//
// y = (AₑAₑᵀ)⁻¹Aₑ∇f
//
// See equation (19.37) of [1].
{
xAD.SetValue(fr_x);
A_e = jacobianCe.Value();
g = gradientF.Value();
// lhs = AₑAₑᵀ
Eigen::SparseMatrix<double> lhs = A_e * A_e.transpose();
// rhs = Aₑ∇f
Eigen::VectorXd rhs = A_e * g;
Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> yEstimator{lhs};
Eigen::VectorXd sol = yEstimator.solve(rhs);
p_y = y - sol.block(0, 0, y.rows(), 1);
}
α = 1.0;
// Accept step
break;
} else if (fr_status.exitCondition == SolverExitCondition::kSuccess) {
status->exitCondition = SolverExitCondition::kLocallyInfeasible;
x = fr_x;
return;
} else {
status->exitCondition =
SolverExitCondition::kFeasibilityRestorationFailed;
x = fr_x;
return;
}
}
}
// If full step was accepted, reset full-step rejected counter
if (α == α_max) {
fullStepRejectedCounter = 0;
}
// Handle very small search directions by letting αₖ = αₖᵐᵃˣ when
// max(|pₖˣ(i)|/(1 + |xₖ(i)|)) < 10ε_mach.
//
// See section 3.9 of [2].
double maxStepScaled = 0.0;
for (int row = 0; row < x.rows(); ++row) {
maxStepScaled = std::max(maxStepScaled,
std::abs(p_x(row)) / (1.0 + std::abs(x(row))));
}
if (maxStepScaled < 10.0 * std::numeric_limits<double>::epsilon()) {
α = α_max;
}
// xₖ₊₁ = xₖ + αₖpₖˣ
// yₖ₊₁ = yₖ + αₖpₖʸ
x += α * p_x;
y += α * p_y;
// Update autodiff for Jacobians and Hessian
xAD.SetValue(x);
yAD.SetValue(y);
A_e = jacobianCe.Value();
g = gradientF.Value();
H = hessianL.Value();
c_e = c_eAD.Value();
// Update the error estimate
E_0 = ErrorEstimate(g, A_e, c_e, y);
if (E_0 < config.acceptableTolerance) {
++acceptableIterCounter;
} else {
acceptableIterCounter = 0;
}
const auto innerIterEndTime = std::chrono::system_clock::now();
// Diagnostics for current iteration
if (config.diagnostics) {
if (iterations % 20 == 0) {
sleipnir::println("{:^4} {:^9} {:^13} {:^13} {:^13}", "iter",
"time (ms)", "error", "cost", "infeasibility");
sleipnir::println("{:=^61}", "");
}
sleipnir::println("{:4} {:9.3f} {:13e} {:13e} {:13e}", iterations,
ToMilliseconds(innerIterEndTime - innerIterStartTime),
E_0, f.Value(), c_e.lpNorm<1>());
}
++iterations;
// Check for max iterations
if (iterations >= config.maxIterations) {
status->exitCondition = SolverExitCondition::kMaxIterationsExceeded;
return;
}
// Check for max wall clock time
if (innerIterEndTime - solveStartTime > config.timeout) {
status->exitCondition = SolverExitCondition::kTimeout;
return;
}
// Check for solve to acceptable tolerance
if (E_0 > config.tolerance &&
acceptableIterCounter == config.maxAcceptableIterations) {
status->exitCondition = SolverExitCondition::kSolvedToAcceptableTolerance;
return;
}
}
}
} // namespace sleipnir

36
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp vendored
View File

@@ -11,6 +11,42 @@
namespace sleipnir {
/**
* Returns the error estimate using the KKT conditions for SQP.
*
* @param g Gradient of the cost function ∇f.
* @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
* current iterate.
* @param c_e The problem's equality constraints cₑ(x) evaluated at the current
* iterate.
* @param y Equality constraint dual variables.
*/
inline double ErrorEstimate(const Eigen::VectorXd& g,
const Eigen::SparseMatrix<double>& A_e,
const Eigen::VectorXd& c_e,
const Eigen::VectorXd& y) {
int numEqualityConstraints = A_e.rows();
// Update the error estimate using the KKT conditions from equations (19.5a)
// through (19.5d) of [1].
//
// ∇f − Aₑᵀy = 0
// cₑ = 0
//
// The error tolerance is the max of the following infinity norms scaled by
// s_d (see equation (5) of [2]).
//
// ‖∇f − Aₑᵀy‖_∞ / s_d
// ‖cₑ‖_∞
// s_d = max(sₘₐₓ, ‖y‖₁ / m) / sₘₐₓ
constexpr double s_max = 100.0;
double s_d = std::max(s_max, y.lpNorm<1>() / numEqualityConstraints) / s_max;
return std::max({(g - A_e.transpose() * y).lpNorm<Eigen::Infinity>() / s_d,
c_e.lpNorm<Eigen::Infinity>()});
}
/**
* Returns the error estimate using the KKT conditions for the interior-point
* method.

162
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp vendored
View File

@@ -20,6 +20,168 @@
namespace sleipnir {
/**
* Finds the iterate that minimizes the constraint violation while not deviating
* too far from the starting point. This is a fallback procedure when the normal
* Sequential Quadratic Programming method fails to converge to a feasible
* point.
*
* @param[in] decisionVariables The list of decision variables.
* @param[in] equalityConstraints The list of equality constraints.
* @param[in] callback The user callback.
* @param[in] config Configuration options for the solver.
* @param[in,out] x The current iterate from the normal solve.
* @param[out] status The solver status.
*/
inline void FeasibilityRestoration(
std::span<Variable> decisionVariables,
std::span<Variable> equalityConstraints,
function_ref<bool(const SolverIterationInfo& info)> callback,
const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status) {
// Feasibility restoration
//
// min ρ Σ (pₑ + nₑ) + ζ/2 (x - x_R)ᵀD_R(x - x_R)
// x
// pₑ,nₑ
//
// s.t. cₑ(x) - pₑ + nₑ = 0
// pₑ ≥ 0
// nₑ ≥ 0
//
// where ρ = 1000, ζ = √μ where μ is the barrier parameter, x_R is original
// iterate before feasibility restoration, and D_R is a scaling matrix defined
// by
//
// D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾)
constexpr double ρ = 1000.0;
double μ = config.tolerance / 10.0;
wpi::SmallVector<Variable> fr_decisionVariables;
fr_decisionVariables.reserve(decisionVariables.size() +
2 * equalityConstraints.size());
// Assign x
fr_decisionVariables.assign(decisionVariables.begin(),
decisionVariables.end());
// Allocate pₑ and nₑ
for (size_t row = 0; row < 2 * equalityConstraints.size(); ++row) {
fr_decisionVariables.emplace_back();
}
auto it = fr_decisionVariables.begin();
VariableMatrix xAD{std::span{it, it + decisionVariables.size()}};
it += decisionVariables.size();
VariableMatrix p_e{std::span{it, it + equalityConstraints.size()}};
it += equalityConstraints.size();
VariableMatrix n_e{std::span{it, it + equalityConstraints.size()}};
it += equalityConstraints.size();
// Set initial values for pₑ and nₑ.
//
//
// From equation (33) of [2]:
// ______________________
// μ − ρ c(x) /(μ − ρ c(x))² μ c(x)
// n = −−−−−−−−−− + / (−−−−−−−−−−) + −−−−−− (1)
// 2ρ √ ( 2ρ ) 2ρ
//
// The quadratic formula:
// ________
// -b + √b² - 4ac
// x = −−−−−−−−−−−−−− (2)
// 2a
//
// Rearrange (1) to fit the quadratic formula better:
// _________________________
// μ - ρ c(x) + √(μ - ρ c(x))² + 2ρ μ c(x)
// n = −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
// 2ρ
//
// Solve for coefficients:
//
// a = ρ (3)
// b = ρ c(x) - μ (4)
//
// -4ac = μ c(x) 2ρ
// -4(ρ)c = 2ρ μ c(x)
// -4c = 2μ c(x)
// c = -μ c(x)/2 (5)
//
// p = c(x) + n (6)
for (int row = 0; row < p_e.Rows(); ++row) {
double c_e = equalityConstraints[row].Value();
constexpr double a = 2 * ρ;
double b = ρ * c_e - μ;
double c = -μ * c_e / 2.0;
double n = -b * std::sqrt(b * b - 4.0 * a * c) / (2.0 * a);
double p = c_e + n;
p_e(row).SetValue(p);
n_e(row).SetValue(n);
}
// cₑ(x) - pₑ + nₑ = 0
wpi::SmallVector<Variable> fr_equalityConstraints;
fr_equalityConstraints.assign(equalityConstraints.begin(),
equalityConstraints.end());
for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) {
auto& constraint = fr_equalityConstraints[row];
constraint = constraint - p_e(row) + n_e(row);
}
// cᵢ(x) - s - pᵢ + nᵢ = 0
wpi::SmallVector<Variable> fr_inequalityConstraints;
// pₑ ≥ 0
std::copy(p_e.begin(), p_e.end(),
std::back_inserter(fr_inequalityConstraints));
// nₑ ≥ 0
std::copy(n_e.begin(), n_e.end(),
std::back_inserter(fr_inequalityConstraints));
Variable J = 0.0;
// J += ρ Σ (pₑ + nₑ)
for (auto& elem : p_e) {
J += elem;
}
for (auto& elem : n_e) {
J += elem;
}
J *= ρ;
// D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾)
Eigen::VectorXd D_R{x.rows()};
for (int row = 0; row < D_R.rows(); ++row) {
D_R(row) = std::min(1.0, 1.0 / std::abs(x(row)));
}
// J += ζ/2 (x - x_R)ᵀD_R(x - x_R)
for (int row = 0; row < x.rows(); ++row) {
J += std::sqrt(μ) / 2.0 * D_R(row) * sleipnir::pow(xAD(row) - x(row), 2);
}
Eigen::VectorXd fr_x = VariableMatrix{fr_decisionVariables}.Value();
// Set up initial value for inequality constraint slack variables
Eigen::VectorXd fr_s{fr_inequalityConstraints.size()};
fr_s.setOnes();
InteriorPoint(fr_decisionVariables, fr_equalityConstraints,
fr_inequalityConstraints, J, callback, config, true, fr_x, fr_s,
status);
x = fr_x.segment(0, decisionVariables.size());
}
/**
* Finds the iterate that minimizes the constraint violation while not deviating
* too far from the starting point. This is a fallback procedure when the normal

49
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp vendored
View File

@@ -12,6 +12,8 @@
#include "sleipnir/autodiff/Variable.hpp"
// See docs/algorithms.md#Works_cited for citation definitions.
namespace sleipnir {
/**
@@ -32,26 +34,14 @@ struct FilterEntry {
* @param cost The cost function's value.
* @param constraintViolation The constraint violation.
*/
FilterEntry(double cost, double constraintViolation)
constexpr FilterEntry(double cost, double constraintViolation)
: cost{cost}, constraintViolation{constraintViolation} {}
/**
* Constructs a FilterEntry.
*
* @param f The cost function.
* @param μ The barrier parameter.
* @param s The inequality constraint slack variables.
* @param c_e The equality constraint values (nonzero means violation).
* @param c_i The inequality constraint values (negative means violation).
*/
FilterEntry(Variable& f, double μ, const Eigen::VectorXd& s,
const Eigen::VectorXd& c_e, const Eigen::VectorXd& c_i)
: cost{f.Value() - μ * s.array().log().sum()},
constraintViolation{c_e.lpNorm<1>() + (c_i - s).lpNorm<1>()} {}
};
/**
* Interior-point step filter.
* Step filter.
*
* See the section on filters in chapter 15 of [1].
*/
class Filter {
public:
@@ -64,11 +54,9 @@ class Filter {
* Construct an empty filter.
*
* @param f The cost function.
* @param μ The barrier parameter.
*/
explicit Filter(Variable& f, double μ) {
explicit Filter(Variable& f) {
m_f = &f;
m_μ = μ;
// Initial filter entry rejects constraint violations above max
m_filter.emplace_back(std::numeric_limits<double>::infinity(),
@@ -77,11 +65,8 @@ class Filter {
/**
* Reset the filter.
*
* @param μ The new barrier parameter.
*/
void Reset(double μ) {
m_μ = μ;
void Reset() {
m_filter.clear();
// Initial filter entry rejects constraint violations above max
@@ -90,15 +75,26 @@ class Filter {
}
/**
* Creates a new filter entry.
* Creates a new Sequential Quadratic Programming filter entry.
*
* @param c_e The equality constraint values (nonzero means violation).
*/
FilterEntry MakeEntry(const Eigen::VectorXd& c_e) {
return FilterEntry{m_f->Value(), c_e.lpNorm<1>()};
}
/**
* Creates a new interior-point method filter entry.
*
* @param s The inequality constraint slack variables.
* @param c_e The equality constraint values (nonzero means violation).
* @param c_i The inequality constraint values (negative means violation).
* @param μ The barrier parameter.
*/
FilterEntry MakeEntry(Eigen::VectorXd& s, const Eigen::VectorXd& c_e,
const Eigen::VectorXd& c_i) {
return FilterEntry{*m_f, m_μ, s, c_e, c_i};
const Eigen::VectorXd& c_i, double μ) {
return FilterEntry{m_f->Value() - μ * s.array().log().sum(),
c_e.lpNorm<1>() + (c_i - s).lpNorm<1>()};
}
/**
@@ -181,7 +177,6 @@ class Filter {
private:
Variable* m_f = nullptr;
double m_μ = 0.0;
wpi::SmallVector<FilterEntry> m_filter;
};

22
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp vendored
View File

@@ -9,6 +9,28 @@
namespace sleipnir {
/**
* Returns the KKT error for Sequential Quadratic Programming.
*
* @param g Gradient of the cost function ∇f.
* @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
* current iterate.
* @param c_e The problem's equality constraints cₑ(x) evaluated at the current
* iterate.
* @param y Equality constraint dual variables.
*/
inline double KKTError(const Eigen::VectorXd& g,
const Eigen::SparseMatrix<double>& A_e,
const Eigen::VectorXd& c_e, const Eigen::VectorXd& y) {
// Compute the KKT error as the 1-norm of the KKT conditions from equations
// (19.5a) through (19.5d) of [1].
//
// ∇f − Aₑᵀy = 0
// cₑ = 0
return (g - A_e.transpose() * y).lpNorm<1>() + c_e.lpNorm<1>();
}
/**
* Returns the KKT error for the interior-point method.
*

16
wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java
View File

@@ -30,7 +30,7 @@ class Ellipse2dTest {
}
@Test
void testIntersectsPoint() {
void testIntersects() {
var center = new Pose2d(1.0, 2.0, new Rotation2d());
var ellipse = new Ellipse2d(center, 2.0, 1.0);
@@ -43,7 +43,7 @@ class Ellipse2dTest {
}
@Test
void testContainsPoint() {
void testContains() {
var center = new Pose2d(-1.0, -2.0, Rotation2d.fromDegrees(45.0));
var ellipse = new Ellipse2d(center, 2.0, 1.0);
@@ -55,7 +55,7 @@ class Ellipse2dTest {
}
@Test
void testDistanceToPoint() {
void testDistance() {
var center = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(270.0));
var ellipse = new Ellipse2d(center, 1.0, 2.0);
@@ -73,30 +73,30 @@ class Ellipse2dTest {
}
@Test
void testFindNearestPoint() {
void testNearest() {
var center = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(270.0));
var ellipse = new Ellipse2d(center, 1.0, 2.0);
var point1 = new Translation2d(2.5, 2.0);
var nearestPoint1 = ellipse.findNearestPoint(point1);
var nearestPoint1 = ellipse.nearest(point1);
assertAll(
() -> assertEquals(2.5, nearestPoint1.getX(), kEpsilon),
() -> assertEquals(2.0, nearestPoint1.getY(), kEpsilon));
var point2 = new Translation2d(1.0, 2.0);
var nearestPoint2 = ellipse.findNearestPoint(point2);
var nearestPoint2 = ellipse.nearest(point2);
assertAll(
() -> assertEquals(1.0, nearestPoint2.getX(), kEpsilon),
() -> assertEquals(2.0, nearestPoint2.getY(), kEpsilon));
var point3 = new Translation2d(1.0, 1.0);
var nearestPoint3 = ellipse.findNearestPoint(point3);
var nearestPoint3 = ellipse.nearest(point3);
assertAll(
() -> assertEquals(1.0, nearestPoint3.getX(), kEpsilon),
() -> assertEquals(1.0, nearestPoint3.getY(), kEpsilon));
var point4 = new Translation2d(-1.0, 2.5);
var nearestPoint4 = ellipse.findNearestPoint(point4);
var nearestPoint4 = ellipse.nearest(point4);
assertAll(
() -> assertEquals(-0.8512799937611617, nearestPoint4.getX(), kEpsilon),
() -> assertEquals(2.378405333174535, nearestPoint4.getY(), kEpsilon));

12
wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java
View File

@@ -30,7 +30,7 @@ class Rectangle2dTest {
}
@Test
void testIntersectsPoint() {
void testIntersects() {
var center = new Pose2d(4.0, 3.0, Rotation2d.fromDegrees(90.0));
var rect = new Rectangle2d(center, 2.0, 3.0);
@@ -42,7 +42,7 @@ class Rectangle2dTest {
}
@Test
void testContainsPoint() {
void testContains() {
var center = new Pose2d(2.0, 3.0, Rotation2d.fromDegrees(45.0));
var rect = new Rectangle2d(center, 3.0, 1.0);
@@ -53,7 +53,7 @@ class Rectangle2dTest {
}
@Test
void testDistanceToPoint() {
void testDistance() {
var center = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(270.0));
var rect = new Rectangle2d(center, 1.0, 2.0);
@@ -71,18 +71,18 @@ class Rectangle2dTest {
}
@Test
void testFindNearestPoint() {
void testNearest() {
var center = new Pose2d(1.0, 1.0, Rotation2d.fromDegrees(90.0));
var rect = new Rectangle2d(center, 3.0, 4.0);
var point1 = new Translation2d(1.0, 3.0);
var nearestPoint1 = rect.findNearestPoint(point1);
var nearestPoint1 = rect.nearest(point1);
assertAll(
() -> assertEquals(1.0, nearestPoint1.getX(), kEpsilon),
() -> assertEquals(2.5, nearestPoint1.getY(), kEpsilon));
var point2 = new Translation2d(0.0, 0.0);
var nearestPoint2 = rect.findNearestPoint(point2);
var nearestPoint2 = rect.nearest(point2);
assertAll(
() -> assertEquals(0.0, nearestPoint2.getX(), kEpsilon),
() -> assertEquals(0.0, nearestPoint2.getY(), kEpsilon));

16
wpimath/src/test/native/cpp/geometry/Ellipse2dTest.cpp
View File

@@ -16,7 +16,7 @@ TEST(Ellipse2dTest, FocalPoints) {
EXPECT_EQ(frc::Translation2d(4_m, 2_m), b);
}
TEST(Ellipse2dTest, IntersectsPoint) {
TEST(Ellipse2dTest, Intersects) {
constexpr frc::Pose2d center{1_m, 2_m, 0_deg};
constexpr frc::Ellipse2d ellipse{center, 2_m, 1_m};
@@ -27,7 +27,7 @@ TEST(Ellipse2dTest, IntersectsPoint) {
EXPECT_FALSE(ellipse.Intersects(pointB));
}
TEST(Ellipse2dTest, ContainsPoint) {
TEST(Ellipse2dTest, Contains) {
constexpr frc::Pose2d center{-1_m, -2_m, 45_deg};
constexpr frc::Ellipse2d ellipse{center, 2_m, 1_m};
@@ -38,7 +38,7 @@ TEST(Ellipse2dTest, ContainsPoint) {
EXPECT_FALSE(ellipse.Contains(pointB));
}
TEST(Ellipse2dTest, DistanceToPoint) {
TEST(Ellipse2dTest, Distance) {
constexpr double kEpsilon = 1E-9;
constexpr frc::Pose2d center{1_m, 2_m, 270_deg};
@@ -57,29 +57,29 @@ TEST(Ellipse2dTest, DistanceToPoint) {
EXPECT_NEAR(0.19210128384806818, ellipse.Distance(point4).value(), kEpsilon);
}
TEST(Ellipse2dTest, FindNearestPoint) {
TEST(Ellipse2dTest, Nearest) {
constexpr double kEpsilon = 1E-9;
constexpr frc::Pose2d center{1_m, 2_m, 270_deg};
constexpr frc::Ellipse2d ellipse{center, 1_m, 2_m};
constexpr frc::Translation2d point1{2.5_m, 2_m};
auto nearestPoint1 = ellipse.FindNearestPoint(point1);
auto nearestPoint1 = ellipse.Nearest(point1);
EXPECT_NEAR(2.5, nearestPoint1.X().value(), kEpsilon);
EXPECT_NEAR(2.0, nearestPoint1.Y().value(), kEpsilon);
constexpr frc::Translation2d point2{1_m, 2_m};
auto nearestPoint2 = ellipse.FindNearestPoint(point2);
auto nearestPoint2 = ellipse.Nearest(point2);
EXPECT_NEAR(1.0, nearestPoint2.X().value(), kEpsilon);
EXPECT_NEAR(2.0, nearestPoint2.Y().value(), kEpsilon);
constexpr frc::Translation2d point3{1_m, 1_m};
auto nearestPoint3 = ellipse.FindNearestPoint(point3);
auto nearestPoint3 = ellipse.Nearest(point3);
EXPECT_NEAR(1.0, nearestPoint3.X().value(), kEpsilon);
EXPECT_NEAR(1.0, nearestPoint3.Y().value(), kEpsilon);
constexpr frc::Translation2d point4{-1_m, 2.5_m};
auto nearestPoint4 = ellipse.FindNearestPoint(point4);
auto nearestPoint4 = ellipse.Nearest(point4);
EXPECT_NEAR(-0.8512799937611617, nearestPoint4.X().value(), kEpsilon);
EXPECT_NEAR(2.378405333174535, nearestPoint4.Y().value(), kEpsilon);
}

12
wpimath/src/test/native/cpp/geometry/Rectangle2dTest.cpp
View File

@@ -18,7 +18,7 @@ TEST(Rectangle2dTest, NewWithCorners) {
EXPECT_EQ(4.0, rect.Center().Y().value());
}
TEST(Rectangle2dTest, IntersectsPoint) {
TEST(Rectangle2dTest, Intersects) {
constexpr frc::Pose2d center{4_m, 3_m, 90_deg};
constexpr frc::Rectangle2d rect{center, 2_m, 3_m};
@@ -28,7 +28,7 @@ TEST(Rectangle2dTest, IntersectsPoint) {
EXPECT_FALSE(rect.Intersects(frc::Translation2d{4_m, 3.5_m}));
}
TEST(Rectangle2dTest, ContainsPoint) {
TEST(Rectangle2dTest, Contains) {
constexpr frc::Pose2d center{2_m, 3_m, 45_deg};
constexpr frc::Rectangle2d rect{center, 3_m, 1_m};
@@ -37,7 +37,7 @@ TEST(Rectangle2dTest, ContainsPoint) {
EXPECT_FALSE(rect.Contains(frc::Translation2d{3_m, 3_m}));
}
TEST(Rectangle2dTest, DistanceToPoint) {
TEST(Rectangle2dTest, Distance) {
constexpr double kEpsilon = 1E-9;
constexpr frc::Pose2d center{1_m, 2_m, 270_deg};
@@ -56,19 +56,19 @@ TEST(Rectangle2dTest, DistanceToPoint) {
EXPECT_NEAR(1, rect.Distance(point4).value(), kEpsilon);
}
TEST(Rectangle2dTest, FindNearestPoint) {
TEST(Rectangle2dTest, Nearest) {
constexpr double kEpsilon = 1E-9;
constexpr frc::Pose2d center{1_m, 1_m, 90_deg};
constexpr frc::Rectangle2d rect{center, 3_m, 4_m};
constexpr frc::Translation2d point1{1_m, 3_m};
auto nearestPoint1 = rect.FindNearestPoint(point1);
auto nearestPoint1 = rect.Nearest(point1);
EXPECT_NEAR(1.0, nearestPoint1.X().value(), kEpsilon);
EXPECT_NEAR(2.5, nearestPoint1.Y().value(), kEpsilon);
constexpr frc::Translation2d point2{0_m, 0_m};
auto nearestPoint2 = rect.FindNearestPoint(point2);
auto nearestPoint2 = rect.Nearest(point2);
EXPECT_NEAR(0.0, nearestPoint2.X().value(), kEpsilon);
EXPECT_NEAR(0.0, nearestPoint2.Y().value(), kEpsilon);
}