commit cb091717cd465dfca867172266b68108c74b8251
author Austin Schuh <austin.linux@gmail.com> Wed Feb 21 20:01:55 2018 -0800
committer Austin Schuh <austin.linux@gmail.com> Sat Mar 03 00:00:56 2018 -0800
tree c0f957acb0e86cf94e4f2b4089963e767922e371
parent f853bded2f6c029448a01389fd481aeec43a0db1

Arm works

Added gravity and calibrated it.  Terifying...

Change-Id: I70babb1cd3b83ddd7a81f06fb2a75cefd55bcdb8

frc971/control_loops/dlqr.h

diff --git a/frc971/control_loops/dlqr.h b/frc971/control_loops/dlqr.h
index d52a9dd..98dccb2 100644
--- a/frc971/control_loops/dlqr.h
+++ b/frc971/control_loops/dlqr.h
@@ -6,6 +6,25 @@
 namespace frc971 {
 namespace controls {
 
+template <int num_states, int num_inputs>
+int Controllability(const ::Eigen::Matrix<double, num_states, num_states> &A,
+                    const ::Eigen::Matrix<double, num_states, num_inputs> &B) {
+    Eigen::Matrix<double, num_states, num_states * num_inputs> controllability;
+    controllability.block(0, 0, num_states, num_inputs) = B;
+
+    for (size_t i = 1; i < num_states; i++) {
+      controllability.block(0, i * num_inputs, num_states, num_inputs) =
+          A *
+          controllability.block(0, (i - 1) * num_inputs, num_states,
+                                num_inputs);
+    }
+
+    return Eigen::FullPivLU<
+               Eigen::Matrix<double, num_states, num_states * num_inputs>>(
+               controllability)
+        .rank();
+}
+
 extern "C" {
 // Forward declaration for slicot fortran library.
 void sb02od_(char *DICO, char *JOBB, char *FACT, char *UPLO, char *JOBL,
@@ -17,8 +36,9 @@
              long *IWORK, double *DWORK, long *LDWORK, long *BWORK, long *INFO);
 }
 
+// Computes the optimal LQR controller K for the provided system and costs.
 template <int kN, int kM>
-void dlqr(::Eigen::Matrix<double, kN, kN> A, ::Eigen::Matrix<double, kN, kM> B,
+int dlqr(::Eigen::Matrix<double, kN, kN> A, ::Eigen::Matrix<double, kN, kM> B,
           ::Eigen::Matrix<double, kN, kN> Q, ::Eigen::Matrix<double, kM, kM> R,
           ::Eigen::Matrix<double, kM, kN> *K,
           ::Eigen::Matrix<double, kN, kN> *S) {
@@ -86,12 +106,13 @@
           B.data(), &N, Q.data(), &N, R.data(), &M, nullptr, &LDL, &RCOND,
           X.data(), &N, ALFAR, ALFAI, BETA, S_schur.data(), &LDS, T.data(),
           &LDT, U.data(), &LDU, &TOL, IWORK, DWORK, &LDWORK, BWORK, &INFO);
-  //::std::cout << "slycot result: " << INFO << ::std::endl;
 
   *K = (R + B.transpose() * X * B).inverse() * B.transpose() * X * A;
   if (S != nullptr) {
     *S = X;
   }
+
+  return INFO;
 }
 
 }  // namespace controls

frc971/control_loops/jacobian.h

diff --git a/frc971/control_loops/jacobian.h b/frc971/control_loops/jacobian.h
index fe3c157..8b0d4a5 100644
--- a/frc971/control_loops/jacobian.h
+++ b/frc971/control_loops/jacobian.h
@@ -9,7 +9,7 @@
 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;
+  constexpr double kEpsilon = 1e-5;
   ::Eigen::Matrix<double, num_states, num_inputs> result =
       ::Eigen::Matrix<double, num_states, num_inputs>::Zero();