Skip to content

Commit

Permalink
Merge branch 'fix-jacobian' into 'master'
Browse files Browse the repository at this point in the history 
Version 0.6.2 release

See merge request altairLab/optcontrol/libmpc!49
  • Loading branch information
@nicolapiccinelli
nicolapiccinelli committed Jul 24, 2024
2 parents 37b4d87 + 793757a commit 5e4b3a0
Show file tree
Hide file tree
Showing 12 changed files with 217 additions and 91 deletions.
7 changes: 7 additions & 0 deletions CHANGELOG.md
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,12 @@
# Changelog

## [0.6.2] - 2024-07-24
### Added
- The `Result` struct now contains the feasibility of the solution vector in the `is_feasible` field

### Fixed
- Version 0.6 and 0.6.1 were affected by a bug which was preventing the proper computation of the Jacobian of the user-defined inequality constraints. This bug has been fixed in this version

## [0.6.1] - 2024-06-07
### Fixed
- Fixed the horizon slicing for the non-linear mpc. The horizon slicing was not working properly when set via the HorizonSlice struct
Expand Down
2 changes: 1 addition & 1 deletion docs/source/conf.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@
# The short X.Y version
version = u''
# The full version, including alpha/beta/rc tags
release = u'0.6.1'
release = u'0.6.2'


# -- General configuration ---------------------------------------------------
Expand Down
2 changes: 2 additions & 0 deletions docs/source/manual/manual.rst
View file
Original file line number Diff line number Diff line change
Expand Up @@ -144,6 +144,7 @@ Optimization result
The optimization result is stored in the **Result** structure. The structure contains the following fields:

* solver_status: the return code of the optimization solver
* is_feasible: a boolean flag that indicates if the optimal solution is feasible
* solver_status_msg: the status message of the optimization solver
* cost: the optimal cost of the optimization problem
* status: the status of the MPC
Expand All @@ -154,6 +155,7 @@ The optimization result is stored in the **Result** structure. The structure con
Result<Tnu> res = ctrl.optimize(mpc::cvec<Tnx>::Zero(), mpc::cvec<Tnu>::Zero());

std::cout << "Solver status code: " << res.solver_status << std::endl;
std::cout << "Is feasible: " << res.is_feasible << std::endl;
std::cout << "Solver status message: " << res.solver_status_msg << std::endl;
std::cout << "Cost: " << res.cost << std::endl;
std::cout << "Status: " << res.status << std::endl;
Expand Down
1 change: 1 addition & 0 deletions include/mpc/IMPC.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -158,6 +158,7 @@ namespace mpc
<< "Optimization step completed" << std::endl
<< "duration: " << duration_s.count() << " (sec)" << std::endl
<< "status: " << optPtr->result.status << " (opt code: " << optPtr->result.solver_status << ")" << std::endl
<< "feasibility: " << optPtr->result.is_feasible << std::endl
<< "status message: " << optPtr->result.solver_status_msg << std::endl
<< "cost: " << optPtr->result.cost << std::endl;

Expand Down
1 change: 1 addition & 0 deletions include/mpc/LMPC/LOptimizer.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -341,6 +341,7 @@ namespace mpc
r.cmd = sequence.input.row(0);
r.solver_status = work->info->status_val;
r.cost = work->info->obj_val;
r.is_feasible = work->info->status_val == OSQP_SOLVED || work->info->status_val == OSQP_SOLVED_INACCURATE || work->info->status_val == OSQP_MAX_ITER_REACHED;
// convert the return code from the optimizer to the result status
r.status = convertToResultStatus(r.solver_status);
}
Expand Down
4 changes: 2 additions & 2 deletions include/mpc/NLMPC.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -231,7 +231,7 @@ namespace mpc
cvec<Tineq> tol_vec;
tol_vec = cvec<Tineq>::Ones(ineq());

auto res = conF->setIneqConstraints(handle);
auto res = conF->setIneqConstraints(handle, tol);
((NLOptimizer<MPCSize(Tnx, Tnu, 0, Tny, Tph, Tch, Tineq, Teq)> *)optPtr)->bindUserIneq(constraints_type::UINEQ, tol_vec * tol);
return res;
}
Expand All @@ -253,7 +253,7 @@ namespace mpc
cvec<Teq> tol_vec;
tol_vec = cvec<Teq>::Ones(Size(Teq));

auto res = conF->setEqConstraints(handle);
auto res = conF->setEqConstraints(handle, tol);
((NLOptimizer<MPCSize(Tnx, Tnu, 0, Tny, Tph, Tch, Tineq, Teq)> *)optPtr)->bindUserEq(constraints_type::UEQ, tol_vec * tol);
return res;
}
Expand Down
180 changes: 115 additions & 65 deletions include/mpc/NLMPC/Constraints.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,9 +111,11 @@ namespace mpc
* @return false
*/
bool setIneqConstraints(
const typename Base<sizer>::IConFunHandle handle)
const typename Base<sizer>::IConFunHandle handle, const float& tolerance)
{
checkOrQuit();

ieq_tolerance = tolerance;
return ieqUser = handle, true;
}

