Merge branch 'get-linear-fix' into 'master'

Get linear fix See merge request altairLab/optcontrol/libmpc!28
nicolapiccinelli · Jun 15, 2022 · 5cf38bb · 5cf38bb
2 parents bf29c60 + 5bf6d95
commit 5cf38bb
Show file tree

Hide file tree

Showing 5 changed files with 137 additions and 105 deletions.
diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -78,7 +78,14 @@
         "lu": "cpp",
         "qr": "cpp",
         "svd": "cpp",
-        "orderingmethods": "cpp"
+        "orderingmethods": "cpp",
+        "__hash_table": "cpp",
+        "__tree": "cpp",
+        "__tuple": "cpp",
+        "locale": "cpp",
+        "__config": "cpp",
+        "__functional_03": "cpp",
+        "__functional_base": "cpp"
     },
     "restructuredtext.confPath": "${workspaceFolder}/docs/source"
 }
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -1,5 +1,13 @@
 # Changelog
 
+## [0.0.8.1] - 2022-06-15
+
+### Changed
+- Catch2 dependency updated to version 3
+
+### Fixed
+- Improved performances while building the optimization problem in LOptimizer
+
 ## [0.0.8] - 2022-03-02
 ### Added
 - Changelog added :)

diff --git a/include/mpc/LOptimizer.hpp b/include/mpc/LOptimizer.hpp
@@ -131,7 +131,12 @@ class LOptimizer : public IOptimizer<Tnx, Tnu, Tndu, Tny, Tph, Tch, 0, 0> {
         checkOrQuit();
         Result<Tnu> r;
 
-        auto mpcProblem = builder->get(x0, u0, outSysRef, cmdSysRef, deltaCmdSysRef, extInputMeas);
+        auto& mpcProblem = builder->get(x0, u0, outSysRef, cmdSysRef, deltaCmdSysRef, extInputMeas);
+
+        /* Getting number of milliseconds as a double. */
+        duration<double, std::milli> ms_double = t2 - t1;
+        std::cout << "out: " << ms_double.count() << "ms\n";
+
         smat P, A;
         mpcProblem.getSparse(P, A);
 
@@ -162,14 +167,14 @@ class LOptimizer : public IOptimizer<Tnx, Tnu, Tndu, Tny, Tph, Tch, 0, 0> {
                 Logger::instance().log(Logger::log_type::ERROR) << "Unable to create the P matrix" << std::endl;
             }
 
-            data->q = mpcProblem.q.data();
+            data->q = (c_float*) mpcProblem.q.data();
 
             if (!createOsqpSparseMatrix(A, data->A)) {
                 Logger::instance().log(Logger::log_type::ERROR) << "Unable to create the A matrix" << std::endl;
             }
 
-            data->l = mpcProblem.l.data();
-            data->u = mpcProblem.u.data();
+            data->l = (c_float *) mpcProblem.l.data();
+            data->u = (c_float *) mpcProblem.u.data();
         }
 
         // define solver settings as default

