diff --git a/frc971/control_loops/swerve/BUILD b/frc971/control_loops/swerve/BUILD
index 29dbf76..3755ab4 100644
--- a/frc971/control_loops/swerve/BUILD
+++ b/frc971/control_loops/swerve/BUILD
@@ -170,7 +170,7 @@
 )
 
 cc_library(
-    name = "dynamics",
+    name = "eigen_dynamics",
     srcs = ["dynamics.cc"],
     hdrs = ["dynamics.h"],
     deps = [
@@ -196,14 +196,49 @@
 py_test(
     name = "physics_test",
     srcs = [
-        "bigcaster_dynamics.py",
-        "dynamics.py",
-        "nocaster_dynamics.py",
         "physics_test.py",
     ],
     target_compatible_with = ["@platforms//cpu:x86_64"],
     deps = [
+        ":physics_test_utils",
         "@pip//casadi",
         "@pip//numpy",
     ],
 )
+
+py_library(
+    name = "dynamics",
+    srcs = [
+        "bigcaster_dynamics.py",
+        "dynamics.py",
+        "nocaster_dynamics.py",
+    ],
+    deps = [
+        "@pip//casadi",
+    ],
+)
+
+py_library(
+    name = "physics_test_utils",
+    srcs = [
+        "physics_test_utils.py",
+    ],
+    deps = [
+        ":dynamics",
+        "@pip//casadi",
+        "@pip//numpy",
+    ],
+)
+
+py_binary(
+    name = "physics_debug",
+    srcs = [
+        "physics_debug.py",
+    ],
+    deps = [
+        ":physics_test_utils",
+        "@pip//matplotlib",
+        "@pip//pygobject",
+        "@pip//scipy",
+    ],
+)
diff --git a/frc971/control_loops/swerve/physics_debug.py b/frc971/control_loops/swerve/physics_debug.py
new file mode 100644
index 0000000..db9ea85
--- /dev/null
+++ b/frc971/control_loops/swerve/physics_debug.py
@@ -0,0 +1,265 @@
+#!/usr/bin/python3
+
+import numpy, scipy
+
+from matplotlib import pylab
+from frc971.control_loops.swerve import physics_test_utils as utils
+from frc971.control_loops.swerve import dynamics
+
+
+class PhysicsDebug(object):
+
+    def wrap(self, python_module):
+        self.swerve_physics = utils.wrap(python_module.swerve_physics)
+        self.contact_patch_velocity = [
+            utils.wrap_module(python_module.contact_patch_velocity, i)
+            for i in range(4)
+        ]
+        self.wheel_ground_velocity = [
+            utils.wrap_module(python_module.wheel_ground_velocity, i)
+            for i in range(4)
+        ]
+        self.wheel_slip_velocity = [
+            utils.wrap_module(python_module.wheel_slip_velocity, i)
+            for i in range(4)
+        ]
+        self.wheel_force = [
+            utils.wrap_module(python_module.wheel_force, i) for i in range(4)
+        ]
+        self.module_angular_accel = [
+            utils.wrap_module(python_module.module_angular_accel, i)
+            for i in range(4)
+        ]
+        self.F = [utils.wrap_module(python_module.F, i) for i in range(4)]
+        self.mounting_location = [
+            utils.wrap_module(python_module.mounting_location, i)
+            for i in range(4)
+        ]
+
+        self.slip_angle = [
+            utils.wrap_module(python_module.slip_angle, i) for i in range(4)
+        ]
+        self.slip_ratio = [
+            utils.wrap_module(python_module.slip_ratio, i) for i in range(4)
+        ]
+        self.Ms = [utils.wrap_module(python_module.Ms, i) for i in range(4)]
+
+    def print_state(self, swerve_physics, I, x):
+        xdot = swerve_physics(x, I)
+
+        for i in range(4):
+            print(f"  Slip Angle {i} {self.slip_angle[0](x, I)}")
+            print(f"  Slip Ratio {i} {self.slip_ratio[0](x, I)}")
+
+        print("  Steering angle0", x[0])
+        print("  Steering angle1", x[4])
+        print("  Steering angle2", x[8])
+        print("  Steering angle3", x[12])
+        print("  Steering velocity0", xdot[0])
+        print("  Steering velocity1", xdot[4])
+        print("  Steering velocity2", xdot[8])
+        print("  Steering velocity3", xdot[12])
+        print("  Steering accel0", xdot[2])
+        print("  Steering accel1", xdot[6])
+        print("  Steering accel2", xdot[10])
+        print("  Steering accel3", xdot[14])
+        print("  Drive accel0", xdot[3])
+        print("  Drive accel1", xdot[7])
+        print("  Drive accel2", xdot[11])
+        print("  Drive accel3", xdot[15])
+        print("  Drive velocity0", x[3] * 2 * 0.0254)
+        print("  Drive velocity1", x[7] * 2 * 0.0254)
+        print("  Drive velocity2", x[11] * 2 * 0.0254)
+        print("  Drive velocity3", x[15] * 2 * 0.0254)
+        print("  Theta ", x[18])
+        print("  Omega ", x[21])
+        print("  Alpha", xdot[21])
+        print("  vx", xdot[16])
+        print("  vy", xdot[17])
+        print("  ax", xdot[19])
+        print("  ay", xdot[20])
+        print("  Fdx ", x[22])
+        print("  Fdy ", x[23])
+        print("  Moment_d", xdot[24])
+
+    def plot(self):
+        velocity = numpy.array([[1.0], [0.0]])
+
+        flip = False
+        if flip:
+            module_angles = [0.01 + numpy.pi] * 4
+        else:
+            module_angles = [-0.1, -0.1, 0.1, 0.1]
+
+        X = utils.state_vector(
+            velocity=velocity,
+            drive_wheel_velocity=-1.0 if flip else 1.0,
+            omega=0.0,
+            module_angles=module_angles,
+        )
+
+        self.I = numpy.array([[40.0], [0.0], [40.0], [0.0], [40.0], [0.0],
+                              [40.0], [0.0]])
+
+        def calc_I(t, x):
+            x_goal = numpy.zeros((16, 1))
+
+            Kp_steer = 15.0
+            Kp_drive = 0.0
+            Kd_steer = 7.0
+            Kd_drive = 0.0
+            Idrive = 5.0 if t < 5.0 else 15.0
+
+            return numpy.array([
+                [
+                    Kd_steer * (x_goal[2] - x[2]) + Kp_steer *
+                    (x_goal[0] - x[0])
+                ],
+                [Idrive],
+                [
+                    Kd_steer * (x_goal[6] - x[6]) + Kp_steer *
+                    (x_goal[4] - x[4])
+                ],
+                [Idrive],
+                [
+                    Kd_steer * (x_goal[10] - x[10]) + Kp_steer *
+                    (x_goal[8] - x[8])
+                ],
+                [Idrive],
+                [
+                    Kd_steer * (x_goal[14] - x[14]) + Kp_steer *
+                    (x_goal[12] - x[12])
+                ],
+                [Idrive],
+            ]).flatten()
+            return numpy.array([
+                [
+                    Kd_steer * (x_goal[2] - x[2]) + Kp_steer *
+                    (x_goal[0] - x[0])
+                ],
+                [
+                    Kd_drive * (x_goal[3] - x[3]) + Kp_drive *
+                    (x_goal[1] - x[1])
+                ],
+                [
+                    Kd_steer * (x_goal[6] - x[6]) + Kp_steer *
+                    (x_goal[4] - x[4])
+                ],
+                [
+                    Kd_drive * (x_goal[7] - x[7]) + Kp_drive *
+                    (x_goal[5] - x[5])
+                ],
+                [
+                    Kd_steer * (x_goal[10] - x[10]) + Kp_steer *
+                    (x_goal[8] - x[8])
+                ],
+                [
+                    Kd_drive * (x_goal[11] - x[11]) + Kp_drive *
+                    (x_goal[9] - x[9])
+                ],
+                [
+                    Kd_steer * (x_goal[14] - x[14]) + Kp_steer *
+                    (x_goal[12] - x[12])
+                ],
+                [
+                    Kd_drive * (x_goal[15] - x[15]) + Kp_drive *
+                    (x_goal[13] - x[13])
+                ],
+            ]).flatten()
+
+        t_eval = numpy.arange(0, 10.0, 0.005)
+        self.wrap(dynamics)
+        result = scipy.integrate.solve_ivp(
+            lambda t, x: self.swerve_physics(x, calc_I(t, x)).flatten(),
+            [0, t_eval[-1]],
+            X.flatten(),
+            t_eval=t_eval,
+        )
+        print("Function evaluations", result.nfev)
+
+        # continue
+        print(result.y.shape)
+        print(result.t.shape)
+        xdot = numpy.zeros(result.y.shape)
+        print("shape", xdot.shape)
+        for i in range(xdot.shape[1]):
+            xdot[:, i] = self.swerve_physics(result.y[:, i], self.I)[:, 0]
+
+        for i in range(2):
+            print(f"For t {i * 0.005}")
+            self.print_state(self.swerve_physics, self.I, result.y[:, i])
+
+        def ev(fn, Y):
+            return [fn(Y[:, i], self.I)[0, 0] for i in range(Y.shape[1])]
+
+        fig, axs = pylab.subplots(2)
+        axs[0].plot(result.t, result.y[0, :], label="steer0")
+        axs[0].plot(result.t, result.y[4, :], label="steer1")
+        axs[0].plot(result.t, result.y[8, :], label="steer2")
+        axs[0].plot(result.t, result.y[12, :], label="steer3")
+        axs[0].legend()
+        axs[1].plot(result.t, result.y[2, :], label="steer_velocity0")
+        axs[1].plot(result.t, result.y[6, :], label="steer_velocity1")
+        axs[1].plot(result.t, result.y[10, :], label="steer_velocity2")
+        axs[1].plot(result.t, result.y[14, :], label="steer_velocity3")
+        axs[1].legend()
+
+        fig, axs = pylab.subplots(2)
+        for i in range(len(axs)):
+            axs[i].plot(result.t,
+                        ev(self.slip_angle[i], result.y),
+                        label=f"slip_angle{i}")
+            axs[i].plot(result.t,
+                        ev(self.slip_ratio[i], result.y),
+                        label=f"slip_ratio{i}")
+            axs[i].plot(result.t,
+                        xdot[2 + i * 4, :],
+                        label=f'steering_accel{i}')
+            axs[i].legend()
+
+        fig, axs = pylab.subplots(3)
+        axs[0].plot(result.t, result.y[3, :], label="drive_velocity0")
+        axs[0].plot(result.t, result.y[7, :], label="drive_velocity1")
+        axs[0].plot(result.t, result.y[11, :], label="drive_velocity2")
+        axs[0].plot(result.t, result.y[15, :], label="drive_velocity3")
+        axs[0].legend()
+
+        axs[1].plot(result.t, result.y[20, :], label="vy")
+        axs[1].plot(result.t, result.y[21, :], label="omega")
+        axs[1].plot(result.t, xdot[20, :], label="ay")
+        axs[1].plot(result.t, xdot[21, :], label="alpha")
+        axs[1].legend()
+
+        axs[2].plot(result.t, result.y[19, :], label="vx")
+        axs[2].plot(result.t, xdot[19, :], label="ax")
+
+        axs[2].plot(result.t,
+                    numpy.hypot(result.y[19, :], result.y[20, :]),
+                    label="speed")
+        axs[2].legend()
+
+        pylab.figure()
+        U_control = numpy.zeros((8, 1))
+
+        for i in range(numpy.shape(result.t)[0]):
+            U_control = numpy.hstack((
+                U_control,
+                numpy.reshape(
+                    calc_I(result.t[i], numpy.reshape(result.y[:, i],
+                                                      (25, 1))),
+                    (8, 1),
+                ),
+            ))
+
+        U_control = numpy.delete(U_control, 0, 1)
+
+        pylab.plot(result.t, U_control[0, :], label="Is0")
+        pylab.plot(result.t, U_control[1, :], label="Id0")
+        pylab.legend()
+
+        pylab.show()
+
+
+if __name__ == "__main__":
+    debug = PhysicsDebug()
+    debug.plot()
diff --git a/frc971/control_loops/swerve/physics_test.py b/frc971/control_loops/swerve/physics_test.py
index ecfc1ce..c7d60e9 100644
--- a/frc971/control_loops/swerve/physics_test.py
+++ b/frc971/control_loops/swerve/physics_test.py
@@ -9,101 +9,46 @@
 from frc971.control_loops.swerve import bigcaster_dynamics
 from frc971.control_loops.swerve import dynamics
 from frc971.control_loops.swerve import nocaster_dynamics
-
-
-def state_vector(velocity=numpy.array([[1.0], [0.0]]),
-                 dx=0.0,
-                 dy=0.0,
-                 theta=0.0,
-                 omega=0.0,
-                 module_omega=0.0,
-                 module_angle=0.0,
-                 drive_wheel_velocity=None,
-                 module_angles=None):
-    """Returns the state vector with the requested state."""
-    X_initial = numpy.zeros((25, 1))
-    # All the wheels are spinning at the speed needed to hit the velocity in m/s
-    drive_wheel_velocity = (drive_wheel_velocity
-                            or numpy.linalg.norm(velocity))
-
-    X_initial[2, 0] = module_omega
-    X_initial[3, 0] = drive_wheel_velocity / (dynamics.WHEEL_RADIUS)
-
-    X_initial[6, 0] = module_omega
-    X_initial[7, 0] = drive_wheel_velocity / (dynamics.WHEEL_RADIUS)
-
-    X_initial[10, 0] = module_omega
-    X_initial[11, 0] = drive_wheel_velocity / (dynamics.WHEEL_RADIUS)
-
-    X_initial[14, 0] = module_omega
-    X_initial[15, 0] = drive_wheel_velocity / (dynamics.WHEEL_RADIUS)
-
-    X_initial[0, 0] = module_angle
-    X_initial[4, 0] = module_angle
-    X_initial[8, 0] = module_angle
-    X_initial[12, 0] = module_angle
-
-    if module_angles is not None:
-        assert (len(module_angles) == 4)
-        X_initial[0, 0] = module_angles[0]
-        X_initial[4, 0] = module_angles[1]
-        X_initial[8, 0] = module_angles[2]
-        X_initial[12, 0] = module_angles[3]
-
-    X_initial[18, 0] = theta
-
-    X_initial[19, 0] = velocity[0, 0] + dx
-    X_initial[20, 0] = velocity[1, 0] + dy
-    X_initial[21, 0] = omega
-
-    return X_initial
-
-
-def wrap(fn):
-    evaluated_fn = fn(casadi.SX.sym("X", 25, 1), casadi.SX.sym("U", 8, 1))
-    return lambda X, U: numpy.array(evaluated_fn(X, U))
-
-
-def wrap_module(fn, i):
-    evaluated_fn = fn(i, casadi.SX.sym("X", 25, 1), casadi.SX.sym("U", 8, 1))
-    return lambda X, U: numpy.array(evaluated_fn(X, U))
+from frc971.control_loops.swerve import physics_test_utils as utils
 
 
 class TestSwervePhysics(unittest.TestCase):
     I = numpy.zeros((8, 1))
 
     def wrap(self, python_module):
-        self.swerve_physics = wrap(python_module.swerve_physics)
+        self.swerve_physics = utils.wrap(python_module.swerve_physics)
         self.contact_patch_velocity = [
-            wrap_module(python_module.contact_patch_velocity, i)
+            utils.wrap_module(python_module.contact_patch_velocity, i)
             for i in range(4)
         ]
         self.wheel_ground_velocity = [
-            wrap_module(python_module.wheel_ground_velocity, i)
+            utils.wrap_module(python_module.wheel_ground_velocity, i)
             for i in range(4)
         ]
         self.wheel_slip_velocity = [
-            wrap_module(python_module.wheel_slip_velocity, i) for i in range(4)
-        ]
-        self.wheel_force = [
-            wrap_module(python_module.wheel_force, i) for i in range(4)
-        ]
-        self.module_angular_accel = [
-            wrap_module(python_module.module_angular_accel, i)
+            utils.wrap_module(python_module.wheel_slip_velocity, i)
             for i in range(4)
         ]
-        self.F = [wrap_module(python_module.F, i) for i in range(4)]
+        self.wheel_force = [
+            utils.wrap_module(python_module.wheel_force, i) for i in range(4)
+        ]
+        self.module_angular_accel = [
+            utils.wrap_module(python_module.module_angular_accel, i)
+            for i in range(4)
+        ]
+        self.F = [utils.wrap_module(python_module.F, i) for i in range(4)]
         self.mounting_location = [
-            wrap_module(python_module.mounting_location, i) for i in range(4)
+            utils.wrap_module(python_module.mounting_location, i)
+            for i in range(4)
         ]
 
         self.slip_angle = [
-            wrap_module(python_module.slip_angle, i) for i in range(4)
+            utils.wrap_module(python_module.slip_angle, i) for i in range(4)
         ]
         self.slip_ratio = [
-            wrap_module(python_module.slip_ratio, i) for i in range(4)
+            utils.wrap_module(python_module.slip_ratio, i) for i in range(4)
         ]
-        self.Ms = [wrap_module(python_module.Ms, i) for i in range(4)]
+        self.Ms = [utils.wrap_module(python_module.Ms, i) for i in range(4)]
 
     def setUp(self):
         self.wrap(dynamics)
@@ -111,10 +56,8 @@
     def test_contact_patch_velocity(self):
         """Tests that the contact patch velocity makes sense."""
         for i in range(4):
-            contact_patch_velocity = wrap_module(
-                dynamics.contact_patch_velocity, i)
-            wheel_ground_velocity = wrap_module(dynamics.wheel_ground_velocity,
-                                                i)
+            contact_patch_velocity = self.contact_patch_velocity[i]
+            wheel_ground_velocity = self.wheel_ground_velocity[i]
 
             # No angular velocity should result in just linear motion.
             for velocity in [
@@ -126,7 +69,8 @@
             ]:
                 for theta in [0.0, 1.0, -1.0, 100.0]:
                     patch_velocity = contact_patch_velocity(
-                        state_vector(velocity=velocity, theta=theta), self.I)
+                        utils.state_vector(velocity=velocity, theta=theta),
+                        self.I)
 
                     assert_array_equal(patch_velocity, velocity)
 
@@ -137,33 +81,41 @@
                 for omega in [0.65, -0.1]:
                     # Point each module to the center to make the math easier.
                     patch_velocity = contact_patch_velocity(
-                        state_vector(velocity=numpy.array([[0.0], [0.0]]),
-                                     theta=theta,
-                                     omega=omega,
-                                     module_angle=module_center_of_mass_angle),
-                        self.I)
+                        utils.state_vector(
+                            velocity=numpy.array([[0.0], [0.0]]),
+                            theta=theta,
+                            omega=omega,
+                            module_angle=module_center_of_mass_angle,
+                        ),
+                        self.I,
+                    )
 
                     assert_array_almost_equal(
                         patch_velocity,
                         (dynamics.ROBOT_WIDTH / numpy.sqrt(2.0) -
-                         dynamics.CASTER) * omega * numpy.array([[
-                             -numpy.sin(theta + module_center_of_mass_angle)
-                         ], [numpy.cos(theta + module_center_of_mass_angle)]]))
+                         dynamics.CASTER) * omega * numpy.array([
+                             [-numpy.sin(theta + module_center_of_mass_angle)],
+                             [numpy.cos(theta + module_center_of_mass_angle)],
+                         ]),
+                    )
 
                     # Point the wheel along +x, rotate it by theta, then spin it.
                     # Confirm the velocities come out right.
                     patch_velocity = contact_patch_velocity(
-                        state_vector(
+                        utils.state_vector(
                             velocity=numpy.array([[0.0], [0.0]]),
                             theta=-module_center_of_mass_angle,
                             module_omega=omega,
-                            module_angle=(theta +
-                                          module_center_of_mass_angle)),
-                        self.I)
+                            module_angle=(theta + module_center_of_mass_angle),
+                        ),
+                        self.I,
+                    )
 
                     assert_array_almost_equal(
-                        patch_velocity, -dynamics.CASTER * omega *
-                        numpy.array([[-numpy.sin(theta)], [numpy.cos(theta)]]))
+                        patch_velocity,
+                        -dynamics.CASTER * omega *
+                        numpy.array([[-numpy.sin(theta)], [numpy.cos(theta)]]),
+                    )
 
             # Now, test that the rotation back to wheel coordinates works.
             # The easiest way to do this is to point the wheel in a direction,
