Added dlqr pole placement library.
This is a wrapper around slicot. Some day, we'll replace it... The
code involved is mildly terrifying
Change-Id: I6e09f7f84d9e85bc12d446e38b0014e3d4df6e6f
diff --git a/debian/clapack.BUILD b/debian/clapack.BUILD
index 528b6a7..0acad74 100644
--- a/debian/clapack.BUILD
+++ b/debian/clapack.BUILD
@@ -255,7 +255,7 @@
'F2CLIBS/libf2c/fmt.c',
'F2CLIBS/libf2c/rdfmt.c',
'F2CLIBS/libf2c/wrtfmt.c',
- 'F2CLIBS/libf2c/ctype.c',
+ #'F2CLIBS/libf2c/ctype.c',
'F2CLIBS/libf2c/wref.c',
'F2CLIBS/libf2c/fmtlib.c',
'F2CLIBS/libf2c/lread.c',
@@ -269,7 +269,8 @@
hdrs = glob([
'INCLUDE/*.h',
'F2CLIBS/libf2c/*.h',
- ]),
+ ],
+ exclude=['F2CLIBS/libf2c/ctype.h']),
includes = [
'INCLUDE',
'F2CLIBS/libf2c',
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 01dc25e..360097d 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -226,3 +226,14 @@
],
)
+cc_library(
+ name = "dlqr",
+ hdrs = [
+ "dlqr.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//third_party/eigen",
+ "@slycot_repo//:slicot",
+ ],
+)
diff --git a/frc971/control_loops/dlqr.h b/frc971/control_loops/dlqr.h
new file mode 100644
index 0000000..d52a9dd
--- /dev/null
+++ b/frc971/control_loops/dlqr.h
@@ -0,0 +1,100 @@
+#ifndef FRC971_CONTROL_LOOPS_DLQR_H_
+#define FRC971_CONTROL_LOOPS_DLQR_H_
+
+#include <Eigen/Dense>
+
+namespace frc971 {
+namespace controls {
+
+extern "C" {
+// Forward declaration for slicot fortran library.
+void sb02od_(char *DICO, char *JOBB, char *FACT, char *UPLO, char *JOBL,
+ char *SORT, long *N, long *M, long *P, double *A, long *LDA,
+ double *B, long *LDB, double *Q, long *LDQ, double *R, long *LDR,
+ double *L, long *LDL, double *RCOND, double *X, long *LDX,
+ double *ALFAR, double *ALFAI, double *BETA, double *S, long *LDS,
+ double *T, long *LDT, double *U, long *LDU, double *TOL,
+ long *IWORK, double *DWORK, long *LDWORK, long *BWORK, long *INFO);
+}
+
+template <int kN, int kM>
+void 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) {
+ *K = ::Eigen::Matrix<double, kM, kN>::Zero();
+ *S = ::Eigen::Matrix<double, kN, kN>::Zero();
+ // Discrete (not continuous)
+ char DICO = 'D';
+ // B and R are provided instead of G.
+ char JOBB = 'B';
+ // Not factored, Q and R are provide.
+ char FACT = 'N';
+ // Store the upper triangle of Q and R.
+ char UPLO = 'U';
+ // L is zero.
+ char JOBL = 'Z';
+ // Stable eigenvalues first in the sort order
+ char SORT = 'S';
+
+ long N = 4;
+ long M = 2;
+ // Not needed since FACT = N
+ long P = 0;
+
+ long LDL = 1;
+
+ double RCOND = 0.0;
+ ::Eigen::Matrix<double, kN, kN> X = ::Eigen::Matrix<double, kN, kN>::Zero();
+
+ double ALFAR[kN * 2];
+ double ALFAI[kN * 2];
+ double BETA[kN * 2];
+ memset(ALFAR, 0, kN * 2);
+ memset(ALFAI, 0, kN * 2);
+ memset(BETA, 0, kN * 2);
+
+ long LDS = 2 * kN + kM;
+ ::Eigen::Matrix<double, 2 * kN + kM, 2 *kN + kM> S_schur =
+ ::Eigen::Matrix<double, 2 * kN + kM, 2 * kN + kM>::Zero();
+
+ ::Eigen::Matrix<double, 2 * kN + kM, 2 *kN> T =
+ ::Eigen::Matrix<double, 2 * kN + kM, 2 * kN>::Zero();
+ long LDT = 2 * kN + kM;
+
+ ::Eigen::Matrix<double, 2 * kN, 2 *kN> U =
+ ::Eigen::Matrix<double, 2 * kN, 2 * kN>::Zero();
+ long LDU = 2 * kN;
+
+ double TOL = 0.0;
+
+ long IWORK[2 * kN > kM ? 2 * kN : kM];
+ memset(IWORK, 0, 2 * kN > kM ? 2 * kN : kM);
+
+ long LDWORK = 16 * kN + 3 * kM + 16;
+
+ double DWORK[LDWORK];
+ memset(DWORK, 0, LDWORK);
+
+ long INFO = 0;
+
+ long BWORK[2 * kN];
+ memset(BWORK, 0, 2 * kN);
+
+ // TODO(austin): I can't tell if anything here is transposed...
+ sb02od_(&DICO, &JOBB, &FACT, &UPLO, &JOBL, &SORT, &N, &M, &P, A.data(), &N,
+ 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;
+ }
+}
+
+} // namespace controls
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DLQR_H_
diff --git a/y2018/control_loops/superstructure/arm/BUILD b/y2018/control_loops/superstructure/arm/BUILD
index ae2ed51..6224df1 100644
--- a/y2018/control_loops/superstructure/arm/BUILD
+++ b/y2018/control_loops/superstructure/arm/BUILD
@@ -4,11 +4,11 @@
"trajectory.cc",
],
hdrs = [
- "dlqr.h",
"trajectory.h",
],
deps = [
":dynamics",
+ "//frc971/control_loops:dlqr",
"//frc971/control_loops:jacobian",
"//third_party/eigen",
],
@@ -19,7 +19,6 @@
srcs = [
"trajectory_test.cc",
],
- linkopts = ["-lslicot"],
restricted_to = ["//tools:k8"],
deps = [
":demo_path",
@@ -69,7 +68,6 @@
srcs = [
"trajectory_plot.cc",
],
- linkopts = ["-lslicot"],
restricted_to = ["//tools:k8"],
deps = [
":demo_path",
@@ -78,4 +76,3 @@
"//third_party/matplotlib-cpp",
],
)
-
diff --git a/y2018/control_loops/superstructure/arm/trajectory.cc b/y2018/control_loops/superstructure/arm/trajectory.cc
index fbe38eb..14ec413 100644
--- a/y2018/control_loops/superstructure/arm/trajectory.cc
+++ b/y2018/control_loops/superstructure/arm/trajectory.cc
@@ -1,8 +1,8 @@
#include "y2018/control_loops/superstructure/arm/trajectory.h"
#include "Eigen/Dense"
+#include "frc971/control_loops/dlqr.h"
#include "frc971/control_loops/jacobian.h"
-#include "y2018/control_loops/superstructure/arm/dlqr.h"
#include "y2018/control_loops/superstructure/arm/dynamics.h"
namespace y2018 {