Add numerical jacobian library.
Change-Id: I1f54172a561f00c9cca2b218f16a1f1dba776d5c
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index eccbb14..01dc25e 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -1,204 +1,228 @@
-package(default_visibility = ['//visibility:public'])
+package(default_visibility = ["//visibility:public"])
-load('//aos/build:queues.bzl', 'queue_library')
+load("//aos/build:queues.bzl", "queue_library")
cc_library(
- name = 'team_number_test_environment',
- srcs = [
- 'team_number_test_environment.cc',
- ],
- hdrs = [
- 'team_number_test_environment.h',
- ],
- deps = [
- '//aos/common/network:team_number',
- '//aos/testing:googletest',
- ],
- testonly = True,
+ name = "team_number_test_environment",
+ testonly = True,
+ srcs = [
+ "team_number_test_environment.cc",
+ ],
+ hdrs = [
+ "team_number_test_environment.h",
+ ],
+ deps = [
+ "//aos/common/network:team_number",
+ "//aos/testing:googletest",
+ ],
)
cc_test(
- name = 'hybrid_state_feedback_loop_test',
- srcs = [
- 'hybrid_state_feedback_loop_test.cc',
- ],
- deps = [
- ':hybrid_state_feedback_loop',
- '//aos/testing:googletest',
- ],
+ name = "hybrid_state_feedback_loop_test",
+ srcs = [
+ "hybrid_state_feedback_loop_test.cc",
+ ],
+ deps = [
+ ":hybrid_state_feedback_loop",
+ "//aos/testing:googletest",
+ ],
)
cc_library(
- name = 'hall_effect_tracker',
- hdrs = [
- 'hall_effect_tracker.h',
- ],
- deps = [
- ':queues',
- ],
+ name = "hall_effect_tracker",
+ hdrs = [
+ "hall_effect_tracker.h",
+ ],
+ deps = [
+ ":queues",
+ ],
)
queue_library(
- name = 'queues',
- srcs = [
- 'control_loops.q',
- ],
+ name = "queues",
+ srcs = [
+ "control_loops.q",
+ ],
)
cc_test(
- name = 'position_sensor_sim_test',
- srcs = [
- 'position_sensor_sim_test.cc',
- ],
- deps = [
- ':queues',
- ':position_sensor_sim',
- '//aos/testing:googletest',
- '//aos/common/logging',
- ],
+ name = "position_sensor_sim_test",
+ srcs = [
+ "position_sensor_sim_test.cc",
+ ],
+ deps = [
+ ":position_sensor_sim",
+ ":queues",
+ "//aos/common/logging",
+ "//aos/testing:googletest",
+ ],
)
cc_library(
- name = 'position_sensor_sim',
- testonly = True,
- srcs = [
- 'position_sensor_sim.cc',
- ],
- hdrs = [
- 'position_sensor_sim.h',
- ],
- deps = [
- ':queues',
- ':gaussian_noise',
- '//debian:libm',
- '//aos/testing:random_seed',
- ],
+ name = "position_sensor_sim",
+ testonly = True,
+ srcs = [
+ "position_sensor_sim.cc",
+ ],
+ hdrs = [
+ "position_sensor_sim.h",
+ ],
+ deps = [
+ ":gaussian_noise",
+ ":queues",
+ "//aos/testing:random_seed",
+ "//debian:libm",
+ ],
)
cc_library(
- name = 'gaussian_noise',
- srcs = [
- 'gaussian_noise.cc',
- ],
- hdrs = [
- 'gaussian_noise.h',
- ],
- deps = [
- '//debian:libm',
- ],
+ name = "gaussian_noise",
+ srcs = [
+ "gaussian_noise.cc",
+ ],
+ hdrs = [
+ "gaussian_noise.h",
+ ],
+ deps = [
+ "//debian:libm",
+ ],
)
cc_library(
- name = 'coerce_goal',
- srcs = [
- 'coerce_goal.cc',
- ],
- hdrs = [
- 'coerce_goal.h',
- ],
- deps = [
- '//third_party/eigen',
- '//aos/common/controls:polytope',
- '//debian:libm',
- ],
+ name = "coerce_goal",
+ srcs = [
+ "coerce_goal.cc",
+ ],
+ hdrs = [
+ "coerce_goal.h",
+ ],
+ deps = [
+ "//aos/common/controls:polytope",
+ "//debian:libm",
+ "//third_party/eigen",
+ ],
)
# TODO(austin): Select isn't working right. We should be able to remove
# logging conditionally with select and have CPU constraints work correctly.
cc_library(
- name = 'state_feedback_loop_uc',
- hdrs = [
- 'state_feedback_loop.h',
- ],
- deps = [
- '//aos/common:macros',
- '//third_party/eigen',
- ],
- restricted_to = ['//tools:cortex-m4f'],
-)
-cc_library(
- name = 'state_feedback_loop',
- hdrs = [
- 'state_feedback_loop.h',
- ],
- deps = [
- '//aos/common/logging',
- '//aos/common:macros',
- '//third_party/eigen',
- ],
+ name = "state_feedback_loop_uc",
+ hdrs = [
+ "state_feedback_loop.h",
+ ],
+ restricted_to = ["//tools:cortex-m4f"],
+ deps = [
+ "//aos/common:macros",
+ "//third_party/eigen",
+ ],
)
cc_library(
- name = 'hybrid_state_feedback_loop',
- hdrs = [
- 'hybrid_state_feedback_loop.h',
- ],
- deps = [
- ':state_feedback_loop',
- '//aos/common/controls:control_loop',
- '//aos/common/logging',
- '//aos/common:macros',
- '//third_party/eigen',
- ],
+ name = "state_feedback_loop",
+ hdrs = [
+ "state_feedback_loop.h",
+ ],
+ deps = [
+ "//aos/common:macros",
+ "//aos/common/logging",
+ "//third_party/eigen",
+ ],
)
cc_library(
- name = 'simple_capped_state_feedback_loop',
- hdrs = [
- 'simple_capped_state_feedback_loop.h',
- ],
- deps = [
- '//third_party/eigen',
- ':state_feedback_loop',
- ],
+ name = "hybrid_state_feedback_loop",
+ hdrs = [
+ "hybrid_state_feedback_loop.h",
+ ],
+ deps = [
+ ":state_feedback_loop",
+ "//aos/common:macros",
+ "//aos/common/controls:control_loop",
+ "//aos/common/logging",
+ "//third_party/eigen",
+ ],
)
cc_library(
- name = 'runge_kutta',
- hdrs = [
- 'runge_kutta.h',
- ],
- deps = [
- '//third_party/eigen',
- ],
+ name = "simple_capped_state_feedback_loop",
+ hdrs = [
+ "simple_capped_state_feedback_loop.h",
+ ],
+ deps = [
+ ":state_feedback_loop",
+ "//third_party/eigen",
+ ],
+)
+
+cc_library(
+ name = "runge_kutta",
+ hdrs = [
+ "runge_kutta.h",
+ ],
+ deps = [
+ "//third_party/eigen",
+ ],
)
cc_test(
- name = 'runge_kutta_test',
- srcs = [
- 'runge_kutta_test.cc',
- ],
- deps = [
- ':runge_kutta',
- '//aos/testing:googletest',
- '//third_party/eigen',
- ],
+ name = "runge_kutta_test",
+ srcs = [
+ "runge_kutta_test.cc",
+ ],
+ deps = [
+ ":runge_kutta",
+ "//aos/testing:googletest",
+ "//third_party/eigen",
+ ],
)
queue_library(
- name = 'profiled_subsystem_queue',
- srcs = [
- 'profiled_subsystem.q',
- ],
- deps = [
- ':queues',
- ],
+ name = "profiled_subsystem_queue",
+ srcs = [
+ "profiled_subsystem.q",
+ ],
+ deps = [
+ ":queues",
+ ],
)
cc_library(
- name = 'profiled_subsystem',
- srcs = [
- 'profiled_subsystem.cc',
- ],
- hdrs = [
- 'profiled_subsystem.h',
- ],
- deps = [
- ':profiled_subsystem_queue',
- ':simple_capped_state_feedback_loop',
- ':state_feedback_loop',
- '//aos/common/controls:control_loop',
- '//aos/common/util:trapezoid_profile',
- '//frc971/zeroing:zeroing',
- ],
+ name = "profiled_subsystem",
+ srcs = [
+ "profiled_subsystem.cc",
+ ],
+ hdrs = [
+ "profiled_subsystem.h",
+ ],
+ deps = [
+ ":profiled_subsystem_queue",
+ ":simple_capped_state_feedback_loop",
+ ":state_feedback_loop",
+ "//aos/common/controls:control_loop",
+ "//aos/common/util:trapezoid_profile",
+ "//frc971/zeroing",
+ ],
)
+
+cc_library(
+ name = "jacobian",
+ hdrs = [
+ "jacobian.h",
+ ],
+ deps = [
+ "//third_party/eigen",
+ ],
+)
+
+cc_test(
+ name = "jacobian_test",
+ srcs = [
+ "jacobian_test.cc",
+ ],
+ deps = [
+ ":jacobian",
+ "//aos/testing:googletest",
+ "//third_party/eigen",
+ ],
+)
+
diff --git a/frc971/control_loops/jacobian.h b/frc971/control_loops/jacobian.h
new file mode 100644
index 0000000..fe3c157
--- /dev/null
+++ b/frc971/control_loops/jacobian.h
@@ -0,0 +1,52 @@
+#ifndef FRC971_CONTROL_LOOPS_JACOBIAN_H_
+#define FRC971_CONTROL_LOOPS_JACOBIAN_H_
+
+#include <Eigen/Dense>
+
+namespace frc971 {
+namespace 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-4;
+ ::Eigen::Matrix<double, num_states, num_inputs> result =
+ ::Eigen::Matrix<double, 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;
+ dX_plus(i, 0) += kEpsilon;
+ ::Eigen::Matrix<double, num_inputs, 1> dX_minus = input;
+ dX_minus(i, 0) -= kEpsilon;
+ result.col(i) = (fn(dX_plus) - fn(dX_minus)) / (kEpsilon * 2.0);
+ }
+ return result;
+}
+
+// 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) {
+ return NumericalJacobian<num_states, num_states>(
+ [&](::Eigen::Matrix<double, 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) {
+ return NumericalJacobian<num_states, num_u>(
+ [&](::Eigen::Matrix<double, num_u, 1> U) { return fn(X, U, args...); },
+ U);
+}
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_JACOBIAN_H_
diff --git a/frc971/control_loops/jacobian_test.cc b/frc971/control_loops/jacobian_test.cc
new file mode 100644
index 0000000..6046c37
--- /dev/null
+++ b/frc971/control_loops/jacobian_test.cc
@@ -0,0 +1,40 @@
+#include "frc971/control_loops/jacobian.h"
+
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+::Eigen::Matrix<double, 4, 4> A = (::Eigen::Matrix<double, 4, 4>() << 1, 2, 4,
+ 1, 5, 2, 3, 4, 5, 1, 3, 2, 1, 1, 3,
+ 7).finished();
+
+::Eigen::Matrix<double, 4, 2> B =
+ (::Eigen::Matrix<double, 4, 2>() << 1, 1, 2, 1, 3, 2, 3, 7).finished();
+
+// Function to recover A and B from.
+::Eigen::Matrix<double, 4, 1> AxBufn(const ::Eigen::Matrix<double, 4, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U) {
+ return A * X + B * U;
+}
+
+// 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());
+ 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());
+ EXPECT_TRUE(NewB.isApprox(B));
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971