From 544553a58f1d78b0e9b4a47ecce34a90ab2acb1a Mon Sep 17 00:00:00 2001 From: Wispy <101812473+WispySparks@users.noreply.github.com> Date: Sun, 8 Dec 2024 01:00:49 -0600 Subject: [PATCH 1/3] [developerRobot] Add Epilogue and wpiunits to developerRobot (#7510) --- developerRobot/build.gradle | 3 +++ 1 file changed, 3 insertions(+) diff --git a/developerRobot/build.gradle b/developerRobot/build.gradle index 684d0ae63d..5a8b95c4b1 100644 --- a/developerRobot/build.gradle +++ b/developerRobot/build.gradle @@ -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 { From e08fdeba21463ffaa4c2025d6a6ccdbdebc706a0 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 7 Dec 2024 23:01:18 -0800 Subject: [PATCH 2/3] [wpimath] Rename FindNearestPoint() to Nearest() (#7513) This is easier to type and follows the naming of Pose2d::Nearest(). Since Ellipse2d and Rectangle2d were added for the 2025 season, we don't need to add deprecation notices. --- .../edu/wpi/first/math/geometry/Ellipse2d.java | 6 +++--- .../edu/wpi/first/math/geometry/Rectangle2d.java | 4 ++-- .../edu/wpi/first/math/jni/Ellipse2dJNI.java | 4 ++-- .../src/main/native/cpp/geometry/Ellipse2d.cpp | 2 +- wpimath/src/main/native/cpp/jni/Ellipse2dJNI.cpp | 6 +++--- .../main/native/include/frc/geometry/Ellipse2d.h | 4 ++-- .../native/include/frc/geometry/Rectangle2d.h | 4 ++-- .../wpi/first/math/geometry/Ellipse2dTest.java | 16 ++++++++-------- .../wpi/first/math/geometry/Rectangle2dTest.java | 12 ++++++------ .../test/native/cpp/geometry/Ellipse2dTest.cpp | 16 ++++++++-------- .../test/native/cpp/geometry/Rectangle2dTest.cpp | 12 ++++++------ 11 files changed, 43 insertions(+), 43 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java index a11aa6d7e9..ee9c302763 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java @@ -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(), diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java index 3b9455e47b..4e252b7f57 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java @@ -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; diff --git a/wpimath/src/main/java/edu/wpi/first/math/jni/Ellipse2dJNI.java b/wpimath/src/main/java/edu/wpi/first/math/jni/Ellipse2dJNI.java index 984ca50401..895caa2bc5 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/jni/Ellipse2dJNI.java +++ b/wpimath/src/main/java/edu/wpi/first/math/jni/Ellipse2dJNI.java @@ -9,7 +9,7 @@ public final class Ellipse2dJNI extends WPIMathJNI { /** * Returns the nearest point that is contained within the ellipse. * - *

Constructs an Ellipse2d object and runs its FindNearestPoint() method. + *

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, diff --git a/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp b/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp index eef320cd12..17ef087c19 100644 --- a/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp @@ -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; diff --git a/wpimath/src/main/native/cpp/jni/Ellipse2dJNI.cpp b/wpimath/src/main/native/cpp/jni/Ellipse2dJNI.cpp index cc53d69315..72538dc3f8 100644 --- a/wpimath/src/main/native/cpp/jni/Ellipse2dJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/Ellipse2dJNI.cpp @@ -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()); diff --git a/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h b/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h index 4cd59f55b3..629faf5922 100644 --- a/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h @@ -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. diff --git a/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h b/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h index f03ca98c44..744b374ba2 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h @@ -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; diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java index 28c9d41d01..016a1c70d5 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java @@ -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)); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java index 7ee86d6d7d..6705d9aecd 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java @@ -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)); diff --git a/wpimath/src/test/native/cpp/geometry/Ellipse2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Ellipse2dTest.cpp index c51e50e4b6..1ee79e7ac6 100644 --- a/wpimath/src/test/native/cpp/geometry/Ellipse2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Ellipse2dTest.cpp @@ -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); } diff --git a/wpimath/src/test/native/cpp/geometry/Rectangle2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rectangle2dTest.cpp index f4d2d4350e..90ac72f51a 100644 --- a/wpimath/src/test/native/cpp/geometry/Rectangle2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rectangle2dTest.cpp @@ -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); } From d5edb4060d570ab88f6d1a29fa63b87746f0842d Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 7 Dec 2024 23:02:39 -0800 Subject: [PATCH 3/3] [upstream_utils] Upgrade Sleipnir (#7512) It now uses SQP for problems without inequality constraints, which is faster. main: ``` [ RUN ] Ellipse2dTest.DistanceToPoint 0.203 ms [ OK ] Ellipse2dTest.DistanceToPoint (0 ms) [ RUN ] Ellipse2dTest.FindNearestPoint 0.019 ms [ OK ] Ellipse2dTest.FindNearestPoint (0 ms) ``` upgrade: ``` [ RUN ] Ellipse2dTest.DistanceToPoint 0.197 ms [ OK ] Ellipse2dTest.DistanceToPoint (0 ms) [ RUN ] Ellipse2dTest.FindNearestPoint 0.015 ms [ OK ] Ellipse2dTest.FindNearestPoint (0 ms) ``` --- upstream_utils/sleipnir.py | 4 +- .../0002-Use-wpi-SmallVector.patch | 132 +++-- .../optimization/OptimizationProblem.hpp | 14 +- .../sleipnir/optimization/solver/SQP.hpp | 46 ++ .../src/optimization/solver/InteriorPoint.cpp | 17 +- .../sleipnir/src/optimization/solver/SQP.cpp | 559 ++++++++++++++++++ .../solver/util/ErrorEstimate.hpp | 36 ++ .../solver/util/FeasibilityRestoration.hpp | 162 +++++ .../src/optimization/solver/util/Filter.hpp | 49 +- .../src/optimization/solver/util/KKTError.hpp | 22 + 10 files changed, 968 insertions(+), 73 deletions(-) create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/SQP.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp diff --git a/upstream_utils/sleipnir.py b/upstream_utils/sleipnir.py index 9b5d3e6aae..ac25370ff8 100755 --- a/upstream_utils/sleipnir.py +++ b/upstream_utils/sleipnir.py @@ -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() diff --git a/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch b/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch index 61e887791c..6b816784a7 100644 --- a/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch +++ b/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch @@ -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 + + #include ++#include + + #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 decisionVariables, + Filter filter{f}; + + // Kept outside the loop so its storage can be reused +- small_vector> triplets; ++ wpi::SmallVector> 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 fr_decisionVariables; + wpi::SmallVector 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 fr_inequalityConstraints; ++ wpi::SmallVector 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 fr_decisionVariables; ++ wpi::SmallVector 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 fr_equalityConstraints; ++ wpi::SmallVector 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 m_filter; + wpi::SmallVector m_filter; }; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp index e812fa5e34..66883fed98 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp @@ -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)); diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/SQP.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/SQP.hpp new file mode 100644 index 0000000000..ed10ffedff --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/SQP.hpp @@ -0,0 +1,46 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#include + +#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 decisionVariables, + std::span equalityConstraints, Variable& f, + function_ref callback, + const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status); + +} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp index 892d2dd20f..d3981c59d1 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp @@ -195,7 +195,7 @@ void InteriorPoint(std::span 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 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 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 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 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 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 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 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) { diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp new file mode 100644 index 0000000000..662abc2fb6 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp @@ -0,0 +1,559 @@ +// Copyright (c) Sleipnir contributors + +#include "sleipnir/optimization/solver/SQP.hpp" + +#include +#include +#include +#include +#include + +#include +#include + +#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 decisionVariables, + std::span equalityConstraints, Variable& f, + function_ref 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 A_e = jacobianCe.Value(); + + // Gradient of f ∇f + Gradient gradientF{f, xAD}; + Eigen::SparseVector g = gradientF.Value(); + + // Hessian of the Lagrangian H + // + // Hₖ = ∇²ₓₓL(xₖ, yₖ) + Hessian hessianL{L, xAD}; + Eigen::SparseMatrix 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> 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::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() > 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{}})) { + status->exitCondition = SolverExitCondition::kCallbackRequestedStop; + return; + } + + // lhs = [H Aₑᵀ] + // [Aₑ 0 ] + // + // Don't assign upper triangle because solver only uses lower triangle. + const Eigen::SparseMatrix topLeft = + H.triangularView(); + 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::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::InnerIterator it{A_e, col}; it; ++it) { + triplets.emplace_back(H.rows() + it.row(), it.col(), it.value()); + } + } + Eigen::SparseMatrix 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 lhs = A_e * A_e.transpose(); + + // rhs = Aₑ∇f + Eigen::VectorXd rhs = A_e * g; + + Eigen::SimplicialLDLT> 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::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 diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp index 6b757df3e6..f1490028dd 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp @@ -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& 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() / s_d, + c_e.lpNorm()}); +} + /** * Returns the error estimate using the KKT conditions for the interior-point * method. diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp index 0f13676aea..79b5d99ae2 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp @@ -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 decisionVariables, + std::span equalityConstraints, + function_ref 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 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 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 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 diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp index 02bdb5a8db..0c14efd7b8 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp @@ -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::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 m_filter; }; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp index 03fa112b6a..3b603159d2 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp @@ -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& 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. *