Merge branch 'scalar-constraint' into 'master'

Fixed initial values for constraints, fixed ouput command, fixed sequence and...

See merge request altairLab/optcontrol/libmpc!34

.vscode/settings.json

@@ -85,7 +85,13 @@
         "locale": "cpp",
         "__config": "cpp",
         "__functional_03": "cpp",
-        "__functional_base": "cpp"
+        "__functional_base": "cpp",
+        "__bit_reference": "cpp",
+        "ios": "cpp",
+        "__split_buffer": "cpp",
+        "queue": "cpp",
+        "stack": "cpp",
+        "valarray": "cpp"
     },
     "restructuredtext.confPath": "${workspaceFolder}/docs/source",
     "testMate.cpp.test.executables": "${workspaceFolder}/bin/*"

CHANGELOG.md

@@ -1,5 +1,17 @@
 # Changelog
 
+## [0.3.0] - 2023-03-04
+
+## Added
+- Added new api in linear mpc to add a scalar constraints
+
+## Changed
+- In linear mpc the last input command is now used to initialize the optimal control problem
+
+### Fixed
+- The default value for the box constraints in linear mpc are now set -inf and inf
+- In linear mpc optimal input sequence was erroneously the delta input sequence
+
 ## [0.2.0] - 2023-01-09
 
 ### Added

docs/source/cite/cite.rst

@@ -10,7 +10,7 @@ Cite libmpc++ as something like
 
 ::
 
-    @misc{nlopt,
+    @misc{libmpc,
     author = {Piccinelli, N.},
     title = {libmpc++: a library to solve linear and non-linear MPC}
     howpublished = {/~https://github.com/nicolapiccinelli/libmpc}

docs/source/manual/manual.rst

@@ -22,11 +22,12 @@ The linear MPC address the solution of the following convex quadratic optimizati
                         & x_{\rm min} \le x_k  \le x_{\rm max} \\
                         & y_{\rm min} \le y_k  \le y_{\rm max} \\
                         & u_{\rm min} \le u_k  \le u_{\rm max} \\
+                        & s_{\rm min} \le x_s^T x_k + u_s^T u_k \le s_{\rm max}\\
                         & x_0 = \bar{x}
     \end{array}
 
 where the states :math:`x_k \in \mathbf{R}^{n_x}`, the outputs :math:`y_k \in \mathbf{R}^{n_y}` and the inputs :math:`u_k \in \mathbf{R}^{n_u}` are constrained to be between some lower and upper bounds.
-The problem is solved repeatedly for varying initial state :math:`\bar{x} \in \mathbf{R}^{n_x}`.
+THe states and the inputs can be also subjected to the so called "scalar constraints", where :math:`x_s` and :math:`u_s` are constant vectors. The problem is solved repeatedly for varying initial state :math:`\bar{x} \in \mathbf{R}^{n_x}`.
 
 The underlying linear system used within the MPC is defined as
 

include/mpc/Dim.hpp

@@ -131,4 +131,4 @@ namespace mpc
         Size eq;
     };
 
-} // namespace mpc
+} // namespace mpc

include/mpc/LMPC.hpp

@@ -172,27 +172,29 @@ namespace mpc
                     XMinMat, UMinMat, YMinMat,
                     XMaxMat, UMaxMat, YMaxMat);
             }
-
-            // Replicate on segment of the prediction horizon
-            size_t start = static_cast<size_t>(slice[0]);
-            size_t end = static_cast<size_t>(slice[1]);
-
-            if (start >= end || start > ph() || end > ph() || start + end > ph())
-            {
-                Logger::instance().log(Logger::log_type::ERROR) << "The horizon slice is out of bounds" << std::endl;
-                return false;
-            }
             else
             {
-                bool ret = true;
+                // Replicate on segment of the prediction horizon
+                size_t start = static_cast<size_t>(slice[0]);
+                size_t end = static_cast<size_t>(slice[1]);
 
-                for (size_t i = start; i < end; i++)
+                if (start >= end || start > ph() || end > ph() || start + end > ph())
                 {
-                    Logger::instance().log(Logger::log_type::DETAIL) << "Setting constraints for the step " << i << std::endl;
-                    ret = ret && builder.setConstraints(i, XMin, UMin, YMin, XMax, UMax, YMax);
+                    Logger::instance().log(Logger::log_type::ERROR) << "The horizon slice is out of bounds" << std::endl;
+                    return false;
                 }
+                else
+                {
+                    bool ret = true;
 
-                return ret;
+                    for (size_t i = start; i < end; i++)
+                    {
+                        Logger::instance().log(Logger::log_type::DETAIL) << "Setting constraints for the step " << i << std::endl;
+                        ret = ret && builder.setConstraints(i, XMin, UMin, YMin, XMax, UMax, YMax);
+                    }
+
+                    return ret;
+                }
             }
         }
 
