diff --git a/test/include/CartPoleUtil.hpp b/test/include/CartPoleUtil.hpp
new file mode 100644
index 00000000..be6fa4b5
--- /dev/null
+++ b/test/include/CartPoleUtil.hpp
@@ -0,0 +1,33 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <Eigen/Core>
+#include <sleipnir/autodiff/VariableMatrix.hpp>
+#include <units/time.h>
+
+/**
+ * Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
+ *
+ * @param f  The function to integrate. It must take two arguments x and u.
+ * @param x  The initial value of x.
+ * @param u  The value u held constant over the integration period.
+ * @param dt The time over which to integrate.
+ */
+template <typename F, typename T, typename U>
+T RK4(F&& f, T x, U u, units::second_t dt) {
+  const auto h = dt.value();
+
+  T k1 = f(x, u);
+  T k2 = f(x + h * 0.5 * k1, u);
+  T k3 = f(x + h * 0.5 * k2, u);
+  T k4 = f(x + h * k3, u);
+
+  return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+}
+
+Eigen::Vector<double, 4> CartPoleDynamicsDouble(
+    const Eigen::Vector<double, 4>& x, const Eigen::Vector<double, 1>& u);
+
+sleipnir::VariableMatrix CartPoleDynamics(const sleipnir::VariableMatrix& x,
+                                          const sleipnir::VariableMatrix& u);
diff --git a/test/src/CartPoleUtil.cpp b/test/src/CartPoleUtil.cpp
new file mode 100644
index 00000000..95b3390e
--- /dev/null
+++ b/test/src/CartPoleUtil.cpp
@@ -0,0 +1,159 @@
+// Copyright (c) Sleipnir contributors
+
+#include "CartPoleUtil.hpp"
+
+#include <units/acceleration.h>
+#include <units/length.h>
+#include <units/mass.h>
+
+Eigen::Vector<double, 4> CartPoleDynamicsDouble(
+    const Eigen::Vector<double, 4>& x, const Eigen::Vector<double, 1>& u) {
+  // https://underactuated.mit.edu/acrobot.html#cart_pole
+  //
+  // θ is CCW+ measured from negative y-axis.
+  //
+  // q = [x, θ]ᵀ
+  // q̇ = [ẋ, θ̇]ᵀ
+  // u = f_x
+  //
+  // M(q)q̈ + C(q, q̇)q̇ = τ_g(q) + Bu
+  // M(q)q̈ = τ_g(q) − C(q, q̇)q̇ + Bu
+  // q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu)
+  //
+  //        [ m_c + m_p  m_p l cosθ]
+  // M(q) = [m_p l cosθ    m_p l²  ]
+  //
+  //           [0  −m_p lθ̇ sinθ]
+  // C(q, q̇) = [0       0      ]
+  //
+  //          [     0      ]
+  // τ_g(q) = [-m_p gl sinθ]
+  //
+  //     [1]
+  // B = [0]
+  constexpr double m_c = (5_kg).value();        // Cart mass
+  constexpr double m_p = (0.5_kg).value();      // Pole mass
+  constexpr double l = (0.5_m).value();         // Pole length
+  constexpr double g = (9.806_mps_sq).value();  // Acceleration due to gravity
+
+  Eigen::Vector<double, 2> q = x.segment(0, 2);
+  Eigen::Vector<double, 2> qdot = x.segment(2, 2);
+  double theta = q(1);
+  double thetadot = qdot(1);
+
+  //        [ m_c + m_p  m_p l cosθ]
+  // M(q) = [m_p l cosθ    m_p l²  ]
+  Eigen::Matrix<double, 2, 2> M;
+  M(0, 0) = m_c + m_p;
+  M(0, 1) = m_p * l * cos(theta);  // NOLINT
+  M(1, 0) = m_p * l * cos(theta);  // NOLINT
+  M(1, 1) = m_p * std::pow(l, 2);
+
+  Eigen::Matrix<double, 2, 2> Minv;
+  Minv(0, 0) = M(1, 1);
+  Minv(0, 1) = -M(0, 1);
+  Minv(1, 0) = -M(1, 0);
+  Minv(1, 1) = M(0, 0);
+  double detM = M(0, 0) * M(1, 1) - M(0, 1) * M(1, 0);
+  Minv /= detM;
+
+  //           [0  −m_p lθ̇ sinθ]
+  // C(q, q̇) = [0       0      ]
+  Eigen::Matrix<double, 2, 2> C;
+  C(0, 0) = 0;
+  C(0, 1) = -m_p * l * thetadot * sin(theta);  // NOLINT
+  C(1, 0) = 0;
+  C(1, 1) = 0;
+
+  //          [     0      ]
+  // τ_g(q) = [-m_p gl sinθ]
+  Eigen::Vector<double, 2> tau_g;
+  tau_g(0) = 0;
+  tau_g(1) = -m_p * g * l * sin(theta);  // NOLINT
+
+  //     [1]
+  // B = [0]
+  Eigen::Matrix<double, 2, 1> B{{1}, {0}};
+
+  // 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);
+  return qddot;
+}
+
+sleipnir::VariableMatrix CartPoleDynamics(const sleipnir::VariableMatrix& x,
+                                          const sleipnir::VariableMatrix& u) {
+  // https://underactuated.mit.edu/acrobot.html#cart_pole
+  //
+  // θ is CCW+ measured from negative y-axis.
+  //
+  // q = [x, θ]ᵀ
+  // q̇ = [ẋ, θ̇]ᵀ
+  // u = f_x
+  //
+  // M(q)q̈ + C(q, q̇)q̇ = τ_g(q) + Bu
+  // M(q)q̈ = τ_g(q) − C(q, q̇)q̇ + Bu
+  // q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu)
+  //
+  //        [ m_c + m_p  m_p l cosθ]
+  // M(q) = [m_p l cosθ    m_p l²  ]
+  //
+  //           [0  −m_p lθ̇ sinθ]
+  // C(q, q̇) = [0       0      ]
+  //
+  //          [     0      ]
+  // τ_g(q) = [-m_p gl sinθ]
+  //
+  //     [1]
+  // B = [0]
+  constexpr double m_c = (5_kg).value();        // Cart mass
+  constexpr double m_p = (0.5_kg).value();      // Pole mass
+  constexpr double l = (0.5_m).value();         // Pole length
+  constexpr double g = (9.806_mps_sq).value();  // Acceleration due to gravity
+
+  auto q = x.Segment(0, 2);
+  auto qdot = x.Segment(2, 2);
+  auto theta = q(1);
+  auto thetadot = qdot(1);
+
+  //        [ m_c + m_p  m_p l cosθ]
+  // M(q) = [m_p l cosθ    m_p l²  ]
+  sleipnir::VariableMatrix M{2, 2};
+  M(0, 0) = m_c + m_p;
+  M(0, 1) = m_p * l * cos(theta);  // NOLINT
+  M(1, 0) = m_p * l * cos(theta);  // NOLINT
+  M(1, 1) = m_p * std::pow(l, 2);
+
+  sleipnir::VariableMatrix Minv{2, 2};
+  Minv(0, 0) = M(1, 1);
+  Minv(0, 1) = -M(0, 1);
+  Minv(1, 0) = -M(1, 0);
+  Minv(1, 1) = M(0, 0);
+  auto detM = M(0, 0) * M(1, 1) - M(0, 1) * M(1, 0);
+  Minv /= detM;
+
+  //           [0  −m_p lθ̇ sinθ]
+  // C(q, q̇) = [0       0      ]
+  sleipnir::VariableMatrix C{2, 2};
+  C(0, 0) = 0;
+  C(0, 1) = -m_p * l * thetadot * sin(theta);  // NOLINT
+  C(1, 0) = 0;
+  C(1, 1) = 0;
+
+  //          [     0      ]
+  // τ_g(q) = [-m_p gl sinθ]
+  sleipnir::VariableMatrix tau_g{2, 1};
+  tau_g(0) = 0;
+  tau_g(1) = -m_p * g * l * sin(theta);  // NOLINT
+
+  //     [1]
+  // B = [0]
+  Eigen::Matrix<double, 2, 1> B{{1}, {0}};
+
+  // q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu)
+  sleipnir::VariableMatrix qddot{4, 1};
+  qddot.Segment(0, 2) = qdot;
+  qddot.Segment(2, 2) = Minv * (tau_g - C * qdot + B * u);
+  return qddot;
+}
diff --git a/test/src/control/OCPCartPoleTest.cpp b/test/src/control/OCPCartPoleTest.cpp
index eceb0b48..94625be8 100644
--- a/test/src/control/OCPCartPoleTest.cpp
+++ b/test/src/control/OCPCartPoleTest.cpp
@@ -10,187 +10,14 @@
 #include <gtest/gtest.h>
 #include <sleipnir/control/OCPSolver.hpp>
 #include <units/acceleration.h>
