Skip to content

Commit f7875e6

Browse files
olivier-stasseOlivier Stasse
olivier-stasse
authored and
Olivier Stasse
committed
clang-format using gepetto linters.
1 parent 9812f71 commit f7875e6

File tree

13 files changed

+93
-131
lines changed

13 files changed

+93
-131
lines changed

‎CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -50,6 +50,7 @@ SET(${PROJECT_NAME}_HEADERS
5050
include/${PROJECT_NAME}/eiquadprog-fast.hpp
5151
include/${PROJECT_NAME}/eiquadprog-rt.hpp
5252
include/${PROJECT_NAME}/eiquadprog-rt.hxx
53+
include/${PROJECT_NAME}/eiquadprog-utils.hxx
5354
)
5455

5556
ADD_LIBRARY(${PROJECT_NAME} SHARED

‎include/eiquadprog/eiquadprog-fast.hpp‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -66,7 +66,7 @@ namespace solvers {
6666
/**
6767
* Possible states of the solver.
6868
*/
69-
enum EiquadprogFast_status {
69+
enum EiquadprogFast_status {
7070
EIQUADPROG_FAST_OPTIMAL = 0,
7171
EIQUADPROG_FAST_INFEASIBLE = 1,
7272
EIQUADPROG_FAST_UNBOUNDED = 2,
@@ -230,5 +230,4 @@ class EiquadprogFast {
230230
} /* namespace solvers */
231231
} /* namespace eiquadprog */
232232

233-
234233
#endif /* EIQUADPROGFAST_HPP_ */

‎include/eiquadprog/eiquadprog-rt.hpp‎

Lines changed: 5 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -141,9 +141,8 @@ class RtEiquadprog {
141141
const typename RtMatrixX<nEqCon, nVars>::d& CE,
142142
const typename RtVectorX<nEqCon>::d& ce0,
143143
const typename RtMatrixX<nIneqCon, nVars>::d& CI,
144-
const typename RtVectorX<nIneqCon>::d& ci0,
145-
typename RtVectorX<nVars>::d& x);
146-
144+
const typename RtVectorX<nIneqCon>::d& ci0, typename RtVectorX<nVars>::d& x);
145+
147146
typename RtMatrixX<nVars, nVars>::d m_J; // J * J' = Hessian
148147
bool is_inverse_provided_;
149148

@@ -246,11 +245,11 @@ class RtEiquadprog {
246245
}
247246

248247
bool add_constraint(typename RtMatrixX<nVars, nVars>::d& R, typename RtMatrixX<nVars, nVars>::d& J,
249-
typename RtVectorX<nVars>::d& d, int& iq, double& R_norm);
248+
typename RtVectorX<nVars>::d& d, int& iq, double& R_norm);
250249

251250
void delete_constraint(typename RtMatrixX<nVars, nVars>::d& R, typename RtMatrixX<nVars, nVars>::d& J,
252-
typename RtVectorX<nIneqCon + nEqCon>::i& A,
253-
typename RtVectorX<nIneqCon + nEqCon>::d& u, int& iq, int l);
251+
typename RtVectorX<nIneqCon + nEqCon>::i& A, typename RtVectorX<nIneqCon + nEqCon>::d& u,
252+
int& iq, int l);
254253
};
255254

