Add test comparing C++ and Python dynamics codegen
This also adds C++ enums defining the states for the physics (note that
it needs to use regular C enums to not require a static_cast to
convert to integers).
It only passes in trivial scenarios currently....
Change-Id: I97e051c896500715c1252b17723f049981f99f1d
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/swerve/physics_test.py b/frc971/control_loops/swerve/physics_test.py
index 6e7ddf4..31b96c9 100644
--- a/frc971/control_loops/swerve/physics_test.py
+++ b/frc971/control_loops/swerve/physics_test.py
@@ -19,6 +19,7 @@
from frc971.control_loops.swerve import nocaster_dynamics
from frc971.control_loops.swerve import physics_test_utils as utils
from frc971.control_loops.swerve import jax_dynamics
+from frc971.control_loops.swerve.cpp_dynamics import swerve_dynamics as cpp_dynamics
class TestSwervePhysics(unittest.TestCase):
@@ -705,6 +706,29 @@
self.assertAlmostEquals(Xdot[dynamics.STATE_OMEGA, 0],
Xdot_rot[dynamics.STATE_OMEGA, 0])
+ def test_cpp_consistency(self):
+ """Tests that the C++ physics are consistent with the Python physics."""
+ # TODO(james): Currently the physics only match at X = 0 and U = 0.
+ # Fix this.
+ # Maybe due to different atan2 implementations?
+ # TODO(james): Fold this into the general comparisons for JAX versus
+ # casadi once the physics actually match.
+ for current in [0]:
+ print(f"Current: {current}")
+ steer_I = numpy.zeros((8, 1)) + current
+ for state_values in [0.0]:
+ print(f"States all set to: {state_values}")
+ X = numpy.zeros((dynamics.NUM_STATES, 1)) + state_values
+ Xdot_py = self.swerve_full_dynamics(X,
+ steer_I,
+ skip_compare=True)
+ Xdot_cpp = numpy.array(
+ cpp_dynamics(X.flatten().tolist(),
+ steer_I.flatten().tolist())).reshape((25, 1))
+ for index in range(dynamics.NUM_STATES):
+ self.assertAlmostEqual(Xdot_py[index, 0], Xdot_cpp[index,
+ 0])
+
if __name__ == "__main__":
unittest.main()