diff --git a/include/mpc/ProblemBuilder.hpp b/include/mpc/ProblemBuilder.hpp
@@ -28,13 +28,17 @@ class ProblemBuilder : public IComponent<Tnx, Tnu, Tndu, Tny, Tph, Tch, 0, 0> {
      */
     class Problem {
     public:
+        Problem() = default;
+        Problem(const Problem &) = delete;
+        Problem &operator=(const Problem &) = delete;
+
         /**
          * @brief Get the sparse matrices
          * 
          * @param Psparse objective function P matrix
          * @param Asparse constraints A matrix
          */
-        void getSparse(smat& Psparse, smat& Asparse)
+        void getSparse(smat& Psparse, smat& Asparse) const
         {
             // converting P matrix to sparse and
             // getting the upper triangular part of the matrix

diff --git a/include/mpc/Types.hpp b/include/mpc/Types.hpp
@@ -16,7 +16,8 @@
 
 #undef eigen_assert
 #define eigen_assert(x)                              \
-    if (!(x)) {                                      \
+    if (!(x))                                        \
+    {                                                \
         std::cout << boost::stacktrace::stacktrace() \
                   << std::endl                       \
                   << std::endl;                      \
@@ -28,107 +29,114 @@
 #include <Eigen/Dense>
 #include <Eigen/Sparse>
 
-namespace mpc {
-
-template <
-    int M = Eigen::Dynamic,
-    int N = Eigen::Dynamic>
-using mat = Eigen::Matrix<double, M, N>;
-
-using smat = Eigen::SparseMatrix<double, Eigen::ColMajor>;
-
-template <
-    int N = Eigen::Dynamic>
-using cvec = Eigen::Matrix<double, N, 1>;
-
-template <
-    int N = Eigen::Dynamic>
-using rvec = Eigen::Matrix<double, 1, N>;
-
-/**
- * @brief Shared optimizer parameters
- */
-struct Parameters {
-protected:
-    Parameters() = default;
-    virtual ~Parameters() = default;
-
-public:
-    int maximum_iteration = 100;
-};
-
-/**
- * @brief Non-linear optimizer parameters
- */
-struct NLParameters : Parameters {
-    NLParameters() = default;
-
-    double relative_ftol = 1e-10;
-    double relative_xtol = 1e-10;
-    bool hard_constraints = true;
-};
-
-/**
- * @brief Linear optimizer parameters
- */
-struct LParameters : Parameters {
-    LParameters() = default;
-
-    double alpha = 1.6;
-    double rho = 1e-6;
-
-    double eps_rel = 1e-4;
-    double eps_abs = 1e-4;   
-    double eps_prim_inf = 1e-3;
-    double eps_dual_inf = 1e-3;
-
-    bool verbose = false;
-    bool adaptive_rho = true;
-    bool polish = true;
-};
-
-/**
- * @brief Optimization control input result
- * 
- * @tparam Tnu dimension of the input space
- */
-template <int Tnu = Eigen::Dynamic>
-struct Result {
-    Result()
-        : retcode(0)
-        , cost(0)
+namespace mpc
+{
+
+    template <
+        int M = Eigen::Dynamic,
+        int N = Eigen::Dynamic>
+    using mat = Eigen::Matrix<double, M, N>;
+
+    using smat = Eigen::SparseMatrix<double, Eigen::ColMajor>;
+
+    template <
+        int N = Eigen::Dynamic>
+    using cvec = Eigen::Matrix<double, N, 1>;
+
+    template <
+        int N = Eigen::Dynamic>
+    using rvec = Eigen::Matrix<double, 1, N>;
+
+    /**
+     * @brief Shared optimizer parameters
+     */
+    struct Parameters
     {
-        cmd.setZero();
-    }
+    protected:
+        Parameters() = default;
+        virtual ~Parameters() = default;
+
+    public:
+        int maximum_iteration = 100;
+    };
+
+    /**
+     * @brief Non-linear optimizer parameters
+     */
+    struct NLParameters : Parameters
+    {
+        NLParameters() = default;
 
-    int retcode;
-    double cost;
-    cvec<Tnu> cmd;
-};
-
-enum constraints_type {
-    INEQ,
-    EQ,
-    UINEQ,
-    UEQ
-};
-
-/**
- * @brief Utility to get the dimension based on the input flag
- * 
- * @param n value
- * @param c flag
- * @return constexpr int input dimension or -1
- */
-inline constexpr int makeDim(const int n, bool c)
-{
-    if (c) {
-        return n;
-    } else {
-        return Eigen::Dynamic;
+        double relative_ftol = 1e-10;
+        double relative_xtol = 1e-10;
+        bool hard_constraints = true;
+    };
+
+    /**
+     * @brief Linear optimizer parameters
+     */
+    struct LParameters : Parameters
+    {
+        LParameters() = default;
+
+        double alpha = 1.6;
+        double rho = 1e-6;
+
+        double eps_rel = 1e-4;
+        double eps_abs = 1e-4;
+        double eps_prim_inf = 1e-3;
+        double eps_dual_inf = 1e-3;
+
+        bool verbose = false;
+        bool adaptive_rho = true;
+        bool polish = true;
+    };
+
+    /**
+     * @brief Optimization control input result
+     *
+     * @tparam Tnu dimension of the input space
+     */
+    template <int Tnu = Eigen::Dynamic>
+    struct Result
+    {
+        Result(): retcode(0), cost(0)
+        {
+            cmd.setZero();
+        }
+
+        int retcode;
+        double cost;
+        cvec<Tnu> cmd;
+    };
+
+    enum constraints_type
+    {
+        INEQ,
+        EQ,
+        UINEQ,
+        UEQ
+    };
+
+    /**
+     * @brief Utility to get the dimension based on the input flag
+     *
+     * @param n value
+     * @param c flag
+     * @return constexpr int input dimension or -1
+     */
+    inline constexpr int makeDim(const int n, bool c)
+    {
+        if (c)
+        {
+            return n;
+        }
+        else
+        {
+            return Eigen::Dynamic;
+        }
     }
-}
 
-constexpr double inf = std::numeric_limits<double>::infinity();
+    constexpr double inf = std::numeric_limits<double>::infinity();
 
 } // namespace mpc