Skip to content

Commit aac07b1

Browse files
jcarpentjcarpent
authored
Merge pull request #22 from jcarpent/devel
Remove memory allocations and expose dual variables to the user
2 parents f2113a7 + 59e13e7 commit aac07b1

14 files changed

+523
-351
lines changed

‎.pre-commit-config.yaml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@ repos:
33
rev: v12.0.1
44
hooks:
55
- id: clang-format
6-
args: [--style=Google]
6+
args: [-i, --style=Google]
77
- repo: https://github.com/pre-commit/pre-commit-hooks
88
rev: v2.3.0
99
hooks:

‎CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -63,8 +63,8 @@ ADD_LIBRARY(${PROJECT_NAME} SHARED
6363
)
6464

6565
IF(TRACE_SOLVER)
66-
TARGET_COMPILE_DEFINITIONS(${PROJECT_NAME} PRIVATE TRACE_SOLVER)
6766
ENDIF(TRACE_SOLVER)
67+
TARGET_COMPILE_DEFINITIONS(${PROJECT_NAME} PRIVATE EIQGUADPROG_TRACE_SOLVER)
6868

6969
TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR})
7070
TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} INTERFACE $<INSTALL_INTERFACE:include>)

‎include/eiquadprog/eiquadprog-fast.hpp‎

Lines changed: 24 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -49,20 +49,21 @@
4949
#define EIQUADPROG_FAST_STEP_1 "EIQUADPROG_FAST STEP_1"
5050
#define EIQUADPROG_FAST_STEP_1_1 "EIQUADPROG_FAST STEP_1_1"
5151
#define EIQUADPROG_FAST_STEP_1_2 "EIQUADPROG_FAST STEP_1_2"
52-
#define EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM "EIQUADPROG_FAST STEP_1_UNCONSTR_MINIM"
52+
#define EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM \
53+
"EIQUADPROG_FAST STEP_1_UNCONSTR_MINIM"
5354
#define EIQUADPROG_FAST_STEP_2 "EIQUADPROG_FAST STEP_2"
5455
#define EIQUADPROG_FAST_STEP_2A "EIQUADPROG_FAST STEP_2A"
5556
#define EIQUADPROG_FAST_STEP_2B "EIQUADPROG_FAST STEP_2B"
5657
#define EIQUADPROG_FAST_STEP_2C "EIQUADPROG_FAST STEP_2C"
5758

5859
#define DEFAULT_MAX_ITER 1000
5960

