commit d6e7f69517fd75ab1d7554c7c95ec03da90a460c
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Tue Oct 29 22:29:17 2024 -0700
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Thu Oct 31 23:01:43 2024 -0700
tree 1b8718c40af8cddc8febee56a62140a71a41db4c
parent c5d6d347233edbdeaea567eee64daae97efdd375

Template jacobian.h on Scalar type

This makes it so that we can run numerical Jacobian calculations on
single-precision floats when we want to.

Change-Id: If45ea95366a772883872321c99c14acc8eec7eca
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>

frc971/control_loops/jacobian.h

diff --git a/frc971/control_loops/jacobian.h b/frc971/control_loops/jacobian.h
index 5442ec8..e82c668 100644
--- a/frc971/control_loops/jacobian.h
+++ b/frc971/control_loops/jacobian.h
@@ -5,18 +5,18 @@
 
 namespace frc971::control_loops {
 
-template <int num_states, int num_inputs, typename F>
-::Eigen::Matrix<double, num_states, num_inputs> NumericalJacobian(
-    const F &fn, ::Eigen::Matrix<double, num_inputs, 1> input) {
-  constexpr double kEpsilon = 1e-5;
-  ::Eigen::Matrix<double, num_states, num_inputs> result =
-      ::Eigen::Matrix<double, num_states, num_inputs>::Zero();
+template <int num_states, int num_inputs, typename Scalar, typename F>
+::Eigen::Matrix<Scalar, num_states, num_inputs> NumericalJacobian(
+    const F &fn, ::Eigen::Matrix<Scalar, num_inputs, 1> input) {
+  constexpr Scalar kEpsilon = 1e-5;
+  ::Eigen::Matrix<Scalar, num_states, num_inputs> result =
+      ::Eigen::Matrix<Scalar, num_states, num_inputs>::Zero();
 
   // It's more expensive, but +- epsilon will be more accurate
   for (int i = 0; i < num_inputs; ++i) {
-    ::Eigen::Matrix<double, num_inputs, 1> dX_plus = input;
+    ::Eigen::Matrix<Scalar, num_inputs, 1> dX_plus = input;
     dX_plus(i, 0) += kEpsilon;
-    ::Eigen::Matrix<double, num_inputs, 1> dX_minus = input;
+    ::Eigen::Matrix<Scalar, num_inputs, 1> dX_minus = input;
     dX_minus(i, 0) -= kEpsilon;
     result.col(i) = (fn(dX_plus) - fn(dX_minus)) / (kEpsilon * 2.0);
   }
@@ -24,24 +24,26 @@
 }
 
 // Implements a numerical jacobian with respect to X for f(X, U, ...).
-template <int num_states, int num_u, typename F, typename... Args>
-::Eigen::Matrix<double, num_states, num_states> NumericalJacobianX(
-    const F &fn, ::Eigen::Matrix<double, num_states, 1> X,
-    ::Eigen::Matrix<double, num_u, 1> U, Args &&...args) {
+template <int num_states, int num_u, typename Scalar, typename F,
+          typename... Args>
+::Eigen::Matrix<Scalar, num_states, num_states> NumericalJacobianX(
+    const F &fn, ::Eigen::Matrix<Scalar, num_states, 1> X,
+    ::Eigen::Matrix<Scalar, num_u, 1> U, Args &&...args) {
   return NumericalJacobian<num_states, num_states>(
-      [&](::Eigen::Matrix<double, num_states, 1> X) {
+      [&](::Eigen::Matrix<Scalar, num_states, 1> X) {
         return fn(X, U, args...);
       },
       X);
 }
 
 // Implements a numerical jacobian with respect to U for f(X, U, ...).
-template <int num_states, int num_u, typename F, typename... Args>
-::Eigen::Matrix<double, num_states, num_u> NumericalJacobianU(
-    const F &fn, ::Eigen::Matrix<double, num_states, 1> X,
-    ::Eigen::Matrix<double, num_u, 1> U, Args &&...args) {
+template <int num_states, int num_u, typename Scalar, typename F,
+          typename... Args>
+::Eigen::Matrix<Scalar, num_states, num_u> NumericalJacobianU(
+    const F &fn, ::Eigen::Matrix<Scalar, num_states, 1> X,
+    ::Eigen::Matrix<Scalar, num_u, 1> U, Args &&...args) {
   return NumericalJacobian<num_states, num_u>(
-      [&](::Eigen::Matrix<double, num_u, 1> U) { return fn(X, U, args...); },
+      [&](::Eigen::Matrix<Scalar, num_u, 1> U) { return fn(X, U, args...); },
       U);
 }