Expand All @@ -125,12 +127,68 @@ namespace mpc
* @return false
*/
bool setEqConstraints(
const typename Base<sizer>::EConFunHandle handle)
const typename Base<sizer>::EConFunHandle handle, const float& tolerance)
{
checkOrQuit();

eq_tolerance = tolerance;
return eqUser = handle, true;
}

/**
* @brief Evaluate the feasibility of the optimization vector
*
* @param x optimization vector
* @return true if the vector is feasible
* @return false if the vector is not feasible
*/
bool isFeasible(
const Eigen::Matrix<double, (sizer.ph * sizer.nx + sizer.nu * sizer.ch + 1), 1> &x)
{
checkOrQuit();
mapping->unwrapVector(x, x0, Xmat, Umat, e);

if (hasIneqConstraints())
{
Eigen::Matrix<double, sizer.ph + 1, sizer.ny> Ymat = model->getOutput(Xmat, Umat);
ieqUser(cineq_user, Xmat, Ymat, Umat, e);

// print the vector of inequality constraints
Logger::instance().log(Logger::log_type::DETAIL)
<< "User inequality constraints value (feasibility):\n"
<< std::setprecision(10)
<< cineq_user
<< std::endl;

// Check if all inequality constraints are satisfied (<= 0)
if ((cineq_user.array() > ieq_tolerance).any())
{
return false;
}

}

if (hasEqConstraints())
{
eqUser(ceq_user, Xmat, Umat);

// print the vector of equality constraints
Logger::instance().log(Logger::log_type::DETAIL)
<< "User equality constraints value (feasibility):\n"
<< std::setprecision(10)
<< ceq_user
<< std::endl;

// Check if all equality constraints are satisfied (== 0)
if (ceq_user.array().abs().maxCoeff() > eq_tolerance)
{
return false;
}
}

return true;
}

