From d4c6e983f6d05774ca87966627b437f50f6867ab Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Fri, 5 Jul 2024 12:55:22 -0700
Subject: [PATCH] Replace manual matrix inversion with solve() (#590)

---
 .../optimization/optimization_problem_cart_pole_test.py   | 7 +------
 test/src/CartPoleUtil.cpp                                 | 8 +++-----
 2 files changed, 4 insertions(+), 11 deletions(-)

diff --git a/jormungandr/test/optimization/optimization_problem_cart_pole_test.py b/jormungandr/test/optimization/optimization_problem_cart_pole_test.py
index 065132c3..d4ce613a 100644
--- a/jormungandr/test/optimization/optimization_problem_cart_pole_test.py
+++ b/jormungandr/test/optimization/optimization_problem_cart_pole_test.py
@@ -63,11 +63,6 @@ def cart_pole_dynamics_double(x, u):
         ]
     )
 
-    detM = M[0, 0] * M[1, 1] - M[0, 1] * M[1, 0]
-    Minv = np.array(
-        [[M[1, 1] / detM, -M[0, 1] / detM], [-M[1, 0] / detM, M[0, 0] / detM]]
-    )
-
     #           [0  −m_p lθ̇ sinθ]
     # C(q, q̇) = [0       0      ]
     C = np.array([[0.0, -m_p * l * thetadot * math.sin(theta)], [0.0, 0.0]])
@@ -83,7 +78,7 @@ def cart_pole_dynamics_double(x, u):
     # q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu)
     qddot = np.empty((4, 1))
     qddot[:2, :] = qdot
-    qddot[2:, :] = Minv @ (tau_g - C @ qdot + B @ u)
+    qddot[2:, :] = np.linalg.solve(M, tau_g - C @ qdot + B @ u)
     return qddot
 
 
diff --git a/test/src/CartPoleUtil.cpp b/test/src/CartPoleUtil.cpp
index d3f33055..f24823f4 100644
--- a/test/src/CartPoleUtil.cpp
+++ b/test/src/CartPoleUtil.cpp
@@ -2,6 +2,8 @@
 
 #include "CartPoleUtil.hpp"
 
+#include <Eigen/QR>
+
 // https://underactuated.mit.edu/acrobot.html#cart_pole
 //
 // θ is CCW+ measured from negative y-axis.
@@ -44,10 +46,6 @@ Eigen::Vector<double, 4> CartPoleDynamicsDouble(
       {m_c + m_p, m_p * l * cos(theta)},              // NOLINT
       {m_p * l * cos(theta), m_p * std::pow(l, 2)}};  // NOLINT
 
-  double detM = M(0, 0) * M(1, 1) - M(0, 1) * M(1, 0);
-  Eigen::Matrix<double, 2, 2> Minv{{M(1, 1) / detM, -M(0, 1) / detM},
-                                   {-M(1, 0) / detM, M(0, 0) / detM}};
-
   //           [0  −m_p lθ̇ sinθ]
   // C(q, q̇) = [0       0      ]
   Eigen::Matrix<double, 2, 2> C{
@@ -65,7 +63,7 @@ Eigen::Vector<double, 4> CartPoleDynamicsDouble(
   // q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu)
   Eigen::Vector<double, 4> qddot;
   qddot.segment(0, 2) = qdot;
-  qddot.segment(2, 2) = Minv * (tau_g - C * qdot + B * u);
+  qddot.segment(2, 2) = M.householderQr().solve(tau_g - C * qdot + B * u);
   return qddot;
 }