Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-05 03:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Rewrite DARE solver (#5328)

Browse Source 
I timed the DARE unit tests, and the new solver is 0 to 100% faster in
all cases (that is, it's at least as fast as Drake's and up to 2x faster
in some cases).

The new solver is also much simpler, takes less time to compile, and
drops the libwpimath.so size from 325 MB to 301 MB.

I think most of the compilation time is coming from the eigenvalue
decompositions used to enforce argument preconditions.
This commit is contained in:
Tyler Veness
2023-05-14 22:23:00 -07:00
committed by GitHub
parent 3876a2523a
commit 52bd5b972d
32 changed files with 831 additions and 2024 deletions
Download Patch File Download Diff File Expand all files Collapse all files

76
upstream_utils/drake_patches/0001-Replace-Eigen-Dense-with-Eigen-Core.patch
View File

@@ -1,76 +0,0 @@
From 32dd1aa796fbb45d02f8bd445cc962e6a91318e7 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Wed, 18 May 2022 11:13:21 -0700
Subject: [PATCH 1/2] Replace <Eigen/Dense> with <Eigen/Core>
---
common/is_approx_equal_abstol.h | 2 +-
common/test_utilities/eigen_matrix_compare.h | 2 +-
math/discrete_algebraic_riccati_equation.cc | 3 +++
math/discrete_algebraic_riccati_equation.h | 2 +-
math/test/discrete_algebraic_riccati_equation_test.cc | 1 +
5 files changed, 7 insertions(+), 3 deletions(-)
diff --git a/common/is_approx_equal_abstol.h b/common/is_approx_equal_abstol.h
index 9af0c45252..b3f369ca01 100644
--- a/common/is_approx_equal_abstol.h
+++ b/common/is_approx_equal_abstol.h
@@ -2,7 +2,7 @@
#include <vector>
-#include <Eigen/Dense>
+#include <Eigen/Core>
namespace drake {
diff --git a/common/test_utilities/eigen_matrix_compare.h b/common/test_utilities/eigen_matrix_compare.h
index 01821ff847..105f6b381c 100644
--- a/common/test_utilities/eigen_matrix_compare.h
+++ b/common/test_utilities/eigen_matrix_compare.h
@@ -5,7 +5,7 @@
#include <limits>
#include <string>
-#include <Eigen/Dense>
+#include <Eigen/Core>
#include <gtest/gtest.h>
#include "drake/common/fmt_eigen.h"
diff --git a/math/discrete_algebraic_riccati_equation.cc b/math/discrete_algebraic_riccati_equation.cc
index 901f2ef240..20ea2b7bbe 100644
--- a/math/discrete_algebraic_riccati_equation.cc
+++ b/math/discrete_algebraic_riccati_equation.cc
@@ -1,5 +1,8 @@
#include "drake/math/discrete_algebraic_riccati_equation.h"
+#include <Eigen/Eigenvalues>
+#include <Eigen/QR>
+
#include "drake/common/drake_assert.h"
#include "drake/common/drake_throw.h"
#include "drake/common/is_approx_equal_abstol.h"
diff --git a/math/discrete_algebraic_riccati_equation.h b/math/discrete_algebraic_riccati_equation.h
index 891373ff9d..df7a58b2b8 100644
--- a/math/discrete_algebraic_riccati_equation.h
+++ b/math/discrete_algebraic_riccati_equation.h
@@ -3,7 +3,7 @@
#include <cmath>
#include <cstdlib>
-#include <Eigen/Dense>
+#include <Eigen/Core>
namespace drake {
namespace math {
diff --git a/math/test/discrete_algebraic_riccati_equation_test.cc b/math/test/discrete_algebraic_riccati_equation_test.cc
index 533ced151d..e4ecfd2eb5 100644
--- a/math/test/discrete_algebraic_riccati_equation_test.cc
+++ b/math/test/discrete_algebraic_riccati_equation_test.cc
@@ -1,5 +1,6 @@
#include "drake/math/discrete_algebraic_riccati_equation.h"
+#include <Eigen/Eigenvalues>
#include <gtest/gtest.h>
#include "drake/common/test_utilities/eigen_matrix_compare.h"

37
upstream_utils/drake_patches/0002-Add-WPILIB_DLLEXPORT-to-DARE-function-declarations.patch
View File

@@ -1,37 +0,0 @@
From 48ed223a299304724a38ad9911f7a732bf574b71 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Wed, 18 May 2022 11:15:27 -0700
Subject: [PATCH 2/2] Add WPILIB_DLLEXPORT to DARE function declarations
---
math/discrete_algebraic_riccati_equation.h | 3 +++
1 file changed, 3 insertions(+)
diff --git a/math/discrete_algebraic_riccati_equation.h b/math/discrete_algebraic_riccati_equation.h
index df7a58b2b8..55b8442bf4 100644
--- a/math/discrete_algebraic_riccati_equation.h
+++ b/math/discrete_algebraic_riccati_equation.h
@@ -4,6 +4,7 @@
#include <cstdlib>
#include <Eigen/Core>
+#include <wpi/SymbolExports.h>
namespace drake {
namespace math {
@@ -21,6 +22,7 @@ Based on the Schur Vector approach outlined in this paper:
"On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation"
by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell
*/
+WPILIB_DLLEXPORT
Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B,
@@ -71,6 +73,7 @@ J = Σ [uₖ] [0 R][uₖ] ΔT
@throws std::runtime_error if Q − NR⁻¹Nᵀ is not positive semi-definite.
@throws std::runtime_error if R is not positive definite.
*/
+WPILIB_DLLEXPORT
Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B,

104
upstream_utils/update_drake.py
View File

@@ -1,104 +0,0 @@
#!/usr/bin/env python3
import os
import shutil
from upstream_utils import (
get_repo_root,
clone_repo,
comment_out_invalid_includes,
walk_cwd_and_copy_if,
git_am,
)
def main():
upstream_root = clone_repo("https://github.com/RobotLocomotion/drake", "v1.15.0")
wpilib_root = get_repo_root()
wpimath = os.path.join(wpilib_root, "wpimath")
# Apply patches to upstream Git repo
os.chdir(upstream_root)
for f in [
"0001-Replace-Eigen-Dense-with-Eigen-Core.patch",
"0002-Add-WPILIB_DLLEXPORT-to-DARE-function-declarations.patch",
]:
git_am(os.path.join(wpilib_root, "upstream_utils/drake_patches", f))
# Delete old install
for d in [
"src/main/native/thirdparty/drake/src",
"src/main/native/thirdparty/drake/include",
"src/test/native/cpp/drake",
"src/test/native/include/drake",
]:
shutil.rmtree(os.path.join(wpimath, d), ignore_errors=True)
# Copy drake source files into allwpilib
src_files = walk_cwd_and_copy_if(
lambda dp, f: f
in ["drake_assert_and_throw.cc", "discrete_algebraic_riccati_equation.cc"],
os.path.join(wpimath, "src/main/native/thirdparty/drake/src"),
)
# Copy drake header files into allwpilib
include_files = walk_cwd_and_copy_if(
lambda dp, f: f
in [
"drake_assert.h",
"drake_assertion_error.h",
"is_approx_equal_abstol.h",
"never_destroyed.h",
"drake_copyable.h",
"drake_throw.h",
"discrete_algebraic_riccati_equation.h",
],
os.path.join(wpimath, "src/main/native/thirdparty/drake/include/drake"),
)
# Copy drake test source files into allwpilib
os.chdir(os.path.join(upstream_root, "math/test"))
test_src_files = walk_cwd_and_copy_if(
lambda dp, f: f == "discrete_algebraic_riccati_equation_test.cc",
os.path.join(wpimath, "src/test/native/cpp/drake"),
)
os.chdir(upstream_root)
test_src_files += walk_cwd_and_copy_if(
lambda dp, f: f == "fmt_eigen.cc",
os.path.join(wpimath, "src/test/native/cpp/drake"),
)
# Copy drake test header files into allwpilib
test_include_files = walk_cwd_and_copy_if(
lambda dp, f: f in ["eigen_matrix_compare.h", "fmt.h", "fmt_eigen.h"],
os.path.join(wpimath, "src/test/native/include/drake"),
)
for f in src_files:
comment_out_invalid_includes(
f, [os.path.join(wpimath, "src/main/native/thirdparty/drake/include")]
)
for f in include_files:
comment_out_invalid_includes(
f, [os.path.join(wpimath, "src/main/native/thirdparty/drake/include")]
)
for f in test_src_files:
comment_out_invalid_includes(
f,
[
os.path.join(wpimath, "src/main/native/thirdparty/drake/include"),
os.path.join(wpimath, "src/test/native/include"),
],
)
for f in test_include_files:
comment_out_invalid_includes(
f,
[
os.path.join(wpimath, "src/main/native/thirdparty/drake/include"),
os.path.join(wpimath, "src/test/native/include"),
],
)
if __name__ == "__main__":
main()