| Austin Schuh | 0378513 | 2018-02-19 18:29:06 -0800 | [diff] [blame] | 1 | #ifndef FRC971_CONTROL_LOOPS_DLQR_H_ |
| 2 | #define FRC971_CONTROL_LOOPS_DLQR_H_ |
| 3 | |
| 4 | #include <Eigen/Dense> |
| 5 | |
| 6 | namespace frc971 { |
| 7 | namespace controls { |
| 8 | |
| 9 | extern "C" { |
| 10 | // Forward declaration for slicot fortran library. |
| 11 | void sb02od_(char *DICO, char *JOBB, char *FACT, char *UPLO, char *JOBL, |
| 12 | char *SORT, long *N, long *M, long *P, double *A, long *LDA, |
| 13 | double *B, long *LDB, double *Q, long *LDQ, double *R, long *LDR, |
| 14 | double *L, long *LDL, double *RCOND, double *X, long *LDX, |
| 15 | double *ALFAR, double *ALFAI, double *BETA, double *S, long *LDS, |
| 16 | double *T, long *LDT, double *U, long *LDU, double *TOL, |
| 17 | long *IWORK, double *DWORK, long *LDWORK, long *BWORK, long *INFO); |
| 18 | } |
| 19 | |
| 20 | template <int kN, int kM> |
| 21 | void dlqr(::Eigen::Matrix<double, kN, kN> A, ::Eigen::Matrix<double, kN, kM> B, |
| 22 | ::Eigen::Matrix<double, kN, kN> Q, ::Eigen::Matrix<double, kM, kM> R, |
| 23 | ::Eigen::Matrix<double, kM, kN> *K, |
| 24 | ::Eigen::Matrix<double, kN, kN> *S) { |
| 25 | *K = ::Eigen::Matrix<double, kM, kN>::Zero(); |
| 26 | *S = ::Eigen::Matrix<double, kN, kN>::Zero(); |
| 27 | // Discrete (not continuous) |
| 28 | char DICO = 'D'; |
| 29 | // B and R are provided instead of G. |
| 30 | char JOBB = 'B'; |
| 31 | // Not factored, Q and R are provide. |
| 32 | char FACT = 'N'; |
| 33 | // Store the upper triangle of Q and R. |
| 34 | char UPLO = 'U'; |
| 35 | // L is zero. |
| 36 | char JOBL = 'Z'; |
| 37 | // Stable eigenvalues first in the sort order |
| 38 | char SORT = 'S'; |
| 39 | |
| 40 | long N = 4; |
| 41 | long M = 2; |
| 42 | // Not needed since FACT = N |
| 43 | long P = 0; |
| 44 | |
| 45 | long LDL = 1; |
| 46 | |
| 47 | double RCOND = 0.0; |
| 48 | ::Eigen::Matrix<double, kN, kN> X = ::Eigen::Matrix<double, kN, kN>::Zero(); |
| 49 | |
| 50 | double ALFAR[kN * 2]; |
| 51 | double ALFAI[kN * 2]; |
| 52 | double BETA[kN * 2]; |
| 53 | memset(ALFAR, 0, kN * 2); |
| 54 | memset(ALFAI, 0, kN * 2); |
| 55 | memset(BETA, 0, kN * 2); |
| 56 | |
| 57 | long LDS = 2 * kN + kM; |
| 58 | ::Eigen::Matrix<double, 2 * kN + kM, 2 *kN + kM> S_schur = |
| 59 | ::Eigen::Matrix<double, 2 * kN + kM, 2 * kN + kM>::Zero(); |
| 60 | |
| 61 | ::Eigen::Matrix<double, 2 * kN + kM, 2 *kN> T = |
| 62 | ::Eigen::Matrix<double, 2 * kN + kM, 2 * kN>::Zero(); |
| 63 | long LDT = 2 * kN + kM; |
| 64 | |
| 65 | ::Eigen::Matrix<double, 2 * kN, 2 *kN> U = |
| 66 | ::Eigen::Matrix<double, 2 * kN, 2 * kN>::Zero(); |
| 67 | long LDU = 2 * kN; |
| 68 | |
| 69 | double TOL = 0.0; |
| 70 | |
| 71 | long IWORK[2 * kN > kM ? 2 * kN : kM]; |
| 72 | memset(IWORK, 0, 2 * kN > kM ? 2 * kN : kM); |
| 73 | |
| 74 | long LDWORK = 16 * kN + 3 * kM + 16; |
| 75 | |
| 76 | double DWORK[LDWORK]; |
| 77 | memset(DWORK, 0, LDWORK); |
| 78 | |
| 79 | long INFO = 0; |
| 80 | |
| 81 | long BWORK[2 * kN]; |
| 82 | memset(BWORK, 0, 2 * kN); |
| 83 | |
| 84 | // TODO(austin): I can't tell if anything here is transposed... |
| 85 | sb02od_(&DICO, &JOBB, &FACT, &UPLO, &JOBL, &SORT, &N, &M, &P, A.data(), &N, |
| 86 | B.data(), &N, Q.data(), &N, R.data(), &M, nullptr, &LDL, &RCOND, |
| 87 | X.data(), &N, ALFAR, ALFAI, BETA, S_schur.data(), &LDS, T.data(), |
| 88 | &LDT, U.data(), &LDU, &TOL, IWORK, DWORK, &LDWORK, BWORK, &INFO); |
| 89 | //::std::cout << "slycot result: " << INFO << ::std::endl; |
| 90 | |
| 91 | *K = (R + B.transpose() * X * B).inverse() * B.transpose() * X * A; |
| 92 | if (S != nullptr) { |
| 93 | *S = X; |
| 94 | } |
| 95 | } |
| 96 | |
| 97 | } // namespace controls |
| 98 | } // namespace frc971 |
| 99 | |
| 100 | #endif // FRC971_CONTROL_LOOPS_DLQR_H_ |