@@ -172,11 +124,16 @@
                 for module_angle in [0.0, 1.0, -5.0]:
                     wheel_patch_velocity = numpy.array(
                         wheel_ground_velocity(
-                            state_vector(velocity=numpy.array(
-                                [[numpy.cos(robot_angle + module_angle)],
-                                 [numpy.sin(robot_angle + module_angle)]]),
-                                         theta=robot_angle,
-                                         module_angle=module_angle), self.I))
+                            utils.state_vector(
+                                velocity=numpy.array([
+                                    [numpy.cos(robot_angle + module_angle)],
+                                    [numpy.sin(robot_angle + module_angle)],
+                                ]),
+                                theta=robot_angle,
+                                module_angle=module_angle,
+                            ),
+                            self.I,
+                        ))
 
                     assert_array_almost_equal(wheel_patch_velocity,
                                               numpy.array([[1], [0]]))
@@ -193,21 +150,25 @@
                     # We have redefined the angle to be the sin of the angle.
                     # That way, when the module flips directions, the slip angle also flips
                     # directions to keep it stable.
-                    computed_angle = self.slip_angle[i](state_vector(
-                        velocity=velocity,
-                        module_angle=numpy.pi * wrap + theta), self.I)[0, 0]
+                    computed_angle = self.slip_angle[i](
+                        utils.state_vector(velocity=velocity,
+                                           module_angle=numpy.pi * wrap +
+                                           theta),
+                        self.I,
+                    )[0, 0]
 
                     expected = numpy.sin(numpy.pi * wrap + theta)
 
                     self.assertAlmostEqual(
                         expected,
                         computed_angle,
-                        msg=f"Trying wrap {wrap} theta {theta}")
+                        msg=f"Trying wrap {wrap} theta {theta}",
+                    )
 
     def test_wheel_torque(self):
         """Tests that the per module self aligning forces have the right signs."""
         # Point all the modules in a little bit.