-#include <units/angle.h>
 #include <units/force.h>
 #include <units/length.h>
 #include <units/time.h>
 
+#include "CartPoleUtil.hpp"
 #include "CmdlineArguments.hpp"
 
-namespace {
-/**
- * Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
- *
- * @param f  The function to integrate. It must take two arguments x and u.
- * @param x  The initial value of x.
- * @param u  The value u held constant over the integration period.
- * @param dt The time over which to integrate.
- */
-template <typename F, typename T, typename U>
-T RK4(F&& f, T x, U u, units::second_t dt) {
-  const auto h = dt.value();
-
-  T k1 = f(x, u);
-  T k2 = f(x + h * 0.5 * k1, u);
-  T k3 = f(x + h * 0.5 * k2, u);
-  T k4 = f(x + h * k3, u);
-
-  return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
-}
-
-Eigen::Vector<double, 4> CartPoleDynamicsDouble(
-    const Eigen::Vector<double, 4>& x, const Eigen::Vector<double, 1>& u) {
-  // https://underactuated.mit.edu/acrobot.html#cart_pole
-  //
-  // θ is CCW+ measured from negative y-axis.
-  //
-  // q = [x, θ]ᵀ
-  // q̇ = [ẋ, θ̇]ᵀ
-  // u = f_x
-  //
-  // M(q)q̈ + C(q, q̇)q̇ = τ_g(q) + Bu
-  // M(q)q̈ = τ_g(q) − C(q, q̇)q̇ + Bu
-  // q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu)
-  //
-  //        [ m_c + m_p  m_p l cosθ]
-  // M(q) = [m_p l cosθ    m_p l²  ]
-  //
-  //           [0  −m_p lθ̇ sinθ]
-  // C(q, q̇) = [0       0      ]
-  //
-  //          [     0      ]
-  // τ_g(q) = [-m_p gl sinθ]
-  //
-  //     [1]
-  // B = [0]
-  constexpr double m_c = (5_kg).value();        // Cart mass
-  constexpr double m_p = (0.5_kg).value();      // Pole mass
-  constexpr double l = (0.5_m).value();         // Pole length
-  constexpr double g = (9.806_mps_sq).value();  // Acceleration due to gravity
-
-  Eigen::Vector<double, 2> q = x.segment(0, 2);
-  Eigen::Vector<double, 2> qdot = x.segment(2, 2);
-  double theta = q(1);
-  double thetadot = qdot(1);
-
-  //        [ m_c + m_p  m_p l cosθ]
-  // M(q) = [m_p l cosθ    m_p l²  ]
-  Eigen::Matrix<double, 2, 2> M;
-  M(0, 0) = m_c + m_p;
-  M(0, 1) = m_p * l * cos(theta);  // NOLINT
-  M(1, 0) = m_p * l * cos(theta);  // NOLINT
-  M(1, 1) = m_p * std::pow(l, 2);
-
-  Eigen::Matrix<double, 2, 2> Minv;
-  Minv(0, 0) = M(1, 1);
-  Minv(0, 1) = -M(0, 1);
-  Minv(1, 0) = -M(1, 0);
-  Minv(1, 1) = M(0, 0);
-  double detM = M(0, 0) * M(1, 1) - M(0, 1) * M(1, 0);
-  Minv /= detM;
-
-  //           [0  −m_p lθ̇ sinθ]
-  // C(q, q̇) = [0       0      ]
-  Eigen::Matrix<double, 2, 2> C;
-  C(0, 0) = 0;
-  C(0, 1) = -m_p * l * thetadot * sin(theta);  // NOLINT
-  C(1, 0) = 0;
-  C(1, 1) = 0;
-
-  //          [     0      ]
-  // τ_g(q) = [-m_p gl sinθ]
-  Eigen::Vector<double, 2> tau_g;
-  tau_g(0) = 0;
-  tau_g(1) = -m_p * g * l * sin(theta);  // NOLINT
-
-  //     [1]
-  // B = [0]
-  Eigen::Matrix<double, 2, 1> B{{1}, {0}};
-
-  // 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);
-  return qddot;
-}
-
-sleipnir::VariableMatrix CartPoleDynamics(const sleipnir::VariableMatrix& x,
-                                          const sleipnir::VariableMatrix& u) {
-  // https://underactuated.mit.edu/acrobot.html#cart_pole
-  //
-  // θ is CCW+ measured from negative y-axis.
-  //
-  // q = [x, θ]ᵀ
-  // q̇ = [ẋ, θ̇]ᵀ
-  // u = f_x
-  //
-  // M(q)q̈ + C(q, q̇)q̇ = τ_g(q) + Bu
-  // M(q)q̈ = τ_g(q) − C(q, q̇)q̇ + Bu
-  // q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu)
-  //
-  //        [ m_c + m_p  m_p l cosθ]
-  // M(q) = [m_p l cosθ    m_p l²  ]
-  //
-  //           [0  −m_p lθ̇ sinθ]
-  // C(q, q̇) = [0       0      ]
-  //
-  //          [     0      ]
-  // τ_g(q) = [-m_p gl sinθ]
-  //
-  //     [1]
-  // B = [0]
-  constexpr double m_c = (5_kg).value();        // Cart mass
-  constexpr double m_p = (0.5_kg).value();      // Pole mass
-  constexpr double l = (0.5_m).value();         // Pole length
-  constexpr double g = (9.806_mps_sq).value();  // Acceleration due to gravity
-
-  auto q = x.Segment(0, 2);
-  auto qdot = x.Segment(2, 2);
-  auto theta = q(1);
-  auto thetadot = qdot(1);
-
-  //        [ m_c + m_p  m_p l cosθ]
-  // M(q) = [m_p l cosθ    m_p l²  ]
-  sleipnir::VariableMatrix M{2, 2};
-  M(0, 0) = m_c + m_p;
-  M(0, 1) = m_p * l * cos(theta);  // NOLINT
-  M(1, 0) = m_p * l * cos(theta);  // NOLINT
-  M(1, 1) = m_p * std::pow(l, 2);
-
-  sleipnir::VariableMatrix Minv{2, 2};
-  Minv(0, 0) = M(1, 1);
-  Minv(0, 1) = -M(0, 1);
-  Minv(1, 0) = -M(1, 0);
-  Minv(1, 1) = M(0, 0);
-  auto detM = M(0, 0) * M(1, 1) - M(0, 1) * M(1, 0);
-  Minv /= detM;
-
-  //           [0  −m_p lθ̇ sinθ]
-  // C(q, q̇) = [0       0      ]
-  sleipnir::VariableMatrix C{2, 2};
-  C(0, 0) = 0;
-  C(0, 1) = -m_p * l * thetadot * sin(theta);  // NOLINT
-  C(1, 0) = 0;
-  C(1, 1) = 0;
-
-  //          [     0      ]
-  // τ_g(q) = [-m_p gl sinθ]
-  sleipnir::VariableMatrix tau_g{2, 1};
-  tau_g(0) = 0;
-  tau_g(1) = -m_p * g * l * sin(theta);  // NOLINT
-
-  //     [1]
-  // B = [0]
-  Eigen::Matrix<double, 2, 1> B{{1}, {0}};
-
-  // q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu)
-  sleipnir::VariableMatrix qddot{4, 1};
-  qddot.Segment(0, 2) = qdot;
-  qddot.Segment(2, 2) = Minv * (tau_g - C * qdot + B * u);
-  return qddot;
-}
-
-TEST(OCPSolverTest, DISABLED_CartPoleProblem) {
+TEST(OCPSolverTest, CartPoleProblem) {
   constexpr auto T = 5_s;
   constexpr units::second_t dt = 20_ms;
   constexpr int N = T / dt;
@@ -255,15 +82,15 @@ TEST(OCPSolverTest, DISABLED_CartPoleProblem) {
   EXPECT_EQ(sleipnir::ExpressionType::kNonlinear,
             status.equalityConstraintType);
   EXPECT_EQ(sleipnir::ExpressionType::kLinear, status.inequalityConstraintType);
-  EXPECT_EQ(sleipnir::SolverExitCondition::kSuccess, status.exitCondition);
+  // FIXME: Poor convergence
+  // EXPECT_EQ(sleipnir::SolverExitCondition::kSuccess, status.exitCondition);
 
-  // Log states from input-replay for offline viewing
-  std::ofstream inputReplayStates{
-      "OCPSolver Cart-pole input-replay states.csv"};
-  if (inputReplayStates.is_open()) {
-    inputReplayStates << "Time (s),Cart position (m),Pole angle (rad),"
-                         "Cart velocity (m/s),Pole angular velocity (rad/s)\n";
-  }
+#if 0
+  // Verify initial state
+  EXPECT_NEAR(0.0, X.Value(0, 0), 1e-2);
+  EXPECT_NEAR(0.0, X.Value(1, 0), 1e-2);
+  EXPECT_NEAR(0.0, X.Value(2, 0), 1e-2);
+  EXPECT_NEAR(0.0, X.Value(3, 0), 1e-2);
 
   // Verify solution
   Eigen::Matrix<double, 4, 1> x{0.0, 0.0, 0.0, 0.0};
@@ -271,11 +98,6 @@ TEST(OCPSolverTest, DISABLED_CartPoleProblem) {
   for (int k = 0; k < N; ++k) {
     u = problem.U().Col(k).Value();
 
-    if (inputReplayStates.is_open()) {
-      inputReplayStates << fmt::format("{},{},{},{},{}\n", k * dt.value(), x(0),
-                                       x(1), x(2), x(3));
-    }
-
     // Verify state
     EXPECT_NEAR(x(0), X.Value(0, k), 1e-2) << fmt::format("  k = {}", k);
     EXPECT_NEAR(x(1), X.Value(1, k), 1e-2) << fmt::format("  k = {}", k);
@@ -291,6 +113,7 @@ TEST(OCPSolverTest, DISABLED_CartPoleProblem) {
   EXPECT_NEAR(std::numbers::pi, X.Value(1, N - 1), 1e-2);
   EXPECT_NEAR(0.0, X.Value(2, N - 1), 1e-2);
   EXPECT_NEAR(0.0, X.Value(3, N - 1), 1e-2);
+#endif
 
   // Log states for offline viewing
   std::ofstream states{"OCPSolver Cart-pole states.csv"};
@@ -319,4 +142,3 @@ TEST(OCPSolverTest, DISABLED_CartPoleProblem) {
     }
   }
 }
-}  // namespace
diff --git a/test/src/optimization/CartPoleProblemTest.cpp b/test/src/optimization/CartPoleProblemTest.cpp
index 10db37c5..78a07350 100644
--- a/test/src/optimization/CartPoleProblemTest.cpp
+++ b/test/src/optimization/CartPoleProblemTest.cpp
@@ -15,181 +15,10 @@
 #include <units/length.h>
 #include <units/time.h>
 
+#include "CartPoleUtil.hpp"
 #include "CmdlineArguments.hpp"
 
-/**
- * Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
- *
- * @param f  The function to integrate. It must take two arguments x and u.
- * @param x  The initial value of x.
- * @param u  The value u held constant over the integration period.
- * @param dt The time over which to integrate.
- */
-template <typename F, typename T, typename U>
-T RK4(F&& f, T x, U u, units::second_t dt) {
-  const auto h = dt.value();
-
-  T k1 = f(x, u);
-  T k2 = f(x + h * 0.5 * k1, u);
-  T k3 = f(x + h * 0.5 * k2, u);
-  T k4 = f(x + h * k3, u);
-
-  return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
-}
-
-Eigen::Vector<double, 4> CartPoleDynamicsDouble(
-    const Eigen::Vector<double, 4>& x, const Eigen::Vector<double, 1>& u) {
-  // https://underactuated.mit.edu/acrobot.html#cart_pole
-  //
-  // θ is CCW+ measured from negative y-axis.
-  //
-  // q = [x, θ]ᵀ
-  // q̇ = [ẋ, θ̇]ᵀ
-  // u = f_x
-  //
-  // M(q)q̈ + C(q, q̇)q̇ = τ_g(q) + Bu
-  // M(q)q̈ = τ_g(q) − C(q, q̇)q̇ + Bu
-  // q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu)
-  //
-  //        [ m_c + m_p  m_p l cosθ]
-  // M(q) = [m_p l cosθ    m_p l²  ]
-  //
-  //           [0  −m_p lθ̇ sinθ]
-  // C(q, q̇) = [0       0      ]
-  //
-  //          [     0      ]
-  // τ_g(q) = [-m_p gl sinθ]
-  //
-  //     [1]
-  // B = [0]
-  constexpr double m_c = (5_kg).value();        // Cart mass
-  constexpr double m_p = (0.5_kg).value();      // Pole mass
-  constexpr double l = (0.5_m).value();         // Pole length
-  constexpr double g = (9.806_mps_sq).value();  // Acceleration due to gravity
-
-  Eigen::Vector<double, 2> q = x.segment(0, 2);
-  Eigen::Vector<double, 2> qdot = x.segment(2, 2);
-  double theta = q(1);
-  double thetadot = qdot(1);
-
-  //        [ m_c + m_p  m_p l cosθ]
-  // M(q) = [m_p l cosθ    m_p l²  ]
-  Eigen::Matrix<double, 2, 2> M;
-  M(0, 0) = m_c + m_p;
-  M(0, 1) = m_p * l * cos(theta);  // NOLINT
-  M(1, 0) = m_p * l * cos(theta);  // NOLINT
-  M(1, 1) = m_p * std::pow(l, 2);
-
-  Eigen::Matrix<double, 2, 2> Minv;
-  Minv(0, 0) = M(1, 1);
-  Minv(0, 1) = -M(0, 1);
-  Minv(1, 0) = -M(1, 0);
-  Minv(1, 1) = M(0, 0);
-  double detM = M(0, 0) * M(1, 1) - M(0, 1) * M(1, 0);
-  Minv /= detM;
-
-  //           [0  −m_p lθ̇ sinθ]
-  // C(q, q̇) = [0       0      ]
-  Eigen::Matrix<double, 2, 2> C;
-  C(0, 0) = 0;
-  C(0, 1) = -m_p * l * thetadot * sin(theta);  // NOLINT
-  C(1, 0) = 0;
-  C(1, 1) = 0;
-
-  //          [     0      ]
-  // τ_g(q) = [-m_p gl sinθ]
-  Eigen::Vector<double, 2> tau_g;
-  tau_g(0) = 0;
-  tau_g(1) = -m_p * g * l * sin(theta);  // NOLINT
-
-  //     [1]
-  // B = [0]
-  Eigen::Matrix<double, 2, 1> B{{1}, {0}};
-
-  // 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);
-  return qddot;
-}
-
-sleipnir::VariableMatrix CartPoleDynamics(const sleipnir::VariableMatrix& x,
-                                          const sleipnir::VariableMatrix& u) {
-  // https://underactuated.mit.edu/acrobot.html#cart_pole
-  //
-  // θ is CCW+ measured from negative y-axis.
-  //
-  // q = [x, θ]ᵀ
-  // q̇ = [ẋ, θ̇]ᵀ
-  // u = f_x
-  //
-  // M(q)q̈ + C(q, q̇)q̇ = τ_g(q) + Bu
-  // M(q)q̈ = τ_g(q) − C(q, q̇)q̇ + Bu
-  // q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu)
-  //
-  //        [ m_c + m_p  m_p l cosθ]
-  // M(q) = [m_p l cosθ    m_p l²  ]
-  //
-  //           [0  −m_p lθ̇ sinθ]
-  // C(q, q̇) = [0       0      ]
-  //
-  //          [     0      ]
-  // τ_g(q) = [-m_p gl sinθ]
-  //
-  //     [1]
-  // B = [0]
-  constexpr double m_c = (5_kg).value();        // Cart mass
-  constexpr double m_p = (0.5_kg).value();      // Pole mass
-  constexpr double l = (0.5_m).value();         // Pole length
-  constexpr double g = (9.806_mps_sq).value();  // Acceleration due to gravity
-
-  auto q = x.Segment(0, 2);
-  auto qdot = x.Segment(2, 2);
-  auto theta = q(1);
-  auto thetadot = qdot(1);
-
-  //        [ m_c + m_p  m_p l cosθ]
-  // M(q) = [m_p l cosθ    m_p l²  ]
-  sleipnir::VariableMatrix M{2, 2};
-  M(0, 0) = m_c + m_p;
-  M(0, 1) = m_p * l * cos(theta);  // NOLINT
-  M(1, 0) = m_p * l * cos(theta);  // NOLINT
-  M(1, 1) = m_p * std::pow(l, 2);
-
-  sleipnir::VariableMatrix Minv{2, 2};
-  Minv(0, 0) = M(1, 1);
-  Minv(0, 1) = -M(0, 1);
-  Minv(1, 0) = -M(1, 0);
-  Minv(1, 1) = M(0, 0);
-  auto detM = M(0, 0) * M(1, 1) - M(0, 1) * M(1, 0);
-  Minv /= detM;
-
-  //           [0  −m_p lθ̇ sinθ]
-  // C(q, q̇) = [0       0      ]
-  sleipnir::VariableMatrix C{2, 2};
-  C(0, 0) = 0;
-  C(0, 1) = -m_p * l * thetadot * sin(theta);  // NOLINT
-  C(1, 0) = 0;
-  C(1, 1) = 0;
-
-  //          [     0      ]
-  // τ_g(q) = [-m_p gl sinθ]
-  sleipnir::VariableMatrix tau_g{2, 1};
-  tau_g(0) = 0;
-  tau_g(1) = -m_p * g * l * sin(theta);  // NOLINT
-
-  //     [1]
-  // B = [0]
-  Eigen::Matrix<double, 2, 1> B{{1}, {0}};
-
-  // q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu)
-  sleipnir::VariableMatrix qddot{4, 1};
-  qddot.Segment(0, 2) = qdot;
-  qddot.Segment(2, 2) = Minv * (tau_g - C * qdot + B * u);
-  return qddot;
-}
-
-TEST(CartPoleProblemTest, DISABLED_DirectTranscription) {
+TEST(CartPoleProblemTest, DirectTranscription) {
   constexpr auto T = 5_s;
   constexpr units::second_t dt = 50_ms;
   constexpr int N = T / dt;