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