@@ -217,6 +219,121 @@ namespace mpc
             return builder.setObjective(OWeightMat, UWeightMat, DeltaUWeightMat);
         }
 
+        /**
+         * @brief Set the state, input and output box constraints on a specific horizon step
+         *
+         * @param index the index to apply the constraint
+         * @param XMin minimum state vector
+         * @param UMin minimum input vector
+         * @param YMin minimum output vector
+         * @param XMax maximum state vector
+         * @param UMax maximum input vector
+         * @param YMax maximum output vector
+         * @return true
+         * @return false
+         */
+        bool setConstraints(const unsigned int index,
+                            const cvec<Tnx> XMin, const cvec<Tnu> UMin, const cvec<Tny> YMin,
+                            const cvec<Tnx> XMax, const cvec<Tnu> UMax, const cvec<Tny> YMax)
+        {
+            if (index >= ph())
+            {
+                Logger::instance().log(Logger::log_type::ERROR) << "Horizon index out of bounds" << std::endl;
+                return false;
+            }
+
+            Logger::instance().log(Logger::log_type::DETAIL) << "Setting constraints for the step " << index << std::endl;
+            return builder.setConstraints(index, XMin, UMin, YMin, XMax, UMax, YMax);
+        }
+
+        /**
+         * @brief Set the scalar constraints, the constraints are applied equally
+         * along the prediction horizon segment
+         *
+         * @param Min lower bound
+         * @param Max upper bound
+         * @param X the vector applied to the state variables
+         * @param U the vector applied to the manipulated variables
+         * @param slice slice of the prediction horizon step where to apply the constraints [start end]
+         * (if both ends re set to -1 the whole prediction horizon is used)
+         * @return true
+         * @return false
+         */
+        bool setScalarConstraint(
+            const double min, const double max,
+            const cvec<Tnx> X, const cvec<Tnu> U,
+            const std::array<int, 2> slice)
+        {
+            // Replicate all along the prediction horizon
+            if (slice[0] == -1 && slice[1] == -1)
+            {
+                // replicate the bounds all along the prediction horizon
+                cvec<Tph> Min, Max;
+
+                Min.resize(ph());
+                Max.resize(ph());
+
+                for (size_t i = 0; i < ph(); i++)
+                {
+                    Min.row(i) << min;
+                    Max.row(i) << max;
+                }
+
+                Logger::instance().log(Logger::log_type::DETAIL) << "Setting scalar constraint equally on the horizon" << std::endl;
+                return builder.setScalarConstraint(Min, Max, X, U);
+            }
+            else
+            {
+                // Replicate on segment of the prediction horizon
+                size_t start = static_cast<size_t>(slice[0]);
+                size_t end = static_cast<size_t>(slice[1]);
+
+                if (start >= end || start > ph() || end > ph() || start + end > ph())
+                {
+                    Logger::instance().log(Logger::log_type::ERROR) << "The horizon slice is out of bounds" << std::endl;
+                    return false;
+                }
+                else
+                {
+                    bool ret = true;
+
+                    for (size_t i = start; i < end; i++)
+                    {
+                        Logger::instance().log(Logger::log_type::DETAIL) << "Setting scalar constraints for the step " << i << std::endl;
+                        ret = ret && builder.setScalarConstraint(i, min, max, X, U);
+                    }
+
+                    return ret;
+                }
+            }
+        }
+
+        /**
+         * @brief Set the scalar constraints on a specific horizon step
+         *
+         * @param index the index to apply the constraint
+         * @param min lower bound
+         * @param max upper bound
+         * @param X the vector applied to the state variables
+         * @param U the vector applied to the manipulated variables
+         * @return true
+         * @return false
+         */
+        bool setScalarConstraint(
+            const unsigned int index,
+            const double min, const double max,
+            const cvec<Tnx> X, const cvec<Tnu> U)
+        {
+            if (index >= ph())
+            {
+                Logger::instance().log(Logger::log_type::ERROR) << "Horizon index out of bounds" << std::endl;
+                return false;
+            }
+
+            Logger::instance().log(Logger::log_type::DETAIL) << "Setting scalar constraint" << std::endl;
+            return builder.setScalarConstraint(index, min, max, X, U);
+        }
+
         /**
          * @brief Set the objective function weights, the weights are applied equally
          * along the specified prediction horizon segment
@@ -466,14 +583,9 @@ namespace mpc
          */
         void onSetup()
         {
-            builder.initialize(
-                nx(), nu(), ndu(), ny(),
-                ph(), ch());
-
+            builder.initialize(nx(), nu(), ndu(), ny(), ph(), ch());
             optPtr = new LOptimizer<MPCSize(Tnx, Tnu, Tndu, Tny, Tph, Tch, 0, 0)>();
-            optPtr->initialize(
-                nx(), nu(), ndu(), ny(),
-                ph(), ch());
+            optPtr->initialize(nx(), nu(), ndu(), ny(), ph(), ch());
 
             ((LOptimizer<MPCSize(Tnx, Tnu, Tndu, Tny, Tph, Tch, 0, 0)> *)optPtr)->setBuilder(&builder);
         }