/**
* @brief Evaluate the user defined inequality constraints
*
Expand All @@ -148,6 +206,8 @@ namespace mpc

if (hasIneqConstraints())
{
Logger::instance().log(Logger::log_type::DETAIL) << "User inequality constraints detected" << std::endl;

// check if the output function of the system is defined
// if so, let's compute the output along the horizon
mat<(sizer.ph + 1), sizer.ny> Ymat = model->getOutput(Xmat, Umat);
Expand All @@ -170,6 +230,24 @@ namespace mpc
Umat,
e);

Logger::instance().log(Logger::log_type::DETAIL)
<< "User inequality state constraints gradient:\n"
<< std::setprecision(10)
<< Jieqx
<< std::endl;

Logger::instance().log(Logger::log_type::DETAIL)
<< "User inequality inputs constraints gradient:\n"
<< std::setprecision(10)
<< Jieqmv
<< std::endl;

Logger::instance().log(Logger::log_type::DETAIL)
<< "User inequality slack constraints gradient:\n"
<< std::setprecision(10)
<< Jie
<< std::endl;

glueJacobian<sizer.ineq>(
Jcineq_user,
Jieqx,
Expand All @@ -186,6 +264,7 @@ namespace mpc
{
scaled_Jcineq_user(ioff + ix, j) = scaled_Jcineq_user(ioff + ix, j) * mapping->StateScaling()(ix);
}

ioff = ioff + nx();
}
}
Expand All @@ -194,6 +273,8 @@ namespace mpc
}
else
{
Logger::instance().log(Logger::log_type::DETAIL) << "No user inequality constraints detected" << std::endl;

cineq_user.setZero();
Jcineq_user.setZero();
}
Expand All @@ -209,6 +290,7 @@ namespace mpc
<< std::setprecision(10)
<< c.value
<< std::endl;

if (!hasGradient)
{
Logger::instance().log(Logger::log_type::DETAIL)
Expand Down Expand Up @@ -290,6 +372,8 @@ namespace mpc
// Add user defined constraints
if (hasEqConstraints())
{
Logger::instance().log(Logger::log_type::DETAIL) << "User equality constraints detected" << std::endl;

eqUser(ceq_user, Xmat, Umat);

mat<sizer.eq, (sizer.ph * sizer.nx)> Jeqx;
Expand Down Expand Up @@ -328,6 +412,8 @@ namespace mpc
}
else
{
Logger::instance().log(Logger::log_type::DETAIL) << "No user equality constraints detected" << std::endl;

ceq_user.setZero();
Jceq_user.setZero();
}
Expand Down Expand Up @@ -573,102 +659,65 @@ namespace mpc
for (size_t j = 0; j < nx(); j++)
{
int ix = i + 1;
double dx = dv * Xa(ix, j); // Use Xa instead of Xa.array() to access element
double dx = dv * Xa.array()(j);
x0(ix, j) += dx;

cvec<sizer.ineq> f_plus, f_minus;
COND_RESIZE_CVEC(sizer,f_plus, ineq());
COND_RESIZE_CVEC(sizer,f_minus, ineq());
COND_RESIZE_CVEC(sizer, f_plus, ineq());
COND_RESIZE_CVEC(sizer, f_minus, ineq());

mat<(sizer.ph + 1), sizer.ny> y0_plus = model->getOutput(x0, u0);
ieqUser(f_plus, x0, y0_plus, u0, e0);

mat<(sizer.ph + 1), sizer.ny> y_plus, y_minus;
y_plus = model->getOutput(x0, u0);
x0(ix, j) -= 2 * dx;
y_minus = model->getOutput(x0, u0);
x0(ix, j) += dx;
mat<(sizer.ph + 1), sizer.ny> y0_minus = model->getOutput(x0, u0);
ieqUser(f_minus, x0, y0_minus, u0, e0);

ieqUser(f_plus, x0, y_plus, u0, e0);
ieqUser(f_minus, x0, y_minus, u0, e0);
x0(ix, j) += dx;

cvec<sizer.ineq> df;
df = (f_plus - f_minus) / (2 * dx);
cvec<sizer.ineq> df = (f_plus - f_minus) / (2 * dx);
Jconx.middleCols(i * nx(), nx()).col(j) = df;

// Restore the original value of x0(ix, j)
x0(ix, j) -= dx;
}
}

mat<(sizer.ph + 1), sizer.nu> Ua;
Ua = u0.cwiseAbs().cwiseMax(1.0);

for (size_t i = 0; i < (ph() - 1); i++)
for (size_t i = 0; i < ph(); i++)
{
for (size_t j = 0; j < nu(); j++)
{
double du = dv * Ua(ph() - 1, j); // Access Ua at the correct index
double du = dv * Ua.array()(j);
u0(i, j) += du;

cvec<sizer.ineq> f_plus, f_minus;
COND_RESIZE_CVEC(sizer,f_plus, ineq());
COND_RESIZE_CVEC(sizer,f_minus, ineq());
COND_RESIZE_CVEC(sizer, f_plus, ineq());
COND_RESIZE_CVEC(sizer, f_minus, ineq());

mat<(sizer.ph + 1), sizer.ny> y0_plus = model->getOutput(x0, u0);
ieqUser(f_plus, x0, y0_plus, u0, e0);

mat<(sizer.ph + 1), sizer.ny> y_plus, y_minus;
y_plus = model->getOutput(x0, u0);
u0(i, j) -= 2 * du;
y_minus = model->getOutput(x0, u0);
u0(i, j) += du;
mat<(sizer.ph + 1), sizer.ny> y0_minus = model->getOutput(x0, u0);
ieqUser(f_minus, x0, y0_minus, u0, e0);

ieqUser(f_plus, x0, y_plus, u0, e0);
ieqUser(f_minus, x0, y_minus, u0, e0);
u0(i, j) += du;

cvec<sizer.ineq> df;
df = (f_plus - f_minus) / (2 * du);
cvec<sizer.ineq> df = (f_plus - f_minus) / (2 * du);
Jconmv.middleCols(i * nu(), nu()).col(j) = df;

// Restore the original value of u0(i, j)
u0(i, j) -= du;
}
}

for (size_t j = 0; j < nu(); j++)
{
double du = dv * Ua(ph() - 1, j); // Access Ua at the correct index
u0(ph() - 1, j) += du;
u0(ph(), j) += du;

cvec<sizer.ineq> f_plus, f_minus;
COND_RESIZE_CVEC(sizer,f_plus, ineq());
COND_RESIZE_CVEC(sizer,f_minus, ineq());

mat<(sizer.ph + 1), sizer.ny> y_plus, y_minus;
y_plus = model->getOutput(x0, u0);
u0(ph() - 1, j) -= 2 * du;
u0(ph(), j) -= 2 * du;
y_minus = model->getOutput(x0, u0);
u0(ph() - 1, j) += du;
u0(ph(), j) += du;

ieqUser(f_plus, x0, y_plus, u0, e0);
ieqUser(f_minus, x0, y_minus, u0, e0);

cvec<sizer.ineq> df;
df = (f_plus - f_minus) / (2 * du);
Jconmv.middleCols(((ph() - 1) * nu()), nu()).col(j) = df;
}

double ea = fmax(1e-6, abs(e0));
double ea = fmax(dv, fabs(e0));
double de = ea * dv;
cvec<sizer.ineq> f1;
COND_RESIZE_CVEC(sizer,f1, ineq());
cvec<sizer.ineq> f1, f2;
COND_RESIZE_CVEC(sizer, f1, ineq());
COND_RESIZE_CVEC(sizer, f2, ineq());

mat<(sizer.ph + 1), sizer.ny> y0 = model->getOutput(x0, u0);
ieqUser(f1, x0, y0, u0, e0 + de);

cvec<sizer.ineq> f2;
COND_RESIZE_CVEC(sizer,f2, ineq());

y0 = model->getOutput(x0, u0);
ieqUser(f2, x0, y0, u0, e0 - de);

Jcone = (f1 - f2) / (2 * de);
}

Expand Down Expand Up @@ -880,5 +929,6 @@ namespace mpc
using Base<sizer>::niteration;

const double dv = sqrt(std::numeric_limits<double>::epsilon());
double ieq_tolerance, eq_tolerance;
};
} // namespace mpc
Loading

0 comments on commit 5e4b3a0

Please sign in to comment.