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>
diff --git a/frc971/control_loops/swerve/physics_test.py b/frc971/control_loops/swerve/physics_test.py
index 2ce82ef..9aa6457 100644
--- a/frc971/control_loops/swerve/physics_test.py
+++ b/frc971/control_loops/swerve/physics_test.py
@@ -3,6 +3,7 @@
import numpy
import sys, os
import casadi
+import scipy
from numpy.testing import assert_array_equal, assert_array_almost_equal
import unittest
@@ -173,17 +174,23 @@
for theta in [0.0, 0.6, -0.4]:
module_angle = numpy.pi * wrap + theta
- # We have redefined the angle to be the sin of the angle.
+ # We have redefined the angle to be the softened 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](
utils.state_vector(velocity=velocity,
- module_angle=numpy.pi * wrap +
- theta),
+ module_angle=module_angle),
self.I,
)[0, 0]
- expected = numpy.sin(numpy.pi * wrap + theta)
+ # Compute out the expected value directly to confirm we got it right.
+ loggain = 20.0
+ vy = 1.5 * numpy.sin(-module_angle)
+ vx = 1.5 * numpy.cos(-module_angle)
+ expected = numpy.sin(-numpy.arctan2(
+ vy,
+ scipy.special.logsumexp([1.0, abs(vx) * loggain]) /
+ loggain))
self.assertAlmostEqual(
expected,