include/mpc/LMPC/LOptimizer.hpp

@@ -173,7 +173,7 @@ namespace mpc
          * @return false
          */
         bool setExogenuosInputs(
-            const unsigned int index, 
+            const unsigned int index,
             const cvec<sizer.ndu> &uMeas)
         {
             extInputMeas.col(index) = uMeas;
@@ -269,36 +269,38 @@ namespace mpc
             // keep the last feasible solution
             if (work->solution->x != NULL)
             {
-                int index = 0;
-
-                cvec<sizer.nu> optCmd;
-                optCmd.resize(nu());
-
-                for (size_t i = ((ph() + 1) * (nx() + nu())); i < (((ph() + 1) * (nx() + nu())) + nu()); i++)
+                Logger::instance().log(Logger::log_type::DETAIL) << "Optimal vector: " << std::endl;
+                for (size_t i = 0; i < (size_t) P.rows(); i++)
                 {
-                    optCmd[index++] = work->solution->x[i];
+                    Logger::instance().log(Logger::log_type::DETAIL) << work->solution->x[i] << std::endl;
                 }
 
-                r.cmd = optCmd;
-                r.retcode = work->info->status_val;
-                r.cost = work->info->obj_val;
-                result = r;
-
                 // loop over the rows of the optimal sequence
                 for (size_t i = 1; i < ph() + 1; i++)
                 {
+                    // from the extended state vector [x,x_u] we take the first nx entries
+                    // to get the optimal sequence of system state
                     for (size_t j = 0; j < nx(); j++)
                     {
                         sequence.state.row(i - 1)[j] = work->solution->x[i * (nx() + nu()) + j];
                     }
 
-                    for (size_t j = 0; j < nu(); j++)
+                    // and similarly we take the nu entries to have the optimal sequence of system
+                    // input we also needs to deal with the fact that x_u(k) is u(k-1) (TODO?)
+                    for (size_t j = nx(); j < nx() + nu(); j++)
                     {
-                        sequence.input.row(i - 1)[j] = work->solution->x[((ph() + 1) * (nx() + nu())) + (i * nu()) + j];
+                        sequence.input.row(i - 1)[j - nx()] = work->solution->x[i * (nx() + nu()) + j];
                     }
 
+                    // this just the state mapping together with the optional exogeneous input
                     sequence.output.row(i - 1) = builder->mapToOutput(sequence.state.row(i - 1), extInputMeas.col(i - 1));
                 }
+
+                // the optimal command is the first control input in the sequence
+                r.cmd = sequence.input.row(0);
+                r.retcode = work->info->status_val;
+                r.cost = work->info->obj_val;
+                result = r;
             }
             else
             {