commit ba91eceb2c68fd85ba27f151620111dd33e9000b
author Austin Schuh <austin.linux@gmail.com> Wed Sep 04 22:37:36 2024 -0700
committer Gerrit Code Review <gerrit@software.frc971.org> Wed Sep 04 22:37:36 2024 -0700
tree 04ff2a19b91553efaf2d54d8bf0c29bada92c5d8
parent c37d217ee311124b75518c2dc6cf567b798a3ddb
parent 98fbbe87fb7b71f72e7401c72285ab865805facc

Merge "Compensate steer coupling torque from velocity cost" into main

frc971/control_loops/swerve/casadi_velocity_mpc.py

diff --git a/frc971/control_loops/swerve/casadi_velocity_mpc.py b/frc971/control_loops/swerve/casadi_velocity_mpc.py
index 8b97933..239f49a 100644
--- a/frc971/control_loops/swerve/casadi_velocity_mpc.py
+++ b/frc971/control_loops/swerve/casadi_velocity_mpc.py
@@ -233,10 +233,13 @@
 
         # TODO(austin): Don't penalize torque steering current.
         for i in range(4):
+            Is = U[2 * i + 0]
+            Id = U[2 * i + 1]
             # Steer
-            J += U[2 * i + 0] * U[2 * i + 0] / 100000.0
+            J += ((Is + dynamics.STEER_CURRENT_COUPLING_FACTOR * Id)**
+                  2.0) / 100000.0
             # Drive
-            J += U[2 * i + 1] * U[2 * i + 1] / 1000.0
+            J += Id * Id / 1000.0
 
         return casadi.Function("Jn", [X, U, R], [J])
 

frc971/control_loops/swerve/generate_physics.cc

diff --git a/frc971/control_loops/swerve/generate_physics.cc b/frc971/control_loops/swerve/generate_physics.cc
index e223b74..3c4eaf6 100644
--- a/frc971/control_loops/swerve/generate_physics.cc
+++ b/frc971/control_loops/swerve/generate_physics.cc
@@ -617,6 +617,16 @@
                                               sub(integer(1), div(rb1_, rp_)))),
                             div(rw_, rb2_))))))))));
     result_py.emplace_back("");
+    result_py.emplace_back("# Is = STEER_CURRENT_COUPLING_FACTOR * Id");
+    result_py.emplace_back(absl::Substitute(
+        "STEER_CURRENT_COUPLING_FACTOR = $0",
+        ccode(*(neg(
+            mul(div(Gs_, Kts_),
+                mul(div(Ktd_, mul(Gd_, rw_)),
+                    neg(mul(add(neg(wb_), mul(add(rs_, rp_),
+                                              sub(integer(1), div(rb1_, rp_)))),
+                            div(rw_, rb2_))))))))));
+    result_py.emplace_back("");
 
     result_py.emplace_back("# Returns the derivative of our state vector");
     result_py.emplace_back("# [thetas0, thetad0, omegas0, omegad0,");

frc971/control_loops/swerve/physics_test.py

diff --git a/frc971/control_loops/swerve/physics_test.py b/frc971/control_loops/swerve/physics_test.py
index 9aa6457..0a3fdb6 100644
--- a/frc971/control_loops/swerve/physics_test.py
+++ b/frc971/control_loops/swerve/physics_test.py
@@ -582,6 +582,23 @@
             self.assertLess(xdot[dynamics.STATE_OMEGAD2, 0], -100.0)
             self.assertLess(xdot[dynamics.STATE_OMEGAD3, 0], -100.0)
 
+    def test_steer_coupling(self):
+        """Tests that the steer coupling factor cancels out steer coupling torque."""
+        steer_I = numpy.array(
+            [[dynamics.STEER_CURRENT_COUPLING_FACTOR * 10.0], [10.0]] * 4)
+
+        X = utils.state_vector(
+            velocity=numpy.array([[0.0], [0.0]]),
+            omega=0.0,
+        )
+        X_velocity = self.to_velocity_state(X)
+        Xdot = self.velocity_swerve_physics(X_velocity, steer_I)
+
+        self.assertAlmostEqual(Xdot[dynamics.VELOCITY_STATE_OMEGAS0, 0], 0.0)
+        self.assertAlmostEqual(Xdot[dynamics.VELOCITY_STATE_OMEGAS1, 0], 0.0)
+        self.assertAlmostEqual(Xdot[dynamics.VELOCITY_STATE_OMEGAS2, 0], 0.0)
+        self.assertAlmostEqual(Xdot[dynamics.VELOCITY_STATE_OMEGAS3, 0], 0.0)
+
 
 if __name__ == "__main__":
     unittest.main()