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>
diff --git a/frc971/control_loops/double_jointed_arm/trajectory.cc b/frc971/control_loops/double_jointed_arm/trajectory.cc
index 0eaf67e..a04d6cc 100644
--- a/frc971/control_loops/double_jointed_arm/trajectory.cc
+++ b/frc971/control_loops/double_jointed_arm/trajectory.cc
@@ -391,14 +391,14 @@
const auto x_blocked = X.block<4, 1>(0, 0);
const ::Eigen::Matrix<double, 4, 4> final_A =
- ::frc971::control_loops::NumericalJacobianX<4, 2>(
+ ::frc971::control_loops::NumericalJacobianX<4, 2, double>(
[this](const auto &x_blocked, const auto &U, double dt) {
return this->dynamics_->UnboundedDiscreteDynamics(x_blocked, U, dt);
},
x_blocked, U, 0.00505);
const ::Eigen::Matrix<double, 4, 2> final_B =
- ::frc971::control_loops::NumericalJacobianU<4, 2>(
+ ::frc971::control_loops::NumericalJacobianU<4, 2, double>(
[this](const auto &x_blocked, const auto &U, double dt) {
return this->dynamics_->UnboundedDiscreteDynamics(x_blocked, U, dt);
},
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);
}
diff --git a/frc971/control_loops/jacobian_test.cc b/frc971/control_loops/jacobian_test.cc
index 5606004..92a15a3 100644
--- a/frc971/control_loops/jacobian_test.cc
+++ b/frc971/control_loops/jacobian_test.cc
@@ -19,17 +19,17 @@
// Test that we can recover A from AxBufn pretty accurately.
TEST(RungeKuttaTest, Ax) {
- ::Eigen::Matrix<double, 4, 4> NewA =
- NumericalJacobianX<4, 2>(AxBufn, ::Eigen::Matrix<double, 4, 1>::Zero(),
- ::Eigen::Matrix<double, 2, 1>::Zero());
+ ::Eigen::Matrix<double, 4, 4> NewA = NumericalJacobianX<4, 2, double>(
+ AxBufn, ::Eigen::Matrix<double, 4, 1>::Zero(),
+ ::Eigen::Matrix<double, 2, 1>::Zero());
EXPECT_TRUE(NewA.isApprox(A));
}
// Test that we can recover B from AxBufn pretty accurately.
TEST(RungeKuttaTest, Bu) {
- ::Eigen::Matrix<double, 4, 2> NewB =
- NumericalJacobianU<4, 2>(AxBufn, ::Eigen::Matrix<double, 4, 1>::Zero(),
- ::Eigen::Matrix<double, 2, 1>::Zero());
+ ::Eigen::Matrix<double, 4, 2> NewB = NumericalJacobianU<4, 2, double>(
+ AxBufn, ::Eigen::Matrix<double, 4, 1>::Zero(),
+ ::Eigen::Matrix<double, 2, 1>::Zero());
EXPECT_TRUE(NewB.isApprox(B));
}
diff --git a/y2023/control_loops/superstructure/arm/arm_design.cc b/y2023/control_loops/superstructure/arm/arm_design.cc
index 07439d6..4f7c629 100644
--- a/y2023/control_loops/superstructure/arm/arm_design.cc
+++ b/y2023/control_loops/superstructure/arm/arm_design.cc
@@ -106,13 +106,13 @@
const auto x_blocked = X.block<4, 1>(0, 0);
const ::Eigen::Matrix<double, 4, 4> final_A =
- ::frc971::control_loops::NumericalJacobianX<4, 2>(
+ ::frc971::control_loops::NumericalJacobianX<4, 2, double>(
[dynamics](const auto &x_blocked, const auto &U, double kDt) {
return dynamics.UnboundedDiscreteDynamics(x_blocked, U, kDt);
},
x_blocked, U, 0.00505);
const ::Eigen::Matrix<double, 4, 2> final_B =
- ::frc971::control_loops::NumericalJacobianU<4, 2>(
+ ::frc971::control_loops::NumericalJacobianU<4, 2, double>(
[dynamics](const auto &x_blocked, const auto &U, double kDt) {
return dynamics.UnboundedDiscreteDynamics(x_blocked, U, kDt);
},
diff --git a/y2023/control_loops/superstructure/arm/trajectory.cc b/y2023/control_loops/superstructure/arm/trajectory.cc
index f7ae5f9..4ad4424 100644
--- a/y2023/control_loops/superstructure/arm/trajectory.cc
+++ b/y2023/control_loops/superstructure/arm/trajectory.cc
@@ -736,14 +736,14 @@
const auto x_blocked = arm_X.block<4, 1>(0, 0);
const ::Eigen::Matrix<double, 4, 4> final_A =
- ::frc971::control_loops::NumericalJacobianX<4, 2>(
+ ::frc971::control_loops::NumericalJacobianX<4, 2, double>(
[this](const auto &x_blocked, const auto &U, double dt) {
return this->dynamics_->UnboundedDiscreteDynamics(x_blocked, U, dt);
},
x_blocked, arm_U, 0.00505);
const ::Eigen::Matrix<double, 4, 2> final_B =
- ::frc971::control_loops::NumericalJacobianU<4, 2>(
+ ::frc971::control_loops::NumericalJacobianU<4, 2, double>(
[this](const auto &x_blocked, const auto &U, double dt) {
return this->dynamics_->UnboundedDiscreteDynamics(x_blocked, U, dt);
},