-        X = state_vector(
+        X = utils.state_vector(
             velocity=numpy.array([[1.0], [0.0]]),
             module_angles=[-0.001, -0.001, 0.001, 0.001],
         )
@@ -234,7 +195,7 @@
 
         # Now, make the bot want to go left by going to the other side.
         # The wheels will be going too fast based on our calcs, so they should be decelerating.
-        X = state_vector(
+        X = utils.state_vector(
             velocity=numpy.array([[1.0], [0.0]]),
             module_angles=[0.01, 0.01, 0.01, 0.01],
         )
@@ -262,7 +223,7 @@
         self.assertAlmostEqual(xdot_left[21, 0], 0.0)
 
         # And now do it to the right too.
-        X = state_vector(
+        X = utils.state_vector(
             velocity=numpy.array([[1.0], [0.0]]),
             module_angles=[-0.01, -0.01, -0.01, -0.01],
         )
@@ -293,7 +254,7 @@
         """Tests that the per module self aligning forces have the right signs when going backwards."""
         self.wrap(nocaster_dynamics)
         # Point all the modules in a little bit, going backwards.
-        X = state_vector(
+        X = utils.state_vector(
             velocity=numpy.array([[1.0], [0.0]]),
             module_angles=[
                 numpy.pi - 0.001,
@@ -326,7 +287,7 @@
 
         # Now, make the bot want to go left by going to the other side.
         # The wheels will be going too fast based on our calcs, so they should be decelerating.
-        X = state_vector(
+        X = utils.state_vector(
             velocity=numpy.array([[1.0], [0.0]]),
             module_angles=[numpy.pi + 0.01] * 4,
             drive_wheel_velocity=-1,
@@ -355,7 +316,7 @@
         self.assertAlmostEqual(xdot_left[21, 0], 0.0)
 
         # And now do it to the right too.
-        X = state_vector(
+        X = utils.state_vector(
             velocity=numpy.array([[1.0], [0.0]]),
             drive_wheel_velocity=-1,
             module_angles=[-0.01 + numpy.pi] * 4,
@@ -387,7 +348,7 @@
         """Tests that the per module self aligning forces have the right signs when going backwards with a lot of caster."""
         self.wrap(bigcaster_dynamics)
         # Point all the modules in a little bit, going backwards.
-        X = state_vector(
+        X = utils.state_vector(
             velocity=numpy.array([[1.0], [0.0]]),
             module_angles=[
                 numpy.pi - 0.001,
@@ -420,7 +381,7 @@
 
         # Now, make the bot want to go left by going to the other side.
         # The wheels will be going too fast based on our calcs, so they should be decelerating.
-        X = state_vector(
+        X = utils.state_vector(
             velocity=numpy.array([[1.0], [0.0]]),
             module_angles=[numpy.pi + 0.01] * 4,
             drive_wheel_velocity=-1,
@@ -449,7 +410,7 @@
         self.assertAlmostEqual(xdot_left[21, 0], 0.0)
 
         # And now do it to the right too.
-        X = state_vector(
+        X = utils.state_vector(
             velocity=numpy.array([[1.0], [0.0]]),
             drive_wheel_velocity=-1,
             module_angles=[-0.01 + numpy.pi] * 4,
@@ -480,9 +441,9 @@
     def test_wheel_forces(self):
         """Tests that the per module forces have the right signs."""
         for i in range(4):
-            wheel_force = wrap_module(dynamics.wheel_force, i)
+            wheel_force = self.wheel_force[i]
 
-            X = state_vector()
+            X = utils.state_vector()
             robot_equal = wheel_force(X, self.I)
             xdot_equal = self.swerve_physics(X, self.I)
             self.assertEqual(robot_equal[0, 0], 0.0)
@@ -491,7 +452,7 @@
             self.assertEqual(xdot_equal[3 + 4 * i], 0.0)
 
             # Robot is moving faster than the wheels, it should decelerate.
-            X = state_vector(dx=0.01)
+            X = utils.state_vector(dx=0.01)
             robot_faster = wheel_force(X, self.I)
             xdot_faster = self.swerve_physics(X, self.I)
             self.assertLess(robot_faster[0, 0], -0.1)
@@ -499,7 +460,7 @@
             self.assertGreater(xdot_faster[3 + 4 * i], 0.0)
 
             # Robot is now going slower than the wheels.  It should accelerate.
-            X = state_vector(dx=-0.01)
+            X = utils.state_vector(dx=-0.01)
             robot_slower = wheel_force(X, self.I)
             xdot_slower = self.swerve_physics(X, self.I)
             self.assertGreater(robot_slower[0, 0], 0.1)
@@ -507,15 +468,15 @@
             self.assertLess(xdot_slower[3 + 4 * i], 0.0)
 
             # Positive lateral velocity -> negative force.
-            robot_left = wheel_force(state_vector(dy=0.01), self.I)
+            robot_left = wheel_force(utils.state_vector(dy=0.01), self.I)
             self.assertEqual(robot_left[0, 0], 0.0)
             self.assertLess(robot_left[1, 0], -0.1)
 
             # Negative lateral velocity -> positive force.
-            robot_right = wheel_force(state_vector(dy=-0.01), self.I)
+            robot_right = wheel_force(utils.state_vector(dy=-0.01), self.I)
             self.assertEqual(robot_right[0, 0], 0.0)
             self.assertGreater(robot_right[1, 0], 0.1)
 
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     unittest.main()
diff --git a/frc971/control_loops/swerve/physics_test_utils.py b/frc971/control_loops/swerve/physics_test_utils.py
new file mode 100644
index 0000000..30b352f
--- /dev/null
+++ b/frc971/control_loops/swerve/physics_test_utils.py
@@ -0,0 +1,64 @@
+#!/usr/bin/python3
+
+import casadi, numpy
+
+from frc971.control_loops.swerve import dynamics
+
+
+def state_vector(
+    velocity=numpy.array([[1.0], [0.0]]),
+    dx=0.0,
+    dy=0.0,
+    theta=0.0,
+    omega=0.0,
+    module_omega=0.0,
+    module_angle=0.0,
+    drive_wheel_velocity=None,
+    module_angles=None,
+):
+    """Returns the state vector with the requested state."""
+    X_initial = numpy.zeros((25, 1))
+    # All the wheels are spinning at the speed needed to hit the velocity in m/s
+    drive_wheel_velocity = drive_wheel_velocity or numpy.linalg.norm(velocity)
+
+    X_initial[2, 0] = module_omega
+    X_initial[3, 0] = drive_wheel_velocity / (dynamics.WHEEL_RADIUS)
+
+    X_initial[6, 0] = module_omega
+    X_initial[7, 0] = drive_wheel_velocity / (dynamics.WHEEL_RADIUS)
+
+    X_initial[10, 0] = module_omega
+    X_initial[11, 0] = drive_wheel_velocity / (dynamics.WHEEL_RADIUS)
+
+    X_initial[14, 0] = module_omega
+    X_initial[15, 0] = drive_wheel_velocity / (dynamics.WHEEL_RADIUS)
+
+    X_initial[0, 0] = module_angle
+    X_initial[4, 0] = module_angle
+    X_initial[8, 0] = module_angle
+    X_initial[12, 0] = module_angle
+
+    if module_angles is not None:
+        assert len(module_angles) == 4
+        X_initial[0, 0] = module_angles[0]
+        X_initial[4, 0] = module_angles[1]
+        X_initial[8, 0] = module_angles[2]
+        X_initial[12, 0] = module_angles[3]
+
+    X_initial[18, 0] = theta
+
+    X_initial[19, 0] = velocity[0, 0] + dx
+    X_initial[20, 0] = velocity[1, 0] + dy
+    X_initial[21, 0] = omega
+
+    return X_initial
+
+
+def wrap(fn):
+    evaluated_fn = fn(casadi.SX.sym("X", 25, 1), casadi.SX.sym("U", 8, 1))
+    return lambda X, U: numpy.array(evaluated_fn(X, U))
+
+
+def wrap_module(fn, i):
+    evaluated_fn = fn(i, casadi.SX.sym("X", 25, 1), casadi.SX.sym("U", 8, 1))
+    return lambda X, U: numpy.array(evaluated_fn(X, U))