61+
#include "eiquadprog/eiquadprog-utils.hxx"
62+
6063
namespace eiquadprog {
6164

6265
namespace solvers {
6366

64-
#include "eiquadprog/eiquadprog-utils.hxx"
65-
6667
/**
6768
* Possible states of the solver.
6869
*/
@@ -132,8 +133,10 @@ class EiquadprogFast {
132133
* s.t. CE x + ce0 = 0
133134
* CI x + ci0 >= 0
134135
*/
135-
EiquadprogFast_status solve_quadprog(const MatrixXd& Hess, const VectorXd& g0, const MatrixXd& CE,
136-
const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0, VectorXd& x);
136+
EiquadprogFast_status solve_quadprog(const MatrixXd& Hess, const VectorXd& g0,
137+
const MatrixXd& CE, const VectorXd& ce0,
138+
const MatrixXd& CI, const VectorXd& ci0,
139+
VectorXd& x);
137140

138141
MatrixXd m_J; // J * J' = Hessian <nVars,nVars>::d
139142
bool is_inverse_provided_;
@@ -148,7 +151,8 @@ class EiquadprogFast {
148151

149152
Eigen::LLT<MatrixXd, Eigen::Lower> chol_; // <nVars,nVars>::d
150153

151-
/// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the matrix of active constraints
154+
/// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the
155+
/// matrix of active constraints
152156
MatrixXd R; // <nVars,nVars>::d
153157

154158
/// CI*x+ci0
@@ -182,8 +186,8 @@ class EiquadprogFast {
182186
/// initialized as [1, ..., 1, .]
183187
/// if iaexcl(i)!=1 inequality constraint i cannot be added to the active set
184188
/// if adding ineq constraint i fails => iaexcl(i)=0
185-
/// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active constraints
186-
/// iaexcl(i)=1 otherwise
189+
/// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active
190+
/// constraints iaexcl(i)=1 otherwise
187191
VectorXi iaexcl; //<nIneqCon>::i
188192

189193
VectorXd x_old; // old value of x <nVars>::d
@@ -194,7 +198,8 @@ class EiquadprogFast {
194198
VectorXd T1; /// tmp variable used in add_constraint
195199
#endif
196200

197-
/// size of the active set A (containing the indices of the active constraints)
201+
/// size of the active set A (containing the indices of the active
202+
/// constraints)
198203
size_t q;
199204

200205
/// number of active-set iterations
@@ -208,22 +213,27 @@ class EiquadprogFast {
208213
#endif
209214
}
210215

211-
inline void update_z(VectorXd& z, const MatrixXd& J, const VectorXd& d, size_t iq) {
216+
inline void update_z(VectorXd& z, const MatrixXd& J, const VectorXd& d,
217+
size_t iq) {
212218
#ifdef OPTIMIZE_UPDATE_Z
213219
z.noalias() = J.rightCols(z.size() - iq) * d.tail(z.size() - iq);
214220
#else
215221
z = J.rightCols(J.cols() - iq) * d.tail(J.cols() - iq);
216222
#endif
217223
}
218224

219-
inline void update_r(const MatrixXd& R, VectorXd& r, const VectorXd& d, size_t iq) {
225+
inline void update_r(const MatrixXd& R, VectorXd& r, const VectorXd& d,
226+
size_t iq) {
220227
r.head(iq) = d.head(iq);
221-
R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace(r.head(iq));
228+
R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace(
229+
r.head(iq));
222230
}
223231

224-
inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R_norm);
232+
inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq,
233+
double& R_norm);
225234

226-
inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_t nEqCon, size_t& iq,
235+
inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A,
236+
VectorXd& u, size_t nEqCon, size_t& iq,
227237
size_t l);
228238
};
229239

‎include/eiquadprog/eiquadprog-rt.hpp‎

Lines changed: 43 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,8 @@
2121

2222
#include <Eigen/Dense>
2323

24+
#include "eiquadprog/eiquadprog-utils.hxx"
25+
2426
#define OPTIMIZE_STEP_1_2 // compute s(x) = ci^T * x + ci0
2527
#define OPTIMIZE_COMPUTE_D // use noalias
2628
#define OPTIMIZE_UPDATE_Z // use noalias
@@ -48,7 +50,8 @@
4850
#define PROFILE_EIQUADPROG_STEP_1 "EIQUADPROG_RT STEP_1"
4951
#define PROFILE_EIQUADPROG_STEP_1_1 "EIQUADPROG_RT STEP_1_1"
5052
#define PROFILE_EIQUADPROG_STEP_1_2 "EIQUADPROG_RT STEP_1_2"
51-
#define PROFILE_EIQUADPROG_STEP_1_UNCONSTR_MINIM "EIQUADPROG_RT STEP_1_UNCONSTR_MINIM"
53+
#define PROFILE_EIQUADPROG_STEP_1_UNCONSTR_MINIM \
54+
"EIQUADPROG_RT STEP_1_UNCONSTR_MINIM"
5255
#define PROFILE_EIQUADPROG_STEP_2 "EIQUADPROG_RT STEP_2"
5356
#define PROFILE_EIQUADPROG_STEP_2A "EIQUADPROG_RT STEP_2A"
5457
#define PROFILE_EIQUADPROG_STEP_2B "EIQUADPROG_RT STEP_2B"
@@ -71,7 +74,6 @@ namespace eiquadprog {
7174

7275
namespace solvers {
7376

74-
#include "eiquadprog/eiquadprog-utils.hxx"
7577
/**
7678
* Possible states of the solver.
7779
*/
@@ -118,7 +120,10 @@ class RtEiquadprog {
118120
/**
119121
* @return The Lagrange multipliers
120122
*/
121-
const typename RtVectorX<nIneqCon + nEqCon>::d& getLagrangeMultipliers() const { return u; }
123+
const typename RtVectorX<nIneqCon + nEqCon>::d& getLagrangeMultipliers()
124+
const {
125+
return u;
126+
}
122127

123128
/**
124129
* Return the active set, namely the indeces of active constraints.
@@ -128,20 +133,24 @@ class RtEiquadprog {
128133
* is the size of the active set.
129134
* @return The set of indexes of the active constraints.
130135
*/
131-
const typename RtVectorX<nIneqCon + nEqCon>::i& getActiveSet() const { return A; }
136+
const typename RtVectorX<nIneqCon + nEqCon>::i& getActiveSet() const {
137+
return A;
138+
}
132139

133140
/**
134141
* solves the problem
135142
* min. x' Hess x + 2 g0' x
136143
* s.t. CE x + ce0 = 0
137144
* CI x + ci0 >= 0
138145
*/
139-
RtEiquadprog_status solve_quadprog(const typename RtMatrixX<nVars, nVars>::d& Hess,
140-
const typename RtVectorX<nVars>::d& g0,
141-
const typename RtMatrixX<nEqCon, nVars>::d& CE,
142-
const typename RtVectorX<nEqCon>::d& ce0,
143-
const typename RtMatrixX<nIneqCon, nVars>::d& CI,
144-
const typename RtVectorX<nIneqCon>::d& ci0, typename RtVectorX<nVars>::d& x);
146+
RtEiquadprog_status solve_quadprog(
147+
const typename RtMatrixX<nVars, nVars>::d& Hess,
148+
const typename RtVectorX<nVars>::d& g0,
149+
const typename RtMatrixX<nEqCon, nVars>::d& CE,
150+
const typename RtVectorX<nEqCon>::d& ce0,
151+
const typename RtMatrixX<nIneqCon, nVars>::d& CI,
152+
const typename RtVectorX<nIneqCon>::d& ci0,
153+
typename RtVectorX<nVars>::d& x);
145154

146155
typename RtMatrixX<nVars, nVars>::d m_J; // J * J' = Hessian
147156
bool is_inverse_provided_;
@@ -153,7 +162,8 @@ class RtEiquadprog {
153162
Eigen::LLT<typename RtMatrixX<nVars, nVars>::d, Eigen::Lower> chol_;
154163
double solver_return_;
155164

156-
/// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the matrix of active constraints
165+
/// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the
166+
/// matrix of active constraints
157167
typename RtMatrixX<nVars, nVars>::d R;
158168

159169
/// CI*x+ci0
@@ -187,8 +197,8 @@ class RtEiquadprog {
187197
/// initialized as [1, ..., 1, .]
188198
/// if iaexcl(i)!=1 inequality constraint i cannot be added to the active set
189199
/// if adding ineq constraint i fails => iaexcl(i)=0
190-
/// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active constraints
191-
/// iaexcl(i)=1 otherwise
200+
/// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active
201+
/// constraints iaexcl(i)=1 otherwise
192202
typename RtVectorX<nIneqCon>::i iaexcl;
193203

194204
typename RtVectorX<nVars>::d x_old; // old value of x
@@ -199,7 +209,8 @@ class RtEiquadprog {
199209
typename RtVectorX<nVars>::d T1; // tmp vector
200210
#endif
201211

202-
/// size of the active set A (containing the indices of the active constraints)
212+
/// size of the active set A (containing the indices of the active
213+
/// constraints)
203214
int q;
204215

205216
/// number of active-set iterations
@@ -220,7 +231,8 @@ class RtEiquadprog {
220231
return a1 * std::sqrt(2.0);
221232
}
222233

223-
inline void compute_d(typename RtVectorX<nVars>::d& d, const typename RtMatrixX<nVars, nVars>::d& J,
234+
inline void compute_d(typename RtVectorX<nVars>::d& d,
235+
const typename RtMatrixX<nVars, nVars>::d& J,
224236
const typename RtVectorX<nVars>::d& np) {
225237
#ifdef OPTIMIZE_COMPUTE_D
226238
d.noalias() = J.adjoint() * np;
@@ -229,7 +241,8 @@ class RtEiquadprog {
229241
#endif
230242
}
231243

232-
inline void update_z(typename RtVectorX<nVars>::d& z, const typename RtMatrixX<nVars, nVars>::d& J,
244+
inline void update_z(typename RtVectorX<nVars>::d& z,
245+
const typename RtMatrixX<nVars, nVars>::d& J,
233246
const typename RtVectorX<nVars>::d& d, int iq) {
234247
#ifdef OPTIMIZE_UPDATE_Z
235248
z.noalias() = J.rightCols(nVars - iq) * d.tail(nVars - iq);
@@ -238,24 +251,31 @@ class RtEiquadprog {
238251
#endif
239252
}
240253

241-
inline void update_r(const typename RtMatrixX<nVars, nVars>::d& R, typename RtVectorX<nIneqCon + nEqCon>::d& r,
254+
inline void update_r(const typename RtMatrixX<nVars, nVars>::d& R,
255+
typename RtVectorX<nIneqCon + nEqCon>::d& r,
242256
const typename RtVectorX<nVars>::d& d, int iq) {
243257
r.head(iq) = d.head(iq);
244-
R.topLeftCorner(iq, iq).template triangularView<Eigen::Upper>().solveInPlace(r.head(iq));
258+
R.topLeftCorner(iq, iq)
259+
.template triangularView<Eigen::Upper>()
260+
.solveInPlace(r.head(iq));
245261
}
246262

247-
bool add_constraint(typename RtMatrixX<nVars, nVars>::d& R, typename RtMatrixX<nVars, nVars>::d& J,
263+
bool add_constraint(typename RtMatrixX<nVars, nVars>::d& R,
264+
typename RtMatrixX<nVars, nVars>::d& J,
248265
typename RtVectorX<nVars>::d& d, int& iq, double& R_norm);
249266

250-
void delete_constraint(typename RtMatrixX<nVars, nVars>::d& R, typename RtMatrixX<nVars, nVars>::d& J,
251-
typename RtVectorX<nIneqCon + nEqCon>::i& A, typename RtVectorX<nIneqCon + nEqCon>::d& u,
252-
int& iq, int l);
267+
void delete_constraint(typename RtMatrixX<nVars, nVars>::d& R,
268+
typename RtMatrixX<nVars, nVars>::d& J,
269+
typename RtVectorX<nIneqCon + nEqCon>::i& A,
270+
typename RtVectorX<nIneqCon + nEqCon>::d& u, int& iq,
271+
int l);
253272
};
254273

255274
} /* namespace solvers */
256275
} /* namespace eiquadprog */
257276

258277
#include "eiquadprog/eiquadprog-rt.hxx"
259-
/* --- Details -------------------------------------------------------------------- */
278+
/* --- Details
279+
* -------------------------------------------------------------------- */
260280

261281
#endif /* __eiquadprog_rt_hpp__ */

0 commit comments

Comments
 (0)