256255
} /* namespace solvers */
@@ -259,5 +258,4 @@ class RtEiquadprog {
259258
#include "eiquadprog/eiquadprog-rt.hxx"
260259
/* --- Details -------------------------------------------------------------------- */
261260

262-
263261
#endif /* __eiquadprog_rt_hpp__ */

‎include/eiquadprog/eiquadprog-rt.hxx‎

Lines changed: 4 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -26,8 +26,7 @@ namespace eiquadprog {
2626
namespace solvers {
2727

2828
template <int nVars, int nEqCon, int nIneqCon>
29-
RtEiquadprog<nVars, nEqCon, nIneqCon>::RtEiquadprog() :
30-
solver_return_(std::numeric_limits<double>::infinity()) {
29+
RtEiquadprog<nVars, nEqCon, nIneqCon>::RtEiquadprog() : solver_return_(std::numeric_limits<double>::infinity()) {
3130
m_maxIter = DEFAULT_MAX_ITER;
3231
q = 0; // size of the active set A
3332
// (containing the indices of the active constraints)
@@ -38,10 +37,9 @@ template <int nVars, int nEqCon, int nIneqCon>
3837
RtEiquadprog<nVars, nEqCon, nIneqCon>::~RtEiquadprog() {}
3938

4039
template <int nVars, int nEqCon, int nIneqCon>
41-
bool RtEiquadprog<nVars, nEqCon, nIneqCon>::
42-
add_constraint(typename RtMatrixX<nVars, nVars>::d& R,
43-
typename RtMatrixX<nVars, nVars>::d& J,
44-
typename RtVectorX<nVars>::d& d, int& iq, double& R_norm) {
40+
bool RtEiquadprog<nVars, nEqCon, nIneqCon>::add_constraint(typename RtMatrixX<nVars, nVars>::d& R,
41+
typename RtMatrixX<nVars, nVars>::d& J,
42+
typename RtVectorX<nVars>::d& d, int& iq, double& R_norm) {
4543
// int n=J.rows();
4644
#ifdef TRACE_SOLVER
4745
std::cerr << "Add constraint " << iq << '/';

‎include/eiquadprog/eiquadprog.hpp‎

Lines changed: 17 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -88,52 +88,35 @@
8888
namespace eiquadprog {
8989
namespace solvers {
9090

91-
inline void compute_d(Eigen::VectorXd& d, const Eigen::MatrixXd& J,
92-
const Eigen::VectorXd& np)
93-
{ d = J.adjoint() * np; }
91+
inline void compute_d(Eigen::VectorXd& d, const Eigen::MatrixXd& J, const Eigen::VectorXd& np) {
92+
d = J.adjoint() * np;
93+
}
9494

95-
inline void update_z(Eigen::VectorXd& z, const Eigen::MatrixXd& J,
96-
const Eigen::VectorXd& d,
97-
size_t iq) {
95+
inline void update_z(Eigen::VectorXd& z, const Eigen::MatrixXd& J, const Eigen::VectorXd& d, size_t iq) {
9896
z = J.rightCols(z.size() - iq) * d.tail(d.size() - iq);
9997
}
10098

101-
inline void update_r(const Eigen::MatrixXd& R, Eigen::VectorXd& r,
102-
const Eigen::VectorXd& d,
103-
size_t iq) {
104-
r.head(iq) = R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().
105-
solve(d.head(iq));
99+
inline void update_r(const Eigen::MatrixXd& R, Eigen::VectorXd& r, const Eigen::VectorXd& d, size_t iq) {
100+
r.head(iq) = R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solve(d.head(iq));
106101
}
107102

108-
bool add_constraint(Eigen::MatrixXd& R, Eigen::MatrixXd& J,
109-
Eigen::VectorXd& d,
110-
size_t& iq, double& R_norm);
111-
void delete_constraint(Eigen::MatrixXd& R, Eigen::MatrixXd& J,
112-
Eigen::VectorXi& A,
113-
Eigen::VectorXd& u, size_t p, size_t& iq, size_t l);
103+
bool add_constraint(Eigen::MatrixXd& R, Eigen::MatrixXd& J, Eigen::VectorXd& d, size_t& iq, double& R_norm);
104+
void delete_constraint(Eigen::MatrixXd& R, Eigen::MatrixXd& J, Eigen::VectorXi& A, Eigen::VectorXd& u, size_t p,
105+
size_t& iq, size_t l);
114106

115107
/* solve_quadprog2 is used when the Cholesky decomposition of the G
116108
matrix is precomputed */
117-
double solve_quadprog2(Eigen::LLT<Eigen::MatrixXd, Eigen::Lower>& chol,
118-
double c1,
119-
Eigen::VectorXd& g0, const Eigen::MatrixXd& CE,
120-
const Eigen::VectorXd& ce0,
121-
const Eigen::MatrixXd& CI, const Eigen::VectorXd& ci0,
122-
Eigen::VectorXd& x,
123-
Eigen::VectorXi& A, size_t& q);
109+
double solve_quadprog2(Eigen::LLT<Eigen::MatrixXd, Eigen::Lower>& chol, double c1, Eigen::VectorXd& g0,
110+
const Eigen::MatrixXd& CE, const Eigen::VectorXd& ce0, const Eigen::MatrixXd& CI,
111+
const Eigen::VectorXd& ci0, Eigen::VectorXd& x, Eigen::VectorXi& A, size_t& q);
124112

125113
/* solve_quadprog is used for on-demand QP solving */
126-
double solve_quadprog(Eigen::MatrixXd& G,
127-
Eigen::VectorXd& g0,
128-
const Eigen::MatrixXd& CE,
129-
const Eigen::VectorXd& ce0,
130-
const Eigen::MatrixXd& CI,
131-
const Eigen::VectorXd& ci0, Eigen::VectorXd& x,
132-
Eigen::VectorXi& activeSet, size_t& activeSetSize);
114+
double solve_quadprog(Eigen::MatrixXd& G, Eigen::VectorXd& g0, const Eigen::MatrixXd& CE, const Eigen::VectorXd& ce0,
115+
const Eigen::MatrixXd& CI, const Eigen::VectorXd& ci0, Eigen::VectorXd& x,
116+
Eigen::VectorXi& activeSet, size_t& activeSetSize);
133117
// }
134118

135-
}
136-
}
137-
119+
} // namespace solvers
120+
} // namespace eiquadprog
138121

139122
#endif

‎src/eiquadprog-fast.cpp‎

Lines changed: 33 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,15 @@
1+
#include <iostream>
2+
13
#include "eiquadprog/eiquadprog-fast.hpp"
4+
#define TRACE_SOLVER
25

36
namespace eiquadprog {
47
namespace solvers {
58

6-
79
EiquadprogFast::EiquadprogFast() {
810
m_maxIter = DEFAULT_MAX_ITER;
9-
q = 0; // size of the active set A (containing the indices of the active constraints)
11+
q = 0; // size of the active set A (containing the indices
12+
// of the active constraints)
1013
is_inverse_provided_ = false;
1114
m_nVars = 0;
1215
m_nEqCon = 0;
@@ -182,7 +185,6 @@ void EiquadprogFast::delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, Ve
182185
}
183186
}
184187

185-
186188
EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const VectorXd& g0, const MatrixXd& CE,
187189
const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0,
188190
VectorXd& x) {
@@ -232,7 +234,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
232234
R.setZero(nVars, nVars);
233235
R_norm = 1.0;
234236

235-
/* compute the inverse of the factorized matrix Hess^-1, this is the initial value for H */
237+
/* compute the inverse of the factorized matrix Hess^-1,
238+
this is the initial value for H */
236239
// m_J = L^-T
237240
if (!is_inverse_provided_) {
238241
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOLESKY_INVERSE);
@@ -253,7 +256,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
253256
/* c1 * c2 is an estimate for cond(Hess) */
254257

255258
/*
256-
* Find the unconstrained minimizer of the quadratic form 0.5 * x Hess x + g0 x
259+
* Find the unconstrained minimizer of the quadratic
260+
* form 0.5 * x Hess x + g0 x
257261
* this is a feasible point in the dual space
258262
* x = Hess^-1 * g0
259263
*/
@@ -296,10 +300,12 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
296300
print_vector("d", d, nVars);
297301
#endif
298302

299-
/* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
300-
becomes feasible */
303+
/* compute full step length t2: i.e.,
304+
the minimum step in primal space s.t. the contraint
305+
becomes feasible */
301306
t2 = 0.0;
302-
if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
307+
if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon())
308+
// i.e. z != 0
303309
t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
304310

305311
x += t2 * z;
@@ -326,7 +332,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
326332
for (i = 0; i < nIneqCon; i++) iai(i) = static_cast<VectorXi::Scalar>(i);
327333

328334
#ifdef USE_WARM_START
329-
// DEBUG_STREAM("Gonna warm start using previous active set:\n"<<A.transpose()<<"\n")
335+
// DEBUG_STREAM("Gonna warm start using previous active
336+
// set:\n"<<A.transpose()<<"\n")
330337
for (i = nEqCon; i < q; i++) {
331338
iai(i - nEqCon) = -1;
332339
ip = A(i);
@@ -335,8 +342,9 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
335342
update_z(z, m_J, d, iq);
336343
update_r(R, r, d, iq);
337344

338-
/* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
339-
becomes feasible */
345+
/* compute full step length t2: i.e.,
346+
the minimum step in primal space s.t. the contraint
347+
becomes feasible */
340348
t2 = 0.0;
341349
if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
342350
t2 = (-np.dot(x) - ci0(ip)) / z.dot(np);
@@ -408,7 +416,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
408416
if (std::abs(psi) <= static_cast<double>(nIneqCon) * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) {
409417
/* numerically there are not infeasibilities anymore */
410418
q = iq;
411-
// DEBUG_STREAM("Optimal active set:\n"<<A.head(iq).transpose()<<"\n\n")
419+
// DEBUG_STREAM("Optimal active
420+
// set:\n"<<A.head(iq).transpose()<<"\n\n")
412421
return EIQUADPROG_FAST_OPTIMAL;
413422
}
414423

@@ -419,7 +428,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
419428

420429
l2: /* Step 2: check for feasibility and determine a new S-pair */
421430
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2);
422-
// find constraint with highest violation (what about normalizing constraints?)
431+
// find constraint with highest violation
432+
// (what about normalizing constraints?)
423433
for (i = 0; i < nIneqCon; i++) {
424434
if (s(i) < ss && iai(i) != -1 && iaexcl(i)) {
425435
ss = s(i);
@@ -449,7 +459,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
449459

450460
l2a: /* Step 2a: determine step direction */
451461
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A);
452-
/* compute z = H np: the step direction in the primal space (through m_J, see the paper) */
462+
/* compute z = H np: the step direction in the primal space
463+
(through m_J, see the paper) */
453464
compute_d(d, m_J, np);
454465
// update_z(z, m_J, d, iq);
455466
if (iq >= nVars) {
@@ -458,7 +469,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
458469
} else {
459470
update_z(z, m_J, d, iq);
460471
}
461-
/* compute N* np (if q > 0): the negative of the step direction in the dual space */
472+
/* compute N* np (if q > 0): the negative of the
473+
step direction in the dual space */
462474
update_r(R, r, d, iq);
463475
#ifdef TRACE_SOLVER
464476
std::cerr << "Step direction z" << std::endl;
@@ -473,7 +485,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
473485
/* Step 2b: compute step length */
474486
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B);
475487
l = 0;
476-
/* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
488+
/* Compute t1: partial step length (maximum step in dual
489+
space without violating dual feasibility */
477490
t1 = inf; /* +inf */
478491
/* find the index l s.t. it reaches the minimum of u+(x) / r */
479492
// l: index of constraint to drop (maybe)
@@ -484,8 +497,10 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
484497
l = A(k);
485498
}
486499
}
487-
/* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
488-
if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
500+
/* Compute t2: full step length (minimum step in primal
501+
space such that the constraint ip becomes feasible */
502+
if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon())
503+
// i.e. z != 0
489504
t2 = -s(ip) / z.dot(np);
490505
else
491506
t2 = inf; /* +inf */
@@ -591,5 +606,3 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
591606

592607
} /* namespace solvers */
593608
} /* namespace eiquadprog */
594-
595-

‎src/eiquadprog.cpp‎

Lines changed: 8 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
#include <eiquadprog/eiquadprog.hpp>
2-
namespace eiquadprog{
3-
namespace solvers{
4-
2+
namespace eiquadprog {
3+
namespace solvers {
4+
55
using namespace Eigen;
66

77
/* 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
2222
* @param A Output vector containing the indexes of the active constraints.
2323
* @param q Output value representing the size of the active set.
2424
*/
25-
double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, const MatrixXd& CE,
26-
const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0, VectorXd& x, VectorXi& A,
27-
size_t& q) {
25+
double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0,
26+
const MatrixXd& CI, const VectorXd& ci0, VectorXd& x, VectorXi& A, size_t& q) {
2827
size_t i, k, l; /* indices */
2928
size_t ip, me, mi;
3029
size_t n = g0.size();
@@ -438,6 +437,6 @@ void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_
438437
}
439438
}
440439
}
441-
442-
}
443-
}
440+
441+
} // namespace solvers
442+
} // namespace eiquadprog

