Skip to content

Commit 6aad90c

Browse files
calcmogulcalcmogul
committed
[upstream_utils] Upgrade Sleipnir
1 parent 161f8d1 commit 6aad90c

File tree

71 files changed

+6046
-4489
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

71 files changed

+6046
-4489
lines changed

‎benchmark/src/main/java/frc/robot/Main.java‎

Lines changed: 5 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -12,44 +12,18 @@
1212
import org.openjdk.jmh.profile.GCProfiler;
1313
import org.openjdk.jmh.runner.Runner;
1414
import org.openjdk.jmh.runner.RunnerException;
15-
import org.openjdk.jmh.runner.options.Options;
1615
import org.openjdk.jmh.runner.options.OptionsBuilder;
1716
import org.openjdk.jmh.runner.options.TimeValue;
1817
import org.wpilib.math.geometry.Pose2d;
19-
import org.wpilib.math.geometry.Rotation2d;
20-
import org.wpilib.math.path.TravelingSalesman;
2118

2219
public class Main {
23-
private static final Pose2d[] poses = {
24-
new Pose2d(-1, 1, Rotation2d.kCW_90deg),
25-
new Pose2d(-1, 2, Rotation2d.kCCW_90deg),
26-
new Pose2d(0, 0, Rotation2d.kZero),
27-
new Pose2d(0, 3, Rotation2d.kCW_90deg),
28-
new Pose2d(1, 1, Rotation2d.kCCW_90deg),
29-
new Pose2d(1, 2, Rotation2d.kCCW_90deg),
30-
};
31-
private static final int iterations = 100;
32-
33-
private static final TravelingSalesman transformTraveler =
34-
new TravelingSalesman(
35-
(pose1, pose2) -> {
36-
var transform = pose2.minus(pose1);
37-
return Math.hypot(transform.getX(), transform.getY());
38-
});
39-
private static final TravelingSalesman twistTraveler =
40-
new TravelingSalesman(
41-
(pose1, pose2) -> {
42-
var twist = pose2.minus(pose1).log();
43-
return Math.hypot(twist.dx, twist.dy);
44-
});
45-
4620
/**
4721
* Main function.
4822
*
4923
* @param args The (unused) arguments to the program.
5024
*/
5125
public static void main(String... args) throws RunnerException {
52-
Options opt =
26+
var opt =
5327
new OptionsBuilder()
5428
.include(Main.class.getSimpleName())
5529
.addProfiler(GCProfiler.class)
@@ -66,14 +40,14 @@ public static void main(String... args) throws RunnerException {
6640
@Benchmark
6741
@BenchmarkMode(Mode.AverageTime)
6842
@OutputTimeUnit(TimeUnit.MICROSECONDS)
69-
public Pose2d[] transform() {
70-
return transformTraveler.solve(poses, iterations);
43+
public Pose2d[] travelingSalesmanTransform() {
44+
return TravelingSalesmanBenchmark.transform();
7145
}
7246

7347
@Benchmark
7448
@BenchmarkMode(Mode.AverageTime)
7549
@OutputTimeUnit(TimeUnit.MICROSECONDS)
76-
public Pose2d[] twist() {
77-
return twistTraveler.solve(poses, iterations);
50+
public Pose2d[] travelingSalesmanTwist() {
51+
return TravelingSalesmanBenchmark.twist();
7852
}
7953
}

‎benchmark/src/main/java/frc/robot/TravelingSalesmanBenchmark.java‎

Lines changed: 48 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,48 @@
1+
// Copyright (c) FIRST and other WPILib contributors.
2+
// Open Source Software; you can modify and/or share it under the terms of
3+
// the WPILib BSD license file in the root directory of this project.
4+
5+
package frc.robot;
6+
7+
import org.wpilib.math.geometry.Pose2d;
8+
import org.wpilib.math.geometry.Rotation2d;
9+
import org.wpilib.math.path.TravelingSalesman;
10+
11+
public final class TravelingSalesmanBenchmark {
12+
private TravelingSalesmanBenchmark() {
13+
// Utility class.
14+
}
15+
16+
private static final Pose2d[] poses = {
17+
new Pose2d(-1, 1, Rotation2d.kCW_90deg),
18+
new Pose2d(-1, 2, Rotation2d.kCCW_90deg),
19+
new Pose2d(0, 0, Rotation2d.kZero),
20+
new Pose2d(0, 3, Rotation2d.kCW_90deg),
21+
new Pose2d(1, 1, Rotation2d.kCCW_90deg),
22+
new Pose2d(1, 2, Rotation2d.kCCW_90deg),
23+
};
24+
private static final int iterations = 100;
25+
26+
private static final TravelingSalesman transformTraveler =
27+
new TravelingSalesman(
28+
(pose1, pose2) -> {
29+
var transform = pose2.minus(pose1);
30+
return Math.hypot(transform.getX(), transform.getY());
31+
});
32+
private static final TravelingSalesman twistTraveler =
33+
new TravelingSalesman(
34+
(pose1, pose2) -> {
35+
var twist = pose2.minus(pose1).log();
36+
return Math.hypot(twist.dx, twist.dy);
37+
});
38+
39+
/** TravelingSalesman transform benchmark. */
40+
public static Pose2d[] transform() {
41+
return transformTraveler.solve(poses, iterations);
42+
}
43+
44+
/** TravelingSalesman twist benchmark. */
45+
public static Pose2d[] twist() {
46+
return twistTraveler.solve(poses, iterations);
47+
}
48+
}

‎benchmark/src/main/native/cpp/CartPoleBenchmark.hpp‎

Lines changed: 116 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,116 @@
1+
// Copyright (c) FIRST and other WPILib contributors.
2+
// Open Source Software; you can modify and/or share it under the terms of
3+
// the WPILib BSD license file in the root directory of this project.
4+
5+
#pragma once
6+
7+
#include <benchmark/benchmark.h>
8+
9+
#include <chrono>
10+
11+
#include <sleipnir/optimization/problem.hpp>
12+
13+
#include "wpi/math/system/NumericalIntegration.hpp"
14+
15+
inline slp::VariableMatrix<double> CartPoleDynamics(
16+
const slp::VariableMatrix<double>& x,
17+
const slp::VariableMatrix<double>& u) {
18+
constexpr double m_c = 5.0; // Cart mass (kg)
19+
constexpr double m_p = 0.5; // Pole mass (kg)
20+
constexpr double l = 0.5; // Pole length (m)
21+
constexpr double g = 9.806; // Acceleration due to gravity (m/s²)
22+
23+
auto q = x.segment(0, 2);
24+
auto qdot = x.segment(2, 2);
25+
auto theta = q[1];
26+
auto thetadot = qdot[1];
27+
28+
// [ m_c + m_p m_p l cosθ]
29+
// M(q) = [m_p l cosθ m_p l² ]
30+
slp::VariableMatrix<double> M{{m_c + m_p, m_p * l * cos(theta)},
31+
{m_p * l * cos(theta), m_p * std::pow(l, 2)}};
32+
33+
// [0 −m_p lθ̇ sinθ]
34+
// C(q, q̇) = [0 0 ]
35+
slp::VariableMatrix<double> C{{0, -m_p * l * thetadot * sin(theta)}, {0, 0}};
36+
37+
// [ 0 ]
38+
// τ_g(q) = [-m_p gl sinθ]
39+
slp::VariableMatrix<double> tau_g{{0}, {-m_p * g * l * sin(theta)}};
40+
41+
// [1]
42+
// B = [0]
43+
constexpr Eigen::Matrix<double, 2, 1> B{{1}, {0}};
44+
45+
// q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu)
46+
slp::VariableMatrix<double> qddot{4};
47+
qddot.segment(0, 2) = qdot;
48+
qddot.segment(2, 2) = solve(M, tau_g - C * qdot + B * u);
49+
return qddot;
50+
}
51+
52+
inline void BM_CartPole(benchmark::State& state) {
53+
using namespace std::chrono_literals;
54+
55+
// NOLINTNEXTLINE(clang-analyzer-deadcode.DeadStores)
56+
for (auto _ : state) {
57+
constexpr std::chrono::duration<double> T = 5s;
58+
constexpr std::chrono::duration<double> dt = 50ms;
59+
constexpr int N = T / dt;
60+
61+
constexpr double u_max = 20.0; // N
62+
constexpr double d_max = 2.0; // m
63+
64+
constexpr Eigen::Vector<double, 4> x_initial{{0.0, 0.0, 0.0, 0.0}};
65+
constexpr Eigen::Vector<double, 4> x_final{
66+
{1.0, std::numbers::pi, 0.0, 0.0}};
67+
68+
slp::Problem<double> problem;
69+
70+
// x = [q, q̇]ᵀ = [x, θ, ẋ, θ̇]ᵀ
71+
auto X = problem.decision_variable(4, N + 1);
72+
73+
// Initial guess
74+
for (int k = 0; k < N + 1; ++k) {
75+
X(0, k).set_value(
76+
std::lerp(x_initial[0], x_final[0], static_cast<double>(k) / N));
77+
X(1, k).set_value(
78+
std::lerp(x_initial[1], x_final[1], static_cast<double>(k) / N));
79+
}
80+
81+
// u = f_x
82+
auto U = problem.decision_variable(1, N);
83+
84+
// Initial conditions
85+
problem.subject_to(X.col(0) == x_initial);
86+
87+
// Final conditions
88+
problem.subject_to(X.col(N) == x_final);
89+
90+
// Cart position constraints
91+
problem.subject_to(X.row(0) >= 0.0);
92+
problem.subject_to(X.row(0) <= d_max);
93+
94+
// Input constraints
95+
problem.subject_to(U >= -u_max);
96+
problem.subject_to(U <= u_max);
97+
98+
// Dynamics constraints - RK4 integration
99+
for (int k = 0; k < N; ++k) {
100+
problem.subject_to(X.col(k + 1) ==
101+
wpi::math::RK4<decltype(CartPoleDynamics),
102+
slp::VariableMatrix<double>,
103+
slp::VariableMatrix<double>>(
104+
CartPoleDynamics, X.col(k), U.col(k), dt));
105+
}
106+
107+
// Minimize sum squared inputs
108+
slp::Variable J = 0.0;
109+
for (int k = 0; k < N; ++k) {
110+
J += U.col(k).T() * U.col(k);
111+
}
112+
problem.minimize(J);
113+
114+
problem.solve({.diagnostics = true});
115+
}
116+
}

‎benchmark/src/main/native/cpp/Main.cpp‎

Lines changed: 5 additions & 35 deletions
Original file line numberDiff line numberDiff line change
@@ -4,41 +4,11 @@
44

55
#include <benchmark/benchmark.h>
66

7-
#include "wpi/math/geometry/Pose2d.hpp"
8-
#include "wpi/math/path/TravelingSalesman.hpp"
9-
#include "wpi/units/angle.hpp"
10-
#include "wpi/units/length.hpp"
11-
#include "wpi/util/array.hpp"
7+
#include "CartPoleBenchmark.hpp"
8+
#include "TravelingSalesmanBenchmark.hpp"
129

13-
static constexpr wpi::util::array<wpi::math::Pose2d, 6> poses{
14-
wpi::math::Pose2d{-1_m, 1_m, -90_deg}, wpi::math::Pose2d{-1_m, 2_m, 90_deg},
15-
wpi::math::Pose2d{0_m, 0_m, 0_deg}, wpi::math::Pose2d{0_m, 3_m, -90_deg},
16-
wpi::math::Pose2d{1_m, 1_m, 90_deg}, wpi::math::Pose2d{1_m, 2_m, 90_deg},
17-
};
18-
static constexpr int iterations = 100;
19-
20-
void BM_Transform(benchmark::State& state) {
21-
wpi::math::TravelingSalesman traveler{[](auto pose1, auto pose2) {
22-
auto transform = pose2 - pose1;
23-
return wpi::units::math::hypot(transform.X(), transform.Y()).value();
24-
}};
25-
// NOLINTNEXTLINE(clang-analyzer-deadcode.DeadStores)
26-
for (auto _ : state) {
27-
traveler.Solve(poses, iterations);
28-
}
29-
}
30-
BENCHMARK(BM_Transform);
31-
32-
void BM_Twist(benchmark::State& state) {
33-
wpi::math::TravelingSalesman traveler{[](auto pose1, auto pose2) {
34-
auto twist = (pose2 - pose1).Log();
35-
return wpi::units::math::hypot(twist.dx, twist.dy).value();
36-
}};
37-
// NOLINTNEXTLINE(clang-analyzer-deadcode.DeadStores)
38-
for (auto _ : state) {
39-
traveler.Solve(poses, iterations);
40-
}
41-
}
42-
BENCHMARK(BM_Twist);
10+
BENCHMARK(BM_CartPole);
11+
BENCHMARK(BM_TravelingSalesman_Transform);
12+
BENCHMARK(BM_TravelingSalesman_Twist);
4313

4414
BENCHMARK_MAIN();

‎benchmark/src/main/native/cpp/TravelingSalesmanBenchmark.hpp‎

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
// Copyright (c) FIRST and other WPILib contributors.
2+
// Open Source Software; you can modify and/or share it under the terms of
3+
// the WPILib BSD license file in the root directory of this project.
4+
5+
#pragma once
6+
7+
#include <benchmark/benchmark.h>
8+
9+
#include "wpi/math/geometry/Pose2d.hpp"
10+
#include "wpi/math/path/TravelingSalesman.hpp"
11+
#include "wpi/units/angle.hpp"
12+
#include "wpi/units/length.hpp"
13+
#include "wpi/util/array.hpp"
14+
15+
static constexpr wpi::util::array<wpi::math::Pose2d, 6> poses{
16+
wpi::math::Pose2d{-1_m, 1_m, -90_deg}, wpi::math::Pose2d{-1_m, 2_m, 90_deg},
17+
wpi::math::Pose2d{0_m, 0_m, 0_deg}, wpi::math::Pose2d{0_m, 3_m, -90_deg},
18+
wpi::math::Pose2d{1_m, 1_m, 90_deg}, wpi::math::Pose2d{1_m, 2_m, 90_deg},
19+
};
20+
static constexpr int iterations = 100;
21+
22+
inline void BM_TravelingSalesman_Transform(benchmark::State& state) {
23+
wpi::math::TravelingSalesman traveler{[](auto pose1, auto pose2) {
24+
auto transform = pose2 - pose1;
25+
return wpi::units::math::hypot(transform.X(), transform.Y()).value();
26+
}};
27+
// NOLINTNEXTLINE(clang-analyzer-deadcode.DeadStores)
28+
for (auto _ : state) {
29+
traveler.Solve(poses, iterations);
30+
}
31+
}
32+
33+
inline void BM_TravelingSalesman_Twist(benchmark::State& state) {
34+
wpi::math::TravelingSalesman traveler{[](auto pose1, auto pose2) {
35+
auto twist = (pose2 - pose1).Log();
36+
return wpi::units::math::hypot(twist.dx, twist.dy).value();
37+
}};
38+
// NOLINTNEXTLINE(clang-analyzer-deadcode.DeadStores)
39+
for (auto _ : state) {
40+
traveler.Solve(poses, iterations);
41+
}
42+
}

‎docs/build.gradle‎

Lines changed: 10 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -89,9 +89,6 @@ doxygen.sourceSets.main {
8989
}
9090

9191
if (project.hasProperty('docWarningsAsErrors')) {
92-
// apriltag
93-
exclude 'apriltag_pose.h'
94-
9592
// LLVM
9693
exclude 'wpi/util/Compiler.hpp'
9794
exclude 'wpi/util/ErrorHandling.hpp'
@@ -100,12 +97,20 @@ doxygen.sourceSets.main {
10097
exclude 'wpi/util/SmallVector.hpp'
10198
exclude 'wpi/util/StringExtras.hpp'
10299

103-
// libuv
104-
exclude 'uv/**'
100+
// Sleipnir
101+
exclude 'sleipnir/optimization/solver/interior_point.hpp'
102+
exclude 'sleipnir/optimization/solver/newton.hpp'
103+
exclude 'sleipnir/optimization/solver/sqp.hpp'
104+
105+
// apriltag
106+
exclude 'apriltag_pose.h'
105107

106108
// json
107109
exclude 'wpi/util/detail/**'
108110

111+
// libuv
112+
exclude 'uv/**'
113+
109114
// mpack
110115
exclude 'wpi/util/mpack.h'
111116

‎gradle.properties‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,2 +1,2 @@
1-
org.gradle.jvmargs=-Xmx4g
1+
org.gradle.jvmargs=-Xmx16g
22
org.ysb33r.gradle.doxygen.download.url=https://frcmaven.wpi.edu/artifactory/generic-release-mirror/doxygen

‎upstream_utils/sleipnir.py‎

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -18,8 +18,7 @@ def copy_upstream_src(wpilib_root: Path):
1818

1919
# Copy Sleipnir files into allwpilib
2020
walk_cwd_and_copy_if(
21-
lambda dp, f: (has_prefix(dp, Path("include")) or has_prefix(dp, Path("src")))
22-
and f not in [".styleguide", ".styleguide-license"],
21+
lambda dp, f: (has_prefix(dp, Path("include")) or has_prefix(dp, Path("src"))),
2322
wpimath / "src/main/native/thirdparty/sleipnir",
2423
)
2524

@@ -49,8 +48,8 @@ def copy_upstream_src(wpilib_root: Path):
4948
def main():
5049
name = "sleipnir"
5150
url = "https://github.com/SleipnirGroup/Sleipnir"
52-
# main on 2025-09-19
53-
tag = "7f89d5547702a09e3617bc31fe5bafe6add04fab"
51+
# main on 2025-11-07
52+
tag = "9aed7084cf190ad41f97c6a8b6311283e35d426d"
5453

5554
sleipnir = Lib(name, url, tag, copy_upstream_src)
5655
sleipnir.main()

0 commit comments

Comments
 (0)