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

[upstream_utils] Upgrade to Sleipnir 0.3.2 (#8323)

Browse Source 
Also includes a C++ benchmark, which has a Java counterpart in #8236.
This commit is contained in:
Tyler Veness
2025-12-01 12:51:28 -08:00
committed by GitHub
parent feea24251f
commit 08784dc2d1
65 changed files with 5699 additions and 4446 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
benchmark/src/main/native/cpp/CartPoleBenchmark.hpp
View File

@@ -11,8 +11,9 @@
#include "wpi/math/system/NumericalIntegration.hpp"
inline slp::VariableMatrix CartPoleDynamics(const slp::VariableMatrix& x,
const slp::VariableMatrix& u) {
inline slp::VariableMatrix<double> CartPoleDynamics(
const slp::VariableMatrix<double>& x,
const slp::VariableMatrix<double>& u) {
constexpr double m_c = 5.0; // Cart mass (kg)
constexpr double m_p = 0.5; // Pole mass (kg)
constexpr double l = 0.5; // Pole length (m)
@@ -25,23 +26,23 @@ inline slp::VariableMatrix CartPoleDynamics(const slp::VariableMatrix& x,
// [ m_c + m_p m_p l cosθ]
// M(q) = [m_p l cosθ m_p l² ]
slp::VariableMatrix M{{m_c + m_p, m_p * l * cos(theta)},
{m_p * l * cos(theta), m_p * std::pow(l, 2)}};
slp::VariableMatrix<double> M{{m_c + m_p, m_p * l * cos(theta)},
{m_p * l * cos(theta), m_p * std::pow(l, 2)}};
// [0 −m_p lθ̇ sinθ]
// C(q, q̇) = [0 0 ]
slp::VariableMatrix C{{0, -m_p * l * thetadot * sin(theta)}, {0, 0}};
slp::VariableMatrix<double> C{{0, -m_p * l * thetadot * sin(theta)}, {0, 0}};
// [ 0 ]
// τ_g(q) = [-m_p gl sinθ]
slp::VariableMatrix tau_g{{0}, {-m_p * g * l * sin(theta)}};
slp::VariableMatrix<double> tau_g{{0}, {-m_p * g * l * sin(theta)}};
// [1]
// B = [0]
constexpr Eigen::Matrix<double, 2, 1> B{{1}, {0}};
// q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu)
slp::VariableMatrix qddot{4};
slp::VariableMatrix<double> qddot{4};
qddot.segment(0, 2) = qdot;
qddot.segment(2, 2) = solve(M, tau_g - C * qdot + B * u);
return qddot;
@@ -63,7 +64,7 @@ inline void BM_CartPole(benchmark::State& state) {
constexpr Eigen::Vector<double, 4> x_final{
{1.0, std::numbers::pi, 0.0, 0.0}};
slp::Problem problem;
slp::Problem<double> problem;
// x = [q, q̇]ᵀ = [x, θ, ẋ, θ̇]ᵀ
auto X = problem.decision_variable(4, N + 1);
@@ -95,11 +96,11 @@ inline void BM_CartPole(benchmark::State& state) {
// Dynamics constraints - RK4 integration
for (int k = 0; k < N; ++k) {
problem.subject_to(
X.col(k + 1) ==
wpi::math::RK4<decltype(CartPoleDynamics), slp::VariableMatrix,
slp::VariableMatrix>(CartPoleDynamics, X.col(k),
U.col(k), dt));
problem.subject_to(X.col(k + 1) ==
wpi::math::RK4<decltype(CartPoleDynamics),
slp::VariableMatrix<double>,
slp::VariableMatrix<double>>(
CartPoleDynamics, X.col(k), U.col(k), dt));
}
// Minimize sum squared inputs