‎tests/TestA.cpp‎

Lines changed: 3 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -7,30 +7,18 @@
77
using namespace eiquadprog::solvers;
88
using namespace eiquadprog::tests;
99

10-
A::A():
11-
Q_(2, 2),
12-
C_(2),
13-
Aeq_(0,2),
14-
Beq_(0),
15-
Aineq_(0,2),
16-
Bineq_(0),
17-
QP_()
18-
{
19-
10+
A::A() : Q_(2, 2), C_(2), Aeq_(0, 2), Beq_(0), Aineq_(0, 2), Bineq_(0), QP_() {
2011
QP_.reset(2, 0, 0);
21-
12+
2213
Q_.setZero();
2314
Q_(0, 0) = 1.0;
2415
Q_(1, 1) = 1.0;
2516

26-
2717
C_.setZero();
2818

2919
expected_ = EIQUADPROG_FAST_OPTIMAL;
30-
3120
}
3221

33-
EiquadprogFast_status A::solve(Eigen::VectorXd &x)
34-
{
22+
EiquadprogFast_status A::solve(Eigen::VectorXd &x) {
3523
return QP_.solve_quadprog(Q_, C_, Aeq_, Beq_, Aineq_, Bineq_, x);
3624
}

‎tests/TestA.hpp‎

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,6 @@ namespace eiquadprog {
88
namespace tests {
99

1010
class A {
11-
1211
protected:
1312
eiquadprog::solvers::EiquadprogFast_status expected_;
1413
Eigen::MatrixXd Q_;
@@ -22,11 +21,10 @@ class A {
2221
eiquadprog::solvers::EiquadprogFast QP_;
2322

2423
A();
25-
eiquadprog::solvers::EiquadprogFast_status
26-
solve( Eigen::VectorXd &x);
24+
eiquadprog::solvers::EiquadprogFast_status solve(Eigen::VectorXd &x);
2725
};
2826

29-
} /* namespace solvers */
27+
} // namespace tests
3028
} /* namespace eiquadprog */
3129

3230
#endif /* TEST_EIQUADPROG_CLASS_A_ */

0 commit comments

Comments
 (0)