From 9dc582bd9acfe34cec4204113a8795c4c01b3703 Mon Sep 17 00:00:00 2001 From: Nicola Piccinelli Date: Sat, 4 Mar 2023 15:48:03 +0000 Subject: [PATCH] Fixed initial values for constraints, fixed ouput command, fixed sequence and... --- .vscode/settings.json | 8 +- CHANGELOG.md | 12 + docs/source/cite/cite.rst | 2 +- docs/source/manual/manual.rst | 3 +- include/mpc/Dim.hpp | 2 +- include/mpc/LMPC.hpp | 156 +++++++++++-- include/mpc/LMPC/LOptimizer.hpp | 32 +-- include/mpc/LMPC/ProblemBuilder.hpp | 210 ++++++++++++++---- include/mpc/Types.hpp | 1 - test/CMakeLists.txt | 15 +- test/LMPC/test_lmpc.cpp | 176 +++++++++++++++ test/{ => LMPC}/test_mutiple_instances.cpp | 0 .../test_quadrotor.cpp} | 78 +++---- test/{ => NLMPC}/test_common.cpp | 0 test/{ => NLMPC}/test_constraints.cpp | 0 test/{ => NLMPC}/test_discrete_lti_siso.cpp | 0 test/{ => NLMPC}/test_objective.cpp | 0 test/{ => NLMPC}/test_vanderpol.cpp | 0 18 files changed, 563 insertions(+), 132 deletions(-) create mode 100644 test/LMPC/test_lmpc.cpp rename test/{ => LMPC}/test_mutiple_instances.cpp (100%) rename test/{test_lmpc.cpp => LMPC/test_quadrotor.cpp} (62%) rename test/{ => NLMPC}/test_common.cpp (100%) rename test/{ => NLMPC}/test_constraints.cpp (100%) rename test/{ => NLMPC}/test_discrete_lti_siso.cpp (100%) rename test/{ => NLMPC}/test_objective.cpp (100%) rename test/{ => NLMPC}/test_vanderpol.cpp (100%) diff --git a/.vscode/settings.json b/.vscode/settings.json index 940768f..f2630f5 100644 --- a/.vscode/settings.json +++ b/.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/*" diff --git a/CHANGELOG.md b/CHANGELOG.md index d41d671..d50a397 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/docs/source/cite/cite.rst b/docs/source/cite/cite.rst index 64f4306..b22e58d 100644 --- a/docs/source/cite/cite.rst +++ b/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} diff --git a/docs/source/manual/manual.rst b/docs/source/manual/manual.rst index 7e67fa1..a37d215 100644 --- a/docs/source/manual/manual.rst +++ b/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 diff --git a/include/mpc/Dim.hpp b/include/mpc/Dim.hpp index 77ef048..52f5a26 100644 --- a/include/mpc/Dim.hpp +++ b/include/mpc/Dim.hpp @@ -131,4 +131,4 @@ namespace mpc Size eq; }; -} // namespace mpc +} // namespace mpc \ No newline at end of file diff --git a/include/mpc/LMPC.hpp b/include/mpc/LMPC.hpp index a482ccc..8ceefda 100644 --- a/include/mpc/LMPC.hpp +++ b/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(slice[0]); - size_t end = static_cast(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(slice[0]); + size_t end = static_cast(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 XMin, const cvec UMin, const cvec YMin, + const cvec XMax, const cvec UMax, const cvec 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 X, const cvec U, + const std::array slice) + { + // Replicate all along the prediction horizon + if (slice[0] == -1 && slice[1] == -1) + { + // replicate the bounds all along the prediction horizon + cvec 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(slice[0]); + size_t end = static_cast(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 X, const cvec 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(); - optPtr->initialize( - nx(), nu(), ndu(), ny(), - ph(), ch()); + optPtr->initialize(nx(), nu(), ndu(), ny(), ph(), ch()); ((LOptimizer *)optPtr)->setBuilder(&builder); } diff --git a/include/mpc/LMPC/LOptimizer.hpp b/include/mpc/LMPC/LOptimizer.hpp index 42b41c1..a8212af 100644 --- a/include/mpc/LMPC/LOptimizer.hpp +++ b/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 &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 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 { diff --git a/include/mpc/LMPC/ProblemBuilder.hpp b/include/mpc/LMPC/ProblemBuilder.hpp index 4762f04..69d6685 100644 --- a/include/mpc/LMPC/ProblemBuilder.hpp +++ b/include/mpc/LMPC/ProblemBuilder.hpp @@ -71,9 +71,9 @@ namespace mpc // objective_vector is q cvec<(((sizer.ph + 1) * (sizer.nu + sizer.nx)) + (sizer.ph * sizer.nu))> q; // constraint_matrix is A - mat<(((sizer.ph + 1) * (sizer.nu + sizer.nx)) + (((sizer.ph + 1) * (sizer.nu + sizer.nx)) + (((sizer.ph + 1) * sizer.ny) + (sizer.ph * sizer.nu)))), (((sizer.ph + 1) * (sizer.nu + sizer.nx)) + (sizer.ph * sizer.nu))> A; + mat<(((sizer.ph + 1) * (sizer.nu + sizer.nx)) + (((sizer.ph + 1) * (sizer.nu + sizer.nx)) + (((sizer.ph + 1) * sizer.ny) + (sizer.ph * sizer.nu))) + (sizer.ph + 1)), (((sizer.ph + 1) * (sizer.nu + sizer.nx)) + (sizer.ph * sizer.nu))> A; // lower_bounds is l and upper_bounds is u - cvec<(((sizer.ph + 1) * (sizer.nu + sizer.nx)) + (((sizer.ph + 1) * (sizer.nu + sizer.nx)) + (((sizer.ph + 1) * sizer.ny) + (sizer.ph * sizer.nu))))> l, u; + cvec<(((sizer.ph + 1) * (sizer.nu + sizer.nx)) + (((sizer.ph + 1) * (sizer.nu + sizer.nx)) + (((sizer.ph + 1) * sizer.ny) + (sizer.ph * sizer.nu))) + (sizer.ph + 1))> l, u; }; ProblemBuilder() = default; @@ -106,11 +106,16 @@ namespace mpc minU.resize(nu(), ph()); maxU.resize(nu(), ph()); + sMin.resize(ph() + 1); + sMax.resize(ph() + 1); + sMultiplier.resize((ph() + 1), (ph() + 1) * (nu() + nx())); + leq.resize(((ph() + 1) * (nu() + nx()))); ueq.resize(((ph() + 1) * (nu() + nx()))); - lineq.resize((((ph() + 1) * (nu() + nx())) + (((ph() + 1) * ny()) + (ph() * nu())))); - uineq.resize((((ph() + 1) * (nu() + nx())) + (((ph() + 1) * ny()) + (ph() * nu())))); + lineq.resize((((ph() + 1) * (nu() + nx())) + (((ph() + 1) * ny()) + (ph() * nu()) + (ph() + 1)))); + uineq.resize((((ph() + 1) * (nu() + nx())) + (((ph() + 1) * ny()) + (ph() * nu()) + (ph() + 1)))); + ineq_offset.resize((((ph() + 1) * (nu() + nx())) + (((ph() + 1) * ny()) + (ph() * nu()) + (ph() + 1)))); ssA.setZero(); ssB.setZero(); @@ -122,15 +127,24 @@ namespace mpc wU.setZero(); wDeltaU.setZero(); - minX.setZero(); - maxX.setZero(); - minY.setZero(); - maxY.setZero(); - minU.setZero(); - maxU.setZero(); + // setting the box constraints to -inf and inf + minX.setConstant(-inf); + minY.setConstant(-inf); + minU.setConstant(-inf); + maxX.setConstant(inf); + maxY.setConstant(inf); + maxU.setConstant(inf); + + // setting the scalar constraints to -inf and inf + sMin.setConstant(-inf); + sMax.setConstant(inf); + + // multiplier is defaulted to zero + sMultiplier.setZero(); leq.setZero(); ueq.setZero(); + lineq.setZero(); uineq.setZero(); @@ -140,18 +154,21 @@ namespace mpc mpcProblem.q.resize( (((ph() + 1) * (nu() + nx())) + (ph() * nu()))); mpcProblem.A.resize( - (((ph() + 1) * (nu() + nx())) + ((ph() + 1) * (nu() + nx())) + (((ph() + 1) * ny()) + (ph() * nu()))), + (((ph() + 1) * (nu() + nx())) + ((ph() + 1) * (nu() + nx())) + (((ph() + 1) * ny()) + (ph() * nu()) + (ph() + 1))), (((ph() + 1) * (nu() + nx())) + (ph() * nu()))); mpcProblem.l.resize( - (((ph() + 1) * (nu() + nx())) + (((ph() + 1) * (nu() + nx())) + (((ph() + 1) * ny()) + (ph() * nu()))))); + (((ph() + 1) * (nu() + nx())) + (((ph() + 1) * (nu() + nx())) + (((ph() + 1) * ny()) + (ph() * nu())) + (ph() + 1)))); mpcProblem.u.resize( - (((ph() + 1) * (nu() + nx())) + (((ph() + 1) * (nu() + nx())) + (((ph() + 1) * ny()) + (ph() * nu()))))); + (((ph() + 1) * (nu() + nx())) + (((ph() + 1) * (nu() + nx())) + (((ph() + 1) * ny()) + (ph() * nu())) + (ph() + 1)))); mpcProblem.P.setZero(); mpcProblem.q.setZero(); mpcProblem.A.setZero(); mpcProblem.l.setZero(); mpcProblem.u.setZero(); + + // let's build the time invariant terms using the default conditions + buildTimeInvariantTems(); } /** @@ -170,7 +187,12 @@ namespace mpc { checkOrQuit(); - // augmenting to system to store the command input of the current timestep + // state vector [x(t) x_u(t)], where x_u(t) = u(t-1) + // we are augmenting the system to store the command input of the current timestep + // the system we are using is the following: + // x(t + 1) = A x(t) + B u(t-1) + B Delta_u(t) + // x_u(t + 1) = x_u(t) + Delta_u(t) + ssA.block(0, 0, nx(), nx()) = A; ssA.block(0, nx(), nx(), nu()) = B; ssA.block(nx(), 0, nu(), nx()).setZero(); @@ -180,10 +202,12 @@ namespace mpc ssB.block(nx(), 0, nu(), nu()).setIdentity(); // we put on the output also the command to allow its penalization + // NOTE: here we have that at horizon step p we have in output the + // command applied at the step p-1 ssC.block(0, 0, ny(), nx()) = C; ssC.block(ny(), nx(), nu(), nu()).setIdentity(); - return buildTITerms(); + return buildTimeInvariantTems(); } /** @@ -208,7 +232,7 @@ namespace mpc ssDv.setZero(); ssDv.block(0, 0, ny(), ndu()) = Dd; - return buildTITerms(); + return buildTimeInvariantTems(); } /** @@ -235,7 +259,7 @@ namespace mpc wDeltaU = DeltaUWeight; - return buildTITerms(); + return buildTimeInvariantTems(); } /** @@ -248,7 +272,7 @@ namespace mpc * @return false */ bool setObjective( - const unsigned int index, + const unsigned int &index, const cvec &OWeight, const cvec &UWeight, const cvec &DeltaUWeight) @@ -269,7 +293,83 @@ namespace mpc wDeltaU.block(0, index, nu(), 1) = DeltaUWeight; - return buildTITerms(); + return buildTimeInvariantTems(); + } + + /** + * @brief Set the scalar constraint for a specific horizon step + * + * @param index index of the horizon step + * @param min the lower bound + * @param max the upper bound + * @param X the constant term multiplied to the state + * @param U the constant term multiplied to the input + * @return true + * @return false + */ + bool setScalarConstraint( + const unsigned index, + const double &min, const double &max, + const cvec &X, const cvec &U) + { + checkOrQuit(); + + sMin[index + 1] = min; + if (index == 0) + { + sMin(0) = min; + } + + sMax[index + 1] = max; + if (index == 0) + { + sMax(0) = max; + } + + for (size_t i = 0; i < ph() + 1; i++) + { + for (size_t j = 0; j < ph() + 1; j++) + { + sMultiplier.block(i, j * ph(), 1, nx()) = X.transpose(); + sMultiplier.block(i, (j * ph()) + nx(), 1, nu()) = U.transpose(); + } + } + + return buildTimeInvariantTems(); + } + + /** + * @brief Set the scalar constraint + * + * @param MinMat the lower bound + * @param MaxMat the upper bound + * @param X the constant term multiplied to the state + * @param U the constant term multiplied to the input + * @return true + * @return false + */ + bool setScalarConstraint( + const cvec &MinMat, const cvec &MaxMat, + const cvec &X, const cvec &U) + { + checkOrQuit(); + + sMin.segment(1, ph()) = MinMat; + sMin(0) = MinMat(0); + + sMax.segment(1, ph()) = MaxMat; + sMax(0) = MaxMat(0); + + for (size_t i = 0; i < ph() + 1; i++) + { + for (size_t j = 0; j < ph() + 1; j++) + { + sMultiplier.block(i, j * ph(), 1, nx()) = X.transpose(); + sMultiplier.block(i, (j * ph()) + nx(), 1, nu()) = U.transpose(); + } + } + + return buildTimeInvariantTems(); } /** @@ -303,7 +403,7 @@ namespace mpc minU = UMin; maxU = UMax; - return buildTITerms(); + return buildTimeInvariantTems(); } /** @@ -320,9 +420,9 @@ namespace mpc * @return false */ bool setConstraints( - const unsigned int index, - const cvec XMin, const cvec UMin, const cvec YMin, - const cvec XMax, const cvec UMax, const cvec YMax) + const unsigned int &index, + const cvec &XMin, const cvec &UMin, const cvec &YMin, + const cvec &XMax, const cvec &UMax, const cvec &YMax) { checkOrQuit(); @@ -353,18 +453,18 @@ namespace mpc minU.block(0, index, nu(), 1) = UMin; maxU.block(0, index, nu(), 1) = UMax; - return buildTITerms(); + return buildTimeInvariantTems(); } /** * @brief Compute the relative output of the system based on the desired * state and measured disturbance - * + * * @param desState desired state vector to project * @param measDist measured disturbance vector * @return cvec the output vector */ - cvec mapToOutput(cvec desState, cvec measDist) + cvec mapToOutput(const cvec &desState, const cvec &measDist) { return ssC.block(0, 0, ny(), nx()) * desState + ssDv.block(0, 0, ny(), ndu()) * measDist; } @@ -380,7 +480,7 @@ namespace mpc */ const Problem &get( const cvec &x0, - const cvec & /*u0*/, + const cvec &u0, const mat &yRef, const mat &uRef, const mat &deltaURef, @@ -398,6 +498,13 @@ namespace mpc mpcProblem.q.setZero(); leq.setZero(); + ineq_offset.setZero(); + + // creation of bounds and here is the right place to take into account + // of the measured exogenous inputs on the output (we are gonna treat them as offsets) + mpcProblem.l.setZero(); + mpcProblem.u.setZero(); + for (size_t i = 0; i < ph() + 1; i++) { // definition of the references (this check is needed since at the first @@ -448,27 +555,19 @@ namespace mpc // let's add on the output part of the system // any contribution of the exogenous inputs on the output function - lineq.middleRows( - (i * ny()) + ((ph() + 1) * (nu() + nx())), - ny()) -= ssDv.block(0, 0, ny(), ndu()) * uMeas_ex; - - uineq.middleRows( + ineq_offset.middleRows( (i * ny()) + ((ph() + 1) * (nu() + nx())), - ny()) -= ssDv.block(0, 0, ny(), ndu()) * uMeas_ex; + ny()) = -ssDv.block(0, 0, ny(), ndu()) * uMeas_ex; } // state evolution depends on the initial condition and // on the exogeneous inputs so they are changes over time leq.middleRows(0, nx()) = -x0; + leq.middleRows(nx(), nu()) = -u0; // set lower and upper bound equal in order to have equality constraints ueq = leq; - // creation of bounds and here is the right place to take into account - // of the measured exogenous inputs on the output (we are gonna treat them as offsets) - mpcProblem.l.setZero(); - mpcProblem.u.setZero(); - mpcProblem.l.middleRows( 0, (ph() + 1) * (nu() + nx())) = leq; @@ -477,11 +576,11 @@ namespace mpc mpcProblem.l.middleRows( (ph() + 1) * (nu() + nx()), - ((ph() + 1) * (nu() + nx())) + ((ph() + 1) * ny()) + (ph() * nu())) = lineq; + ((ph() + 1) * (nu() + nx())) + ((ph() + 1) * ny()) + (ph() * nu()) + (ph() + 1)) = lineq + ineq_offset; mpcProblem.u.middleRows( (ph() + 1) * (nu() + nx()), - ((ph() + 1) * (nu() + nx())) + ((ph() + 1) * ny()) + (ph() * nu())) = uineq; + ((ph() + 1) * (nu() + nx())) + ((ph() + 1) * ny()) + (ph() * nu()) + (ph() + 1)) = uineq + ineq_offset; return mpcProblem; } @@ -493,7 +592,7 @@ namespace mpc * @return true * @return false */ - bool buildTITerms() + bool buildTimeInvariantTems() { // quadratic objective mat<(sizer.nu + sizer.ny), (sizer.nu + sizer.ny)> wExtendedState; @@ -556,9 +655,9 @@ namespace mpc ((ph() + 1) * (nu() + nx())), (ph() * nu())) = kroneckerProduct(idenBd, ssB).eval(); // input, state and output constraints - mat<(((sizer.ph + 1) * (sizer.nu + sizer.nx)) + (((sizer.ph + 1) * sizer.ny) + (sizer.ph * sizer.nu))), (((sizer.ph + 1) * (sizer.nu + sizer.nx)) + (sizer.ph * sizer.nu))> Aineq; + mat<(((sizer.ph + 1) * (sizer.nu + sizer.nx)) + (((sizer.ph + 1) * sizer.ny) + (sizer.ph * sizer.nu)) + (sizer.ph + 1)), (((sizer.ph + 1) * (sizer.nu + sizer.nx)) + (sizer.ph * sizer.nu))> Aineq; Aineq.resize( - (((ph() + 1) * (nu() + nx())) + (((ph() + 1) * ny()) + (ph() * nu()))), + (((ph() + 1) * (nu() + nx())) + (((ph() + 1) * ny()) + (ph() * nu()) + (ph() + 1))), (((ph() + 1) * (nu() + nx())) + (ph() * nu()))); Aineq.setZero(); @@ -646,6 +745,22 @@ namespace mpc nu()) = deltaU * maxDeltaU; } + // insert a scalar constraint + // TODO add support for multple scalar constraints + Aineq.block( + ((ph() + 1) * (nu() + nx())) + ((ph() + 1) * ny()) + (ph() * nu()), + 0, + ph() + 1, + (ph() + 1) * (nu() + nx())) = sMultiplier; + + lineq.middleRows( + ((ph() + 1) * (nu() + nx())) + ((ph() + 1) * ny()) + (ph() * nu()), + ph() + 1) = sMin; + + uineq.middleRows( + ((ph() + 1) * (nu() + nx())) + ((ph() + 1) * ny()) + (ph() * nu()), + ph() + 1) = sMax; + // creation of matrix A mpcProblem.A.setZero(); @@ -656,7 +771,7 @@ namespace mpc mpcProblem.A.block( ((ph() + 1) * (nu() + nx())), 0, - (((ph() + 1) * (nu() + nx())) + (((ph() + 1) * ny()) + (ph() * nu()))), + (((ph() + 1) * (nu() + nx())) + (((ph() + 1) * ny()) + (ph() * nu())) + (ph() + 1)), (((ph() + 1) * (nu() + nx())) + (ph() * nu()))) = Aineq; return true; @@ -685,8 +800,13 @@ namespace mpc mat minY, maxY; mat minU, maxU; + // scalar constraint + cvec sMin; + cvec sMax; + mat sMultiplier; + Problem mpcProblem; cvec<((sizer.ph + 1) * (sizer.nu + sizer.nx))> leq, ueq; - cvec<(((sizer.ph + 1) * (sizer.nu + sizer.nx)) + (((sizer.ph + 1) * sizer.ny) + (sizer.ph * sizer.nu)))> lineq, uineq; + cvec<(((sizer.ph + 1) * (sizer.nu + sizer.nx)) + (((sizer.ph + 1) * sizer.ny) + (sizer.ph * sizer.nu)) + (sizer.ph + 1))> lineq, uineq, ineq_offset; }; } \ No newline at end of file diff --git a/include/mpc/Types.hpp b/include/mpc/Types.hpp index 930aa5f..1357c66 100644 --- a/include/mpc/Types.hpp +++ b/include/mpc/Types.hpp @@ -35,7 +35,6 @@ namespace mpc { - template < int M = Eigen::Dynamic, int N = Eigen::Dynamic> diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 59e1c81..c637f75 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -40,18 +40,19 @@ if(CMAKE_COMPILER_IS_GNUCXX) endif() set(MPC_TEST_LIB_SOURCES - "test_constraints.cpp" - "test_common.cpp" - "test_objective.cpp" + "NLMPC/test_constraints.cpp" + "NLMPC/test_objective.cpp" + "NLMPC/test_common.cpp" + "LMPC/test_lmpc.cpp" + "LMPC/test_mutiple_instances.cpp" "test_utils.cpp" - "test_lmpc.cpp" "test_logger.cpp" "test_main.cpp") set(MPC_TEST_CASES_SOURCES - "test_discrete_lti_siso.cpp" - "test_mutiple_instances.cpp" - "test_vanderpol.cpp" + "NLMPC/test_discrete_lti_siso.cpp" + "NLMPC/test_vanderpol.cpp" + "LMPC/test_quadrotor.cpp" "test_main.cpp") add_executable(test_lib_dynamic ${MPC_TEST_LIB_SOURCES}) diff --git a/test/LMPC/test_lmpc.cpp b/test/LMPC/test_lmpc.cpp new file mode 100644 index 0000000..7c9aae3 --- /dev/null +++ b/test/LMPC/test_lmpc.cpp @@ -0,0 +1,176 @@ +/* + * Copyright (c) 2023 Nicola Piccinelli + * All rights reserved. + */ +#include "basic.hpp" +#include +#include + +TEST_CASE( + MPC_TEST_NAME("Linear default constraints"), + MPC_TEST_TAGS("[linear]")) +{ + constexpr int Tnx = 3; + constexpr int Tny = 4; + constexpr int Tnu = 5; + constexpr int Tndu = 6; + constexpr int Tph = 5; + constexpr int Tch = 5; + + mpc::ProblemBuilder builder; + builder.initialize(Tnx, Tnu, Tndu, Tny, Tph, Tch); + + mpc::cvec x0; + x0.fill(1); + mpc::cvec u0; + u0.fill(-1); + + mpc::mat yRef; + mpc::mat uRef; + mpc::mat deltaURef; + mpc::mat uMeas; + + auto &res = builder.get(x0, u0, yRef, uRef, deltaURef, uMeas); + + REQUIRE((res.l.segment(0, Tnx).array() == -1).all()); + REQUIRE((res.l.segment(Tnx, Tnu).array() == 1).all()); + REQUIRE(res.l.segment(Tnx + Tnu, Tph * (Tnx + Tnu)).isZero()); + REQUIRE((res.l.segment((Tph + 1) * (Tnx + Tnu), ((Tph + 1) * (Tnu + Tnx)) + ((Tph + 1) * Tny) + (Tph * Tnu) + (Tph + 1)).array() == -mpc::inf).all()); + + REQUIRE((res.u.segment(0, Tnx).array() == -1).all()); + REQUIRE((res.u.segment(Tnx, Tnu).array() == 1).all()); + REQUIRE(res.u.segment(Tnx + Tnu, Tph * (Tnx + Tnu)).isZero()); + REQUIRE((res.u.segment((Tph + 1) * (Tnx + Tnu), ((Tph + 1) * (Tnu + Tnx)) + ((Tph + 1) * Tny) + (Tph * Tnu) + (Tph + 1)).array() == mpc::inf).all()); +} + +TEST_CASE( + MPC_TEST_NAME("Linear constraints") + MPC_TEST_TAGS("[linear]")) +{ + constexpr int Tnx = 2; + constexpr int Tny = 3; + constexpr int Tnu = 4; + constexpr int Tndu = 0; + constexpr int Tph = 3; + constexpr int Tch = 3; + + mpc::ProblemBuilder builder; + builder.initialize(Tnx, Tnu, Tndu, Tny, Tph, Tch); + + mpc::mat xminmat, xmaxmat; + mpc::mat yminmat, ymaxmat; + mpc::mat uminmat, umaxmat; + + xminmat.setConstant(-1); + xmaxmat.setConstant(1); + + yminmat.setConstant(-2); + ymaxmat.setConstant(2); + + uminmat.setConstant(-3); + umaxmat.setConstant(3); + + builder.setConstraints(xminmat, uminmat, yminmat, xmaxmat, umaxmat, ymaxmat); + + mpc::cvec x0; + x0.fill(42.0); + mpc::cvec u0; + u0.fill(-42.0); + + mpc::cvec smin, smax; + smin.setConstant(-4); + smax.setConstant(4); + + builder.setScalarConstraint(smin, smax, x0, u0); + + mpc::mat yRef; + yRef.setZero(); + mpc::mat uRef; + uRef.setZero(); + mpc::mat deltaURef; + deltaURef.setZero(); + mpc::mat uMeas; + uMeas.setZero(); + + auto &res = builder.get(x0, u0, yRef, uRef, deltaURef, uMeas); + + mpc::cvec<(Tph + 1) * (Tnu + Tnx)> expected_xu_l, expected_xu_u; + for (size_t i = 0; i < Tph + 1; i++) + { + mpc::cvec t; + mpc::cvec tt; + + t.setConstant(-1); + tt.setConstant(-3); + expected_xu_l.segment(i * (Tnu + Tnx), Tnu + Tnx) << t, tt; + + t.setConstant(1); + tt.setConstant(3); + expected_xu_u.segment(i * (Tnu + Tnx), Tnu + Tnx) << t, tt; + } + + REQUIRE(res.l.segment((Tph + 1) * (Tnu + Tnx), expected_xu_l.size()).isApprox(expected_xu_l)); + REQUIRE(res.u.segment((Tph + 1) * (Tnu + Tnx), expected_xu_u.size()).isApprox(expected_xu_u)); + + mpc::cvec<(Tph + 1) * Tny> expected_y_l, expected_y_u; + for (size_t i = 0; i < Tph + 1; i++) + { + expected_y_l.segment(i * Tny, Tny).setConstant(-2); + expected_y_u.segment(i * Tny, Tny).setConstant(2); + } + + REQUIRE(res.l.segment(((Tph + 1) * (Tnu + Tnx)) + expected_xu_l.size(), expected_y_l.size()).isApprox(expected_y_l)); + REQUIRE(res.u.segment(((Tph + 1) * (Tnu + Tnx)) + expected_xu_u.size(), expected_y_u.size()).isApprox(expected_y_u)); + + if (Tch >= Tph) + { + REQUIRE((res.l.segment(((Tph + 1) * (Tnu + Tnx)) + expected_xu_l.size() + expected_y_l.size(), Tph * Tnu).array() == -mpc::inf).all()); + REQUIRE((res.u.segment(((Tph + 1) * (Tnu + Tnx)) + expected_xu_u.size() + expected_y_u.size(), Tph * Tnu).array() == mpc::inf).all()); + } + + REQUIRE((res.l.tail(Tph).array() == -4).all()); + REQUIRE((res.u.tail(Tph).array() == 4).all()); +} + +TEST_CASE( + MPC_TEST_NAME("Linear output mapping"), + MPC_TEST_TAGS("[linear]")) +{ + constexpr int Tnx = 3; + constexpr int Tny = 6; + constexpr int Tnu = 0; + constexpr int Tndu = 7; + constexpr int Tph = 1; + constexpr int Tch = 1; + + mpc::ProblemBuilder builder; + builder.initialize(Tnx, Tnu, Tndu, Tny, Tph, Tch); + + mpc::mat Ad; + Ad.setZero(); + + mpc::mat Bd; + Bd.setZero(); + + mpc::mat Cd; + Cd.setRandom(); + + mpc::mat Dd; + Dd.setZero(); + + mpc::mat Bdv; + Bdv.setZero(); + + mpc::mat Ddv; + Ddv.setRandom(); + + builder.setStateModel(Ad, Bd, Cd); + builder.setExogenuosInput(Bdv, Ddv); + + mpc::cvec x; + x.setRandom(); + mpc::cvec du; + du.setRandom(); + + REQUIRE(builder.mapToOutput(x, du).isApprox(Cd * x + Ddv * du)); +} \ No newline at end of file diff --git a/test/test_mutiple_instances.cpp b/test/LMPC/test_mutiple_instances.cpp similarity index 100% rename from test/test_mutiple_instances.cpp rename to test/LMPC/test_mutiple_instances.cpp diff --git a/test/test_lmpc.cpp b/test/LMPC/test_quadrotor.cpp similarity index 62% rename from test/test_lmpc.cpp rename to test/LMPC/test_quadrotor.cpp index 557ed9c..5437234 100644 --- a/test/test_lmpc.cpp +++ b/test/LMPC/test_quadrotor.cpp @@ -7,30 +7,30 @@ #include TEST_CASE( - MPC_TEST_NAME("Linear example"), + MPC_TEST_NAME("Linear quadrotor example"), MPC_TEST_TAGS("[linear]")) { - constexpr int num_states = 12; - constexpr int num_output = 12; - constexpr int num_inputs = 4; - constexpr int num_dinputs = 4; - constexpr int pred_hor = 10; - constexpr int ctrl_hor = 10; + constexpr int Tnx = 12; + constexpr int Tny = 12; + constexpr int Tnu = 4; + constexpr int Tndu = 4; + constexpr int Tph = 10; + constexpr int Tch = 10; #ifdef MPC_DYNAMIC mpc::LMPC<> optsolver( - num_states, num_inputs, num_dinputs, num_output, - pred_hor, ctrl_hor); + Tnx, Tnu, Tndu, Tny, + Tph, Tch); #else mpc::LMPC< - TVAR(num_states), TVAR(num_inputs), TVAR(num_dinputs), TVAR(num_output), - TVAR(pred_hor), TVAR(ctrl_hor)> + TVAR(Tnx), TVAR(Tnu), TVAR(Tndu), TVAR(Tny), + TVAR(Tph), TVAR(Tch)> optsolver; #endif optsolver.setLoggerLevel(mpc::Logger::log_level::NONE); - mpc::mat Ad; + mpc::mat Ad; Ad << 1, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, @@ -44,7 +44,7 @@ TEST_CASE( 0, -0.9734, 0, 0, 0, 0, 0, -0.0488, 0, 0, 0.9846, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.9846; - mpc::mat Bd; + mpc::mat Bd; Bd << 0, -0.0726, 0, 0.0726, -0.0726, 0, 0.0726, 0, -0.0152, 0.0152, -0.0152, 0.0152, @@ -58,35 +58,35 @@ TEST_CASE( 0.0236, 0, -0.0236, 0, 0.2107, 0.2107, 0.2107, 0.2107; - mpc::mat Cd; + mpc::mat Cd; Cd.setIdentity(); - mpc::mat Dd; + mpc::mat Dd; Dd.setZero(); optsolver.setStateSpaceModel(Ad, Bd, Cd); optsolver.setDisturbances( - mpc::mat::Zero(), - mpc::mat::Zero()); + mpc::mat::Zero(), + mpc::mat::Zero()); - mpc::mat InputWMat, DeltaInputWMat; - mpc::mat OutputWMat; + mpc::mat InputWMat, DeltaInputWMat; + mpc::mat OutputWMat; REQUIRE(optsolver.setObjectiveWeights(OutputWMat, InputWMat, DeltaInputWMat)); - mpc::cvec InputW, DeltaInputW; - mpc::cvec OutputW; + mpc::cvec InputW, DeltaInputW; + mpc::cvec OutputW; OutputW << 0, 0, 10, 10, 10, 10, 0, 0, 0, 5, 5, 5; InputW << 0.1, 0.1, 0.1, 0.1; DeltaInputW << 0, 0, 0, 0; - REQUIRE(optsolver.setObjectiveWeights(OutputW, InputW, DeltaInputW, {0, pred_hor})); + REQUIRE(optsolver.setObjectiveWeights(OutputW, InputW, DeltaInputW, {0, Tph})); - mpc::mat xminmat, xmaxmat; - mpc::mat yminmat, ymaxmat; - mpc::mat uminmat, umaxmat; + mpc::mat xminmat, xmaxmat; + mpc::mat yminmat, ymaxmat; + mpc::mat uminmat, umaxmat; xminmat.setZero(); xmaxmat.setZero(); @@ -97,48 +97,50 @@ TEST_CASE( REQUIRE(optsolver.setConstraints(xminmat, uminmat, yminmat, xmaxmat, umaxmat, ymaxmat)); - mpc::cvec xmin, xmax; + mpc::cvec xmin, xmax; xmin << -M_PI / 6, -M_PI / 6, -mpc::inf, -mpc::inf, -mpc::inf, -1, -mpc::inf, -mpc::inf, -mpc::inf, -mpc::inf, -mpc::inf, -mpc::inf; xmax << M_PI / 6, M_PI / 6, mpc::inf, mpc::inf, mpc::inf, mpc::inf, mpc::inf, mpc::inf, mpc::inf, mpc::inf, mpc::inf, mpc::inf; - mpc::cvec ymin, ymax; + mpc::cvec ymin, ymax; ymin.setOnes(); ymin *= -mpc::inf; ymax.setOnes(); ymax *= mpc::inf; - mpc::cvec umin, umax; + mpc::cvec umin, umax; double u0 = 10.5916; umin << 9.6, 9.6, 9.6, 9.6; umin.array() -= u0; umax << 13, 13, 13, 13; umax.array() -= u0; - REQUIRE(optsolver.setConstraints(xmin, umin, ymin, xmax, umax, ymax, {0, pred_hor})); + REQUIRE(optsolver.setConstraints(xmin, umin, ymin, xmax, umax, ymax, {0, Tph})); REQUIRE(optsolver.setConstraints(xmin, umin, ymin, xmax, umax, ymax, {0, 1})); + + REQUIRE(optsolver.setScalarConstraint(-mpc::inf, mpc::inf, mpc::cvec::Ones(), mpc::cvec::Ones(), {-1, -1})); + REQUIRE(optsolver.setScalarConstraint(0, -mpc::inf, mpc::inf, mpc::cvec::Ones(), mpc::cvec::Ones())); - REQUIRE(optsolver.setReferences(mpc::mat::Zero(), mpc::mat::Zero(), mpc::mat::Zero())); + REQUIRE(optsolver.setReferences(mpc::mat::Zero(), mpc::mat::Zero(), mpc::mat::Zero())); - mpc::cvec yRef; + mpc::cvec yRef; yRef << 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0; - REQUIRE(optsolver.setReferences(yRef, mpc::cvec::Zero(), mpc::cvec::Zero(), {0, pred_hor})); + REQUIRE(optsolver.setReferences(yRef, mpc::cvec::Zero(), mpc::cvec::Zero(), {0, Tph})); mpc::LParameters params; params.maximum_iteration = 250; optsolver.setOptimizerParameters(params); - REQUIRE(optsolver.setExogenuosInputs(mpc::mat::Zero())); - REQUIRE(optsolver.setExogenuosInputs(mpc::cvec::Zero(), {0, pred_hor})); + REQUIRE(optsolver.setExogenuosInputs(mpc::mat::Zero())); + REQUIRE(optsolver.setExogenuosInputs(mpc::cvec::Zero(), {0, Tph})); - auto res = optsolver.step(mpc::cvec::Zero(), mpc::cvec::Zero()); + auto res = optsolver.step(mpc::cvec::Zero(), mpc::cvec::Zero()); auto seq = optsolver.getOptimalSequence(); - (void) seq; - + (void)seq; + mpc::cvec<4> testRes; testRes << -0.9916, 1.74839, -0.9916, 1.74839; - REQUIRE(res.cmd.isApprox(testRes, 1e-4)); } \ No newline at end of file diff --git a/test/test_common.cpp b/test/NLMPC/test_common.cpp similarity index 100% rename from test/test_common.cpp rename to test/NLMPC/test_common.cpp diff --git a/test/test_constraints.cpp b/test/NLMPC/test_constraints.cpp similarity index 100% rename from test/test_constraints.cpp rename to test/NLMPC/test_constraints.cpp diff --git a/test/test_discrete_lti_siso.cpp b/test/NLMPC/test_discrete_lti_siso.cpp similarity index 100% rename from test/test_discrete_lti_siso.cpp rename to test/NLMPC/test_discrete_lti_siso.cpp diff --git a/test/test_objective.cpp b/test/NLMPC/test_objective.cpp similarity index 100% rename from test/test_objective.cpp rename to test/NLMPC/test_objective.cpp diff --git a/test/test_vanderpol.cpp b/test/NLMPC/test_vanderpol.cpp similarity index 100% rename from test/test_vanderpol.cpp rename to test/NLMPC/test_vanderpol.cpp