Skip to content

GitLab

Commit f7875e67 authored by Olivier Stasse's avatar Olivier Stasse Committed by olivier stasse
Browse files

clang-format using gepetto linters.

parent 9812f715
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ f7875e67
......@@ -50,6 +50,7 @@ SET(${PROJECT_NAME}_HEADERS
include/${PROJECT_NAME}/eiquadprog-fast.hpp
include/${PROJECT_NAME}/eiquadprog-rt.hpp
include/${PROJECT_NAME}/eiquadprog-rt.hxx
include/${PROJECT_NAME}/eiquadprog-utils.hxx
)
ADD_LIBRARY(${PROJECT_NAME} SHARED
......
include/eiquadprog/eiquadprog-fast.hpp
View file @ f7875e67
......@@ -66,7 +66,7 @@ namespace solvers {
/**
* Possible states of the solver.
*/
enum EiquadprogFast_status {
enum EiquadprogFast_status {
EIQUADPROG_FAST_OPTIMAL = 0,
EIQUADPROG_FAST_INFEASIBLE = 1,
EIQUADPROG_FAST_UNBOUNDED = 2,
......@@ -230,5 +230,4 @@ class EiquadprogFast {
} /* namespace solvers */
} /* namespace eiquadprog */
#endif /* EIQUADPROGFAST_HPP_ */
include/eiquadprog/eiquadprog-rt.hpp
View file @ f7875e67
......@@ -141,9 +141,8 @@ class RtEiquadprog {
const typename RtMatrixX<nEqCon, nVars>::d& CE,
const typename RtVectorX<nEqCon>::d& ce0,
const typename RtMatrixX<nIneqCon, nVars>::d& CI,
const typename RtVectorX<nIneqCon>::d& ci0,
typename RtVectorX<nVars>::d& x);
const typename RtVectorX<nIneqCon>::d& ci0, typename RtVectorX<nVars>::d& x);
typename RtMatrixX<nVars, nVars>::d m_J; // J * J' = Hessian
bool is_inverse_provided_;
......@@ -246,11 +245,11 @@ class RtEiquadprog {
}
bool add_constraint(typename RtMatrixX<nVars, nVars>::d& R, typename RtMatrixX<nVars, nVars>::d& J,
typename RtVectorX<nVars>::d& d, int& iq, double& R_norm);
typename RtVectorX<nVars>::d& d, int& iq, double& R_norm);
void delete_constraint(typename RtMatrixX<nVars, nVars>::d& R, typename RtMatrixX<nVars, nVars>::d& J,
typename RtVectorX<nIneqCon + nEqCon>::i& A,
typename RtVectorX<nIneqCon + nEqCon>::d& u, int& iq, int l);
typename RtVectorX<nIneqCon + nEqCon>::i& A, typename RtVectorX<nIneqCon + nEqCon>::d& u,
int& iq, int l);
};
} /* namespace solvers */
......@@ -259,5 +258,4 @@ class RtEiquadprog {
#include "eiquadprog/eiquadprog-rt.hxx"
/* --- Details -------------------------------------------------------------------- */
#endif /* __eiquadprog_rt_hpp__ */
include/eiquadprog/eiquadprog-rt.hxx
View file @ f7875e67
......@@ -26,8 +26,7 @@ namespace eiquadprog {
namespace solvers {
template <int nVars, int nEqCon, int nIneqCon>
RtEiquadprog<nVars, nEqCon, nIneqCon>::RtEiquadprog() :
solver_return_(std::numeric_limits<double>::infinity()) {
RtEiquadprog<nVars, nEqCon, nIneqCon>::RtEiquadprog() : solver_return_(std::numeric_limits<double>::infinity()) {
m_maxIter = DEFAULT_MAX_ITER;
q = 0; // size of the active set A
// (containing the indices of the active constraints)
......@@ -38,10 +37,9 @@ template <int nVars, int nEqCon, int nIneqCon>
RtEiquadprog<nVars, nEqCon, nIneqCon>::~RtEiquadprog() {}
template <int nVars, int nEqCon, int nIneqCon>
bool RtEiquadprog<nVars, nEqCon, nIneqCon>::
add_constraint(typename RtMatrixX<nVars, nVars>::d& R,
typename RtMatrixX<nVars, nVars>::d& J,
typename RtVectorX<nVars>::d& d, int& iq, double& R_norm) {
bool RtEiquadprog<nVars, nEqCon, nIneqCon>::add_constraint(typename RtMatrixX<nVars, nVars>::d& R,
typename RtMatrixX<nVars, nVars>::d& J,
typename RtVectorX<nVars>::d& d, int& iq, double& R_norm) {
// int n=J.rows();
#ifdef TRACE_SOLVER
std::cerr << "Add constraint " << iq << '/';
......
include/eiquadprog/eiquadprog.hpp
View file @ f7875e67
......@@ -88,52 +88,35 @@
namespace eiquadprog {
namespace solvers {
inline void compute_d(Eigen::VectorXd& d, const Eigen::MatrixXd& J,
const Eigen::VectorXd& np)
{ d = J.adjoint() * np; }
inline void compute_d(Eigen::VectorXd& d, const Eigen::MatrixXd& J, const Eigen::VectorXd& np) {
d = J.adjoint() * np;
}
inline void update_z(Eigen::VectorXd& z, const Eigen::MatrixXd& J,
const Eigen::VectorXd& d,
size_t iq) {
inline void update_z(Eigen::VectorXd& z, const Eigen::MatrixXd& J, const Eigen::VectorXd& d, size_t iq) {
z = J.rightCols(z.size() - iq) * d.tail(d.size() - iq);
}
inline void update_r(const Eigen::MatrixXd& R, Eigen::VectorXd& r,
const Eigen::VectorXd& d,
size_t iq) {
r.head(iq) = R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().
solve(d.head(iq));
inline void update_r(const Eigen::MatrixXd& R, Eigen::VectorXd& r, const Eigen::VectorXd& d, size_t iq) {
r.head(iq) = R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solve(d.head(iq));
}
bool add_constraint(Eigen::MatrixXd& R, Eigen::MatrixXd& J,
Eigen::VectorXd& d,
size_t& iq, double& R_norm);
void delete_constraint(Eigen::MatrixXd& R, Eigen::MatrixXd& J,
Eigen::VectorXi& A,
Eigen::VectorXd& u, size_t p, size_t& iq, size_t l);
bool add_constraint(Eigen::MatrixXd& R, Eigen::MatrixXd& J, Eigen::VectorXd& d, size_t& iq, double& R_norm);
void delete_constraint(Eigen::MatrixXd& R, Eigen::MatrixXd& J, Eigen::VectorXi& A, Eigen::VectorXd& u, size_t p,
size_t& iq, size_t l);
/* solve_quadprog2 is used when the Cholesky decomposition of the G
matrix is precomputed */
double solve_quadprog2(Eigen::LLT<Eigen::MatrixXd, Eigen::Lower>& chol,
double c1,
Eigen::VectorXd& g0, const Eigen::MatrixXd& CE,
const Eigen::VectorXd& ce0,
const Eigen::MatrixXd& CI, const Eigen::VectorXd& ci0,
Eigen::VectorXd& x,
Eigen::VectorXi& A, size_t& q);
double solve_quadprog2(Eigen::LLT<Eigen::MatrixXd, Eigen::Lower>& chol, double c1, Eigen::VectorXd& g0,
const Eigen::MatrixXd& CE, const Eigen::VectorXd& ce0, const Eigen::MatrixXd& CI,
const Eigen::VectorXd& ci0, Eigen::VectorXd& x, Eigen::VectorXi& A, size_t& q);
/* solve_quadprog is used for on-demand QP solving */
double solve_quadprog(Eigen::MatrixXd& G,
Eigen::VectorXd& g0,
const Eigen::MatrixXd& CE,
const Eigen::VectorXd& ce0,
const Eigen::MatrixXd& CI,
const Eigen::VectorXd& ci0, Eigen::VectorXd& x,
Eigen::VectorXi& activeSet, size_t& activeSetSize);
double solve_quadprog(Eigen::MatrixXd& G, Eigen::VectorXd& g0, const Eigen::MatrixXd& CE, const Eigen::VectorXd& ce0,
const Eigen::MatrixXd& CI, const Eigen::VectorXd& ci0, Eigen::VectorXd& x,
Eigen::VectorXi& activeSet, size_t& activeSetSize);
// }
}
}
} // namespace solvers
} // namespace eiquadprog
#endif
src/eiquadprog-fast.cpp
View file @ f7875e67
#include <iostream>
#include "eiquadprog/eiquadprog-fast.hpp"
#define TRACE_SOLVER
namespace eiquadprog {
namespace solvers {
EiquadprogFast::EiquadprogFast() {
m_maxIter = DEFAULT_MAX_ITER;
q = 0; // size of the active set A (containing the indices of the active constraints)
q = 0; // size of the active set A (containing the indices
// of the active constraints)
is_inverse_provided_ = false;
m_nVars = 0;
m_nEqCon = 0;
......@@ -182,7 +185,6 @@ void EiquadprogFast::delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, Ve
}
}
EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const VectorXd& g0, const MatrixXd& CE,
const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0,
VectorXd& x) {
......@@ -232,7 +234,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
R.setZero(nVars, nVars);
R_norm = 1.0;
/* compute the inverse of the factorized matrix Hess^-1, this is the initial value for H */
/* compute the inverse of the factorized matrix Hess^-1,
this is the initial value for H */
// m_J = L^-T
if (!is_inverse_provided_) {
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOLESKY_INVERSE);
......@@ -253,7 +256,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
/* c1 * c2 is an estimate for cond(Hess) */
/*
* Find the unconstrained minimizer of the quadratic form 0.5 * x Hess x + g0 x
* Find the unconstrained minimizer of the quadratic
* form 0.5 * x Hess x + g0 x
* this is a feasible point in the dual space
* x = Hess^-1 * g0
*/
......@@ -296,10 +300,12 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
print_vector("d", d, nVars);
#endif
/* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
becomes feasible */
/* compute full step length t2: i.e.,
the minimum step in primal space s.t. the contraint
becomes feasible */
t2 = 0.0;
if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon())
// i.e. z != 0
t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
x += t2 * z;
......@@ -326,7 +332,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
for (i = 0; i < nIneqCon; i++) iai(i) = static_cast<VectorXi::Scalar>(i);
#ifdef USE_WARM_START
// DEBUG_STREAM("Gonna warm start using previous active set:\n"<<A.transpose()<<"\n")
// DEBUG_STREAM("Gonna warm start using previous active
// set:\n"<<A.transpose()<<"\n")
for (i = nEqCon; i < q; i++) {
iai(i - nEqCon) = -1;
ip = A(i);
......@@ -335,8 +342,9 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
update_z(z, m_J, d, iq);
update_r(R, r, d, iq);
/* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
becomes feasible */
/* compute full step length t2: i.e.,
the minimum step in primal space s.t. the contraint
becomes feasible */
t2 = 0.0;
if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = (-np.dot(x) - ci0(ip)) / z.dot(np);
......@@ -408,7 +416,8 @@ l1:
if (std::abs(psi) <= static_cast<double>(nIneqCon) * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) {
/* numerically there are not infeasibilities anymore */
q = iq;
// DEBUG_STREAM("Optimal active set:\n"<<A.head(iq).transpose()<<"\n\n")
// DEBUG_STREAM("Optimal active
// set:\n"<<A.head(iq).transpose()<<"\n\n")
return EIQUADPROG_FAST_OPTIMAL;
}
......@@ -419,7 +428,8 @@ l1:
l2: /* Step 2: check for feasibility and determine a new S-pair */
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2);
// find constraint with highest violation (what about normalizing constraints?)
// find constraint with highest violation
// (what about normalizing constraints?)
for (i = 0; i < nIneqCon; i++) {
if (s(i) < ss && iai(i) != -1 && iaexcl(i)) {
ss = s(i);
......@@ -449,7 +459,8 @@ l2: /* Step 2: check for feasibility and determine a new S-pair */
l2a: /* Step 2a: determine step direction */
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A);
/* compute z = H np: the step direction in the primal space (through m_J, see the paper) */
/* compute z = H np: the step direction in the primal space
(through m_J, see the paper) */
compute_d(d, m_J, np);
// update_z(z, m_J, d, iq);
if (iq >= nVars) {
......@@ -458,7 +469,8 @@ l2a: /* Step 2a: determine step direction */
} else {
update_z(z, m_J, d, iq);
}
/* compute N* np (if q > 0): the negative of the step direction in the dual space */
/* compute N* np (if q > 0): the negative of the
step direction in the dual space */
update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
std::cerr << "Step direction z" << std::endl;
......@@ -473,7 +485,8 @@ l2a: /* Step 2a: determine step direction */
/* Step 2b: compute step length */
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B);
l = 0;
/* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
/* Compute t1: partial step length (maximum step in dual
space without violating dual feasibility */
t1 = inf; /* +inf */
/* find the index l s.t. it reaches the minimum of u+(x) / r */
// l: index of constraint to drop (maybe)
......@@ -484,8 +497,10 @@ l2a: /* Step 2a: determine step direction */
l = A(k);
}
}
/* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
/* Compute t2: full step length (minimum step in primal
space such that the constraint ip becomes feasible */
if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon())
// i.e. z != 0
t2 = -s(ip) / z.dot(np);
else
t2 = inf; /* +inf */
......@@ -591,5 +606,3 @@ l2a: /* Step 2a: determine step direction */
} /* namespace solvers */
} /* namespace eiquadprog */
src/eiquadprog.cpp
View file @ f7875e67
#include <eiquadprog/eiquadprog.hpp>
namespace eiquadprog{
namespace solvers{
namespace eiquadprog {
namespace solvers {
using namespace Eigen;
/* solve_quadprog is used for on-demand QP solving */
......@@ -22,9 +22,8 @@ double solve_quadprog(MatrixXd& G, VectorXd& g0, const MatrixXd& CE, const Vecto
* @param A Output vector containing the indexes of the active constraints.
* @param q Output value representing the size of the active set.
*/
double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, const MatrixXd& CE,
const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0, VectorXd& x, VectorXi& A,
size_t& q) {
double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0,
const MatrixXd& CI, const VectorXd& ci0, VectorXd& x, VectorXi& A, size_t& q) {
size_t i, k, l; /* indices */
size_t ip, me, mi;
size_t n = g0.size();
......@@ -438,6 +437,6 @@ void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_
}
}
}
}
}
} // namespace solvers
} // namespace eiquadprog
tests/TestA.cpp
View file @ f7875e67
......@@ -7,30 +7,18 @@
using namespace eiquadprog::solvers;
using namespace eiquadprog::tests;
A::A():
Q_(2, 2),
C_(2),
Aeq_(0,2),
Beq_(0),
Aineq_(0,2),
Bineq_(0),
QP_()
{
A::A() : Q_(2, 2), C_(2), Aeq_(0, 2), Beq_(0), Aineq_(0, 2), Bineq_(0), QP_() {
QP_.reset(2, 0, 0);
Q_.setZero();
Q_(0, 0) = 1.0;
Q_(1, 1) = 1.0;
C_.setZero();
expected_ = EIQUADPROG_FAST_OPTIMAL;
}
EiquadprogFast_status A::solve(Eigen::VectorXd &x)
{
EiquadprogFast_status A::solve(Eigen::VectorXd &x) {
return QP_.solve_quadprog(Q_, C_, Aeq_, Beq_, Aineq_, Bineq_, x);
}
tests/TestA.hpp
View file @ f7875e67
......@@ -8,7 +8,6 @@ namespace eiquadprog {
namespace tests {
class A {
protected:
eiquadprog::solvers::EiquadprogFast_status expected_;
Eigen::MatrixXd Q_;
......@@ -22,11 +21,10 @@ class A {
eiquadprog::solvers::EiquadprogFast QP_;
A();
eiquadprog::solvers::EiquadprogFast_status
solve( Eigen::VectorXd &x);
eiquadprog::solvers::EiquadprogFast_status solve(Eigen::VectorXd &x);
};
} /* namespace solvers */
} // namespace tests
} /* namespace eiquadprog */
#endif /* TEST_EIQUADPROG_CLASS_A_ */
tests/TestB.cpp
View file @ f7875e67
......@@ -8,40 +8,28 @@ using namespace eiquadprog::solvers;
namespace eiquadprog {
namespace tests {
B::B():
solution_(2)
{
solution_.setZero();
}
bool B::do_something()
{
B::B() : solution_(2) { solution_.setZero(); }
bool B::do_something() {
eiquadprog::solvers::EiquadprogFast_status expected = EIQUADPROG_FAST_OPTIMAL;
Eigen::VectorXd x(2);
eiquadprog::solvers::EiquadprogFast_status status = A_.solve(x);
bool rstatus=true;
if (status != expected)
{
std::cerr << "Status not to true for A_"
<< expected << " " << status
<< std::endl;
rstatus=false;
bool rstatus = true;
if (status != expected) {
std::cerr << "Status not to true for A_" << expected << " " << status << std::endl;
rstatus = false;
}
if (!x.isApprox(solution_))
{
std::cerr << "x!=solution : " << x << "!=" << solution_ << std::endl;
rstatus=false;
if (!x.isApprox(solution_)) {
std::cerr << "x!=solution : " << x << "!=" << solution_ << std::endl;
rstatus = false;
}
return rstatus;
}
}
}
} // namespace tests
} // namespace eiquadprog
tests/TestB.hpp
View file @ f7875e67
......@@ -7,16 +7,15 @@ namespace eiquadprog {
namespace tests {
class B {
protected:
Eigen::VectorXd solution_;
public:
A A_;
B();
bool do_something();
};
}
}
} // namespace tests
} // namespace eiquadprog
#endif /* TEST_EIQUADPROG_CLASS_B_ */
tests/eiquadprog-basic.cpp
View file @ f7875e67
......@@ -428,8 +428,7 @@ BOOST_AUTO_TEST_CASE(test_nonconvex) {
double val = -1.;
double out = eiquadprog::solvers::solve_quadprog
(Q, C, Aeq, Beq, Aineq, Bineq, x, activeSet, activeSetSize);
double out = eiquadprog::solvers::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x, activeSet, activeSetSize);
// DOES NOT WORK!?
BOOST_WARN_CLOSE(out, val, 1e-6);
......
tests/eiquadprog-fast.cpp
View file @ f7875e67
......@@ -66,8 +66,7 @@ BOOST_AUTO_TEST_CASE(test_unbiased) {
EiquadprogFast_status expected = EIQUADPROG_FAST_OPTIMAL;
EiquadprogFast_status status = qp.solve_quadprog(Q, C, Aeq, Beq,
Aineq, Bineq, x);
EiquadprogFast_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
BOOST_CHECK_EQUAL(status, expected);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment