Add common QP Eigen interface

Author: vkotaru

.gitmodules

@@ -4,3 +4,6 @@
 [submodule "third_party/matplotlib-cpp"]
 	path = third_party/matplotlib-cpp
 	url = [email protected]:lava/matplotlib-cpp.git
+[submodule "third_party/qpSWIFT"]
+	path = third_party/qpSWIFT
+	url = [email protected]:vkotaru/qpSWIFT.git

CMakeLists.txt

@@ -25,6 +25,10 @@ set(INCLUDE_DIRS
         third_party/matplotlib-cpp # TODO learn why this is working and not include_directories
         )
 
+set(QUADPROG_SRCS
+        src/quadprog/quadprog.cpp
+        src/quadprog/qpswift_eigen.cpp)
+
 set(CONTROL_SRCS
         src/base_control.cpp
         src/geometric_control.cpp
@@ -36,12 +40,13 @@ set(CONTROL_SRCS
         src/SO3_vblmpc.cpp
         src/SE3_vblmpc.cpp
         src/SO3_clf.cpp
+        src/tracking_lmpc.cpp
         )
 
 add_subdirectory(third_party)
 
-add_library(nonlinear_controls ${CONTROL_SRCS})
-target_link_libraries(nonlinear_controls qpOASES_cpp ${PYTHON_LIBRARIES})
+add_library(nonlinear_controls ${CONTROL_SRCS} ${QUADPROG_SRCS})
+target_link_libraries(nonlinear_controls qpOASES_cpp qpswift ${PYTHON_LIBRARIES})
 target_include_directories(nonlinear_controls PUBLIC . ${INCLUDE_DIRS} ${CMAKE_CURRENT_SOURCE_DIR}/include)
 
 add_executable(test_main tests/test_nlc.cpp)

include/common/qp_interface.h

@@ -5,7 +5,7 @@
 
 namespace nonlinear_controls {
 
-struct QuadProg {
+struct QuadProgData {
     Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> H, A, Aeq;
     Eigen::Matrix<double, Eigen::Dynamic, 1> f, b, beq;
     Eigen::Matrix<double, Eigen::Dynamic, 1> xlb, xub;
@@ -18,7 +18,7 @@ class QPInterface {
    public:
     QPInterface(/* args */);
     ~QPInterface();
-    QuadProg problem_; 
+    QuadProgData problem_;
 
     virtual void setup();
     virtual void solve();

include/controls/tracking_lmpc.h

@@ -0,0 +1,23 @@
+//
+// Created by kotaru on 5/26/21.
+//
+
+#ifndef NONLINEAR_CONTROLS_TRACKING_LMPC_H
+#define NONLINEAR_CONTROLS_TRACKING_LMPC_H
+
+#include "deque"
+#include "quadprog/qpswift_eigen.h"
+#include "vector"
+
+namespace nonlinear_controls {
+
+class TrackingLinearMPC {
+protected:
+public:
+  TrackingLinearMPC();
+  ~TrackingLinearMPC();
+};
+
+} // namespace nonlinear_controls
+
+#endif // NONLINEAR_CONTROLS_TRACKING_LMPC_H

include/quadprog/qpswift_eigen.h

@@ -0,0 +1,24 @@
+//
+// Created by kotaru on 5/26/21.
+//
+
+#ifndef NONLINEAR_CONTROLS_QPSWIFT_EIGEN_H
+#define NONLINEAR_CONTROLS_QPSWIFT_EIGEN_H
+#include "Prime.h"
+#include "quadprog/quadprog.h"
+#include <iostream>
+
+namespace nonlinear_controls {
+
+class QPSwiftEigen : public QuadProg{
+protected:
+public:
+  QPSwiftEigen(const int &n, const int &m, const int &p);
+  ~QPSwiftEigen() = default;
+
+  int solve() override;
+};
+
+} // namespace nonlinear_controls
+
+#endif // NONLINEAR_CONTROLS_QPSWIFT_EIGEN_H

include/quadprog/quadprog.h

@@ -0,0 +1,46 @@
+//
+// Created by kotaru on 5/26/21.
+//
+
+#ifndef NONLINEAR_CONTROLS_QUADPROG_H
+#define NONLINEAR_CONTROLS_QUADPROG_H
+
+#include <eigen3/Eigen/Dense>
+
+namespace nonlinear_controls {
+
+class QuadProg {
+protected:
+  /// \brief Equality tolerance value
+  double equality_tol{1e-6};
+
+  /// \brief Number of decision variables
+  int nv;
+  /// \brief Number of inequality constraints
+  int ni;
+  /// \brief Number of equality constraints
+  int ne;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  QuadProg(const int &n, const int &m, const int &p);
+  ~QuadProg();
+
+  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> H;
+  Eigen::Matrix<double, Eigen::Dynamic, 1> f;
+  double c{};
+  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> A;
+  Eigen::Matrix<double, Eigen::Dynamic, 1> b;
+  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Aeq;
+  Eigen::Matrix<double, Eigen::Dynamic, 1> beq;
+  Eigen::Matrix<double, Eigen::Dynamic, 1> lb;
+  Eigen::Matrix<double, Eigen::Dynamic, 1> ub;
+  Eigen::Matrix<double, Eigen::Dynamic, 1> x0;
+  Eigen::Matrix<double, Eigen::Dynamic, 1> xOpt;
+
+  virtual int solve();
+};
+
+} // namespace nonlinear_controls
+
+#endif // NONLINEAR_CONTROLS_QUADPROG_H

src/qp_interface.cpp

@@ -11,11 +11,11 @@ QPInterface::~QPInterface() {
 }
 
 void QPInterface::setup() {
-    
+
 }
 
 void QPInterface::solve() {
-    
+
 }
 
 

src/quadprog/qpswift_eigen.cpp

@@ -0,0 +1,53 @@
+//
+// Created by kotaru on 5/26/21.
+//
+#include "quadprog/qpswift_eigen.h"
+
+namespace nonlinear_controls {
+
+QPSwiftEigen::QPSwiftEigen(const int &n, const int &m, const int &p)
+    : QuadProg(n, m, p) {}
+
+int QPSwiftEigen::solve() {
+  QP *myQP;
+
+  // Setup Function //
+  myQP = QP_SETUP_dense(this->nv, this->ni, this->ne, this->H.data(),
+                        this->Aeq.data(), this->A.data(), this->f.data(),
+                        this->b.data(), this->beq.data(), NULL);
+
+  /****************************************
+   *	After this, you can change the solver settings like this
+   *	myQP->options->maxit  = 30   (to change the maximum number of
+   iterations *to 30; default is 100)
+   *
+   * myQP->options->reltol = 1e-3 (to change the Relative tolerance to 1e-3;
+   *default is 1e-6)
+   *
+   * myQP->options->abstol  = 1e-3 (to change the Absolute
+   *tolerance to 1e-3; default is 1e-6)
+   *
+   * myQP->options->SIGMA  = 50 (to change the SIGMA to 50; default is 100;
+   *recommended not to change this)
+   *
+   *myQP->options->VERBOSE  = 0 (displays no output when set to 0; default
+   is *1 which corresponds to complete verbose mode)
+   ******************************************/
+
+  /* The Solution can be found as real pointer in myQP->x;It is an array of
+   * Dimension n */
+
+  qp_int ExitCode = QP_SOLVE(myQP);
+
+  std::cout << "Solution" << std::endl;
+
+  for (int i = 0; i < 3; ++i) {
+    this->xOpt(i) = myQP->x[i];
+  }
+  std::cout << "xOpt: " << this->xOpt.transpose() << std::endl;
+
+  QP_CLEANUP_dense(myQP);
+  return 1;
+}
+
+} // namespace nonlinear_controls

src/quadprog/quadprog.cpp

@@ -0,0 +1,28 @@
+//
+// Created by kotaru on 5/26/21.
+//
+#include "quadprog/quadprog.h"
+
+namespace nonlinear_controls {
+
+QuadProg::QuadProg(const int &n, const int &m, const int &p)
+    : nv(n), ni(m), ne(p) {
+  H.resize(nv, nv);
+  f.resize(nv);
+  A.resize(ni, nv);
+  b.resize(ni);
+  Aeq.resize(ne, nv);
+  beq.resize(ne);
+  lb.resize(nv);
+  ub.resize(nv);
+  x0.resize(nv);
+  xOpt.resize(nv);
+}
+
+QuadProg::~QuadProg() = default;
+
+int QuadProg::solve() {
+  throw std::string("Quadprog::Solve is not implemented yet.");
+}
+
+} // namespace nonlinear_controls

src/tracking_lmpc.cpp

@@ -0,0 +1,12 @@
+//
+// Created by kotaru on 5/26/21.
+//
+#include "controls/tracking_lmpc.h"
+
+namespace nonlinear_controls {
+
+TrackingLinearMPC::TrackingLinearMPC() {}
+
+TrackingLinearMPC::~TrackingLinearMPC() = default;
+
+} // namespace nonlinear_controls