commit bd75c482af29d100fe4bd011a23a9144e465e077
author Austin Schuh <austin.linux@gmail.com> Sun Aug 18 00:03:51 2024 -0700
committer Austin Schuh <austin.linux@gmail.com> Sun Aug 18 13:58:43 2024 -0700
tree 84bd29a917fe6c99f124a80cb3a56882d1b18996
parent dd1f87295210c6552f9a0de8c4e53a33c9621219

Fix singularities in atan2 at 0, 0 in swerve physics

The derivative of atan2 is
   -y / (x^2 + y^2), x / (x^2 + y^2)

This explodes at 0, 0.  Be much more gentle at 0, 0 in our calculation
to reduce the rate of change.  This makes the solver converge
*significantly* faster at 0, 0.  We still can't go sideways yet from 0,
0.

Change-Id: If5496714ec1fd36fac76913d208e803257dcd02f
Signed-off-by: Austin Schuh <austin.linux@gmail.com>

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 2e3fe73..0e8b013 100644
--- a/frc971/control_loops/swerve/generate_physics.cc
+++ b/frc971/control_loops/swerve/generate_physics.cc
@@ -412,7 +412,7 @@
     result_py->emplace_back("    sin = casadi.sin");
     result_py->emplace_back("    sign = casadi.sign");
     result_py->emplace_back("    cos = casadi.cos");
-    result_py->emplace_back("    atan2 = casadi.atan2");
+    result_py->emplace_back("    atan2 = soft_atan2");
     result_py->emplace_back("    fmax = casadi.fmax");
     result_py->emplace_back("    fabs = casadi.fabs");
 
@@ -456,7 +456,7 @@
     result_py->emplace_back("    sin = casadi.sin");
     result_py->emplace_back("    sign = casadi.sign");
     result_py->emplace_back("    cos = casadi.cos");
-    result_py->emplace_back("    atan2 = casadi.atan2");
+    result_py->emplace_back("    atan2 = soft_atan2");
     result_py->emplace_back("    fmax = casadi.fmax");
     result_py->emplace_back("    fabs = casadi.fabs");
 
@@ -574,6 +574,14 @@
     // result_py.emplace_back("        [X[STATE_MOMENT, 0]],");
     result_py.emplace_back("    ])");
     result_py.emplace_back("");
+    constexpr double kLogGain = 1.0 / 0.05;
+    result_py.emplace_back("def soft_atan2(y, x):");
+    result_py.emplace_back("    return casadi.arctan2(");
+    result_py.emplace_back("        y,");
+    result_py.emplace_back("        casadi.logsumexp(casadi.SX(numpy.array(");
+    result_py.emplace_back(absl::Substitute(
+        "            [1.0, casadi.fabs(x) * $0.0]))) / $0.0)", kLogGain));
+    result_py.emplace_back("");
 
     result_py.emplace_back("# Returns the derivative of our state vector");
     result_py.emplace_back("# [thetas0, thetad0, omegas0, omegad0,");
@@ -839,15 +847,17 @@
     // Velocity of the contact patch in field coordinates
     DenseMatrix temp_matrix = DenseMatrix(2, 1);
     DenseMatrix temp_matrix2 = DenseMatrix(2, 1);
+    DenseMatrix temp_matrix3 = DenseMatrix(2, 1);
     result.contact_patch_velocity = DenseMatrix(2, 1);
 
     mul_dense_dense(R(theta_), result.mounting_location, temp_matrix);
     add_dense_dense(angle_cross(temp_matrix, omega_), robot_velocity,
                     temp_matrix2);
     mul_dense_dense(R(add(theta_, result.thetas)),
-                    DenseMatrix(2, 1, {neg(caster_), integer(0)}), temp_matrix);
+                    DenseMatrix(2, 1, {neg(caster_), integer(0)}),
+                    temp_matrix3);
     add_dense_dense(temp_matrix2,
-                    angle_cross(temp_matrix, add(omega_, result.omegas)),
+                    angle_cross(temp_matrix3, add(omega_, result.omegas)),
                     result.contact_patch_velocity);
 
     VLOG(1);