Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
eiquadprog
Commits
59e13e74
Commit
59e13e74
authored
Mar 30, 2022
by
Guilhem Saurel
Browse files
reformat for standard line length
parent
9102494e
Pipeline
#17945
passed with stage
in 49 seconds
Changes
12
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
include/eiquadprog/eiquadprog-fast.hpp
View file @
59e13e74
...
...
@@ -49,7 +49,8 @@
#define EIQUADPROG_FAST_STEP_1 "EIQUADPROG_FAST STEP_1"
#define EIQUADPROG_FAST_STEP_1_1 "EIQUADPROG_FAST STEP_1_1"
#define EIQUADPROG_FAST_STEP_1_2 "EIQUADPROG_FAST STEP_1_2"
#define EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM "EIQUADPROG_FAST STEP_1_UNCONSTR_MINIM"
#define EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM \
"EIQUADPROG_FAST STEP_1_UNCONSTR_MINIM"
#define EIQUADPROG_FAST_STEP_2 "EIQUADPROG_FAST STEP_2"
#define EIQUADPROG_FAST_STEP_2A "EIQUADPROG_FAST STEP_2A"
#define EIQUADPROG_FAST_STEP_2B "EIQUADPROG_FAST STEP_2B"
...
...
@@ -132,8 +133,10 @@ class EiquadprogFast {
* s.t. CE x + ce0 = 0
* CI x + ci0 >= 0
*/
EiquadprogFast_status
solve_quadprog
(
const
MatrixXd
&
Hess
,
const
VectorXd
&
g0
,
const
MatrixXd
&
CE
,
const
VectorXd
&
ce0
,
const
MatrixXd
&
CI
,
const
VectorXd
&
ci0
,
VectorXd
&
x
);
EiquadprogFast_status
solve_quadprog
(
const
MatrixXd
&
Hess
,
const
VectorXd
&
g0
,
const
MatrixXd
&
CE
,
const
VectorXd
&
ce0
,
const
MatrixXd
&
CI
,
const
VectorXd
&
ci0
,
VectorXd
&
x
);
MatrixXd
m_J
;
// J * J' = Hessian <nVars,nVars>::d
bool
is_inverse_provided_
;
...
...
@@ -148,7 +151,8 @@ class EiquadprogFast {
Eigen
::
LLT
<
MatrixXd
,
Eigen
::
Lower
>
chol_
;
// <nVars,nVars>::d
/// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the matrix of active constraints
/// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the
/// matrix of active constraints
MatrixXd
R
;
// <nVars,nVars>::d
/// CI*x+ci0
...
...
@@ -182,8 +186,8 @@ class EiquadprogFast {
/// initialized as [1, ..., 1, .]
/// if iaexcl(i)!=1 inequality constraint i cannot be added to the active set
/// if adding ineq constraint i fails => iaexcl(i)=0
/// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active
constraints
/// iaexcl(i)=1 otherwise
/// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active
///
constraints
iaexcl(i)=1 otherwise
VectorXi
iaexcl
;
//<nIneqCon>::i
VectorXd
x_old
;
// old value of x <nVars>::d
...
...
@@ -194,7 +198,8 @@ class EiquadprogFast {
VectorXd
T1
;
/// tmp variable used in add_constraint
#endif
/// size of the active set A (containing the indices of the active constraints)
/// size of the active set A (containing the indices of the active
/// constraints)
size_t
q
;
/// number of active-set iterations
...
...
@@ -208,7 +213,8 @@ class EiquadprogFast {
#endif
}
inline
void
update_z
(
VectorXd
&
z
,
const
MatrixXd
&
J
,
const
VectorXd
&
d
,
size_t
iq
)
{
inline
void
update_z
(
VectorXd
&
z
,
const
MatrixXd
&
J
,
const
VectorXd
&
d
,
size_t
iq
)
{
#ifdef OPTIMIZE_UPDATE_Z
z
.
noalias
()
=
J
.
rightCols
(
z
.
size
()
-
iq
)
*
d
.
tail
(
z
.
size
()
-
iq
);
#else
...
...
@@ -216,14 +222,18 @@ class EiquadprogFast {
#endif
}
inline
void
update_r
(
const
MatrixXd
&
R
,
VectorXd
&
r
,
const
VectorXd
&
d
,
size_t
iq
)
{
inline
void
update_r
(
const
MatrixXd
&
R
,
VectorXd
&
r
,
const
VectorXd
&
d
,
size_t
iq
)
{
r
.
head
(
iq
)
=
d
.
head
(
iq
);
R
.
topLeftCorner
(
iq
,
iq
).
triangularView
<
Eigen
::
Upper
>
().
solveInPlace
(
r
.
head
(
iq
));
R
.
topLeftCorner
(
iq
,
iq
).
triangularView
<
Eigen
::
Upper
>
().
solveInPlace
(
r
.
head
(
iq
));
}
inline
bool
add_constraint
(
MatrixXd
&
R
,
MatrixXd
&
J
,
VectorXd
&
d
,
size_t
&
iq
,
double
&
R_norm
);
inline
bool
add_constraint
(
MatrixXd
&
R
,
MatrixXd
&
J
,
VectorXd
&
d
,
size_t
&
iq
,
double
&
R_norm
);
inline
void
delete_constraint
(
MatrixXd
&
R
,
MatrixXd
&
J
,
VectorXi
&
A
,
VectorXd
&
u
,
size_t
nEqCon
,
size_t
&
iq
,
inline
void
delete_constraint
(
MatrixXd
&
R
,
MatrixXd
&
J
,
VectorXi
&
A
,
VectorXd
&
u
,
size_t
nEqCon
,
size_t
&
iq
,
size_t
l
);
};
...
...
include/eiquadprog/eiquadprog-rt.hpp
View file @
59e13e74
...
...
@@ -20,6 +20,7 @@
#define __eiquadprog_rt_hpp__
#include <Eigen/Dense>
#include "eiquadprog/eiquadprog-utils.hxx"
#define OPTIMIZE_STEP_1_2 // compute s(x) = ci^T * x + ci0
...
...
@@ -49,7 +50,8 @@
#define PROFILE_EIQUADPROG_STEP_1 "EIQUADPROG_RT STEP_1"
#define PROFILE_EIQUADPROG_STEP_1_1 "EIQUADPROG_RT STEP_1_1"
#define PROFILE_EIQUADPROG_STEP_1_2 "EIQUADPROG_RT STEP_1_2"
#define PROFILE_EIQUADPROG_STEP_1_UNCONSTR_MINIM "EIQUADPROG_RT STEP_1_UNCONSTR_MINIM"
#define PROFILE_EIQUADPROG_STEP_1_UNCONSTR_MINIM \
"EIQUADPROG_RT STEP_1_UNCONSTR_MINIM"
#define PROFILE_EIQUADPROG_STEP_2 "EIQUADPROG_RT STEP_2"
#define PROFILE_EIQUADPROG_STEP_2A "EIQUADPROG_RT STEP_2A"
#define PROFILE_EIQUADPROG_STEP_2B "EIQUADPROG_RT STEP_2B"
...
...
@@ -118,7 +120,10 @@ class RtEiquadprog {
/**
* @return The Lagrange multipliers
*/
const
typename
RtVectorX
<
nIneqCon
+
nEqCon
>::
d
&
getLagrangeMultipliers
()
const
{
return
u
;
}
const
typename
RtVectorX
<
nIneqCon
+
nEqCon
>::
d
&
getLagrangeMultipliers
()
const
{
return
u
;
}
/**
* Return the active set, namely the indeces of active constraints.
...
...
@@ -128,7 +133,9 @@ class RtEiquadprog {
* is the size of the active set.
* @return The set of indexes of the active constraints.
*/
const
typename
RtVectorX
<
nIneqCon
+
nEqCon
>::
i
&
getActiveSet
()
const
{
return
A
;
}
const
typename
RtVectorX
<
nIneqCon
+
nEqCon
>::
i
&
getActiveSet
()
const
{
return
A
;
}
/**
* solves the problem
...
...
@@ -136,12 +143,14 @@ class RtEiquadprog {
* s.t. CE x + ce0 = 0
* CI x + ci0 >= 0
*/
RtEiquadprog_status
solve_quadprog
(
const
typename
RtMatrixX
<
nVars
,
nVars
>::
d
&
Hess
,
const
typename
RtVectorX
<
nVars
>::
d
&
g0
,
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
);
RtEiquadprog_status
solve_quadprog
(
const
typename
RtMatrixX
<
nVars
,
nVars
>::
d
&
Hess
,
const
typename
RtVectorX
<
nVars
>::
d
&
g0
,
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
);
typename
RtMatrixX
<
nVars
,
nVars
>::
d
m_J
;
// J * J' = Hessian
bool
is_inverse_provided_
;
...
...
@@ -153,7 +162,8 @@ class RtEiquadprog {
Eigen
::
LLT
<
typename
RtMatrixX
<
nVars
,
nVars
>::
d
,
Eigen
::
Lower
>
chol_
;
double
solver_return_
;
/// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the matrix of active constraints
/// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the
/// matrix of active constraints
typename
RtMatrixX
<
nVars
,
nVars
>::
d
R
;
/// CI*x+ci0
...
...
@@ -187,8 +197,8 @@ class RtEiquadprog {
/// initialized as [1, ..., 1, .]
/// if iaexcl(i)!=1 inequality constraint i cannot be added to the active set
/// if adding ineq constraint i fails => iaexcl(i)=0
/// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active
constraints
/// iaexcl(i)=1 otherwise
/// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active
///
constraints
iaexcl(i)=1 otherwise
typename
RtVectorX
<
nIneqCon
>::
i
iaexcl
;
typename
RtVectorX
<
nVars
>::
d
x_old
;
// old value of x
...
...
@@ -199,7 +209,8 @@ class RtEiquadprog {
typename
RtVectorX
<
nVars
>::
d
T1
;
// tmp vector
#endif
/// size of the active set A (containing the indices of the active constraints)
/// size of the active set A (containing the indices of the active
/// constraints)
int
q
;
/// number of active-set iterations
...
...
@@ -220,7 +231,8 @@ class RtEiquadprog {
return
a1
*
std
::
sqrt
(
2.0
);
}
inline
void
compute_d
(
typename
RtVectorX
<
nVars
>::
d
&
d
,
const
typename
RtMatrixX
<
nVars
,
nVars
>::
d
&
J
,
inline
void
compute_d
(
typename
RtVectorX
<
nVars
>::
d
&
d
,
const
typename
RtMatrixX
<
nVars
,
nVars
>::
d
&
J
,
const
typename
RtVectorX
<
nVars
>::
d
&
np
)
{
#ifdef OPTIMIZE_COMPUTE_D
d
.
noalias
()
=
J
.
adjoint
()
*
np
;
...
...
@@ -229,7 +241,8 @@ class RtEiquadprog {
#endif
}
inline
void
update_z
(
typename
RtVectorX
<
nVars
>::
d
&
z
,
const
typename
RtMatrixX
<
nVars
,
nVars
>::
d
&
J
,
inline
void
update_z
(
typename
RtVectorX
<
nVars
>::
d
&
z
,
const
typename
RtMatrixX
<
nVars
,
nVars
>::
d
&
J
,
const
typename
RtVectorX
<
nVars
>::
d
&
d
,
int
iq
)
{
#ifdef OPTIMIZE_UPDATE_Z
z
.
noalias
()
=
J
.
rightCols
(
nVars
-
iq
)
*
d
.
tail
(
nVars
-
iq
);
...
...
@@ -238,24 +251,31 @@ class RtEiquadprog {
#endif
}
inline
void
update_r
(
const
typename
RtMatrixX
<
nVars
,
nVars
>::
d
&
R
,
typename
RtVectorX
<
nIneqCon
+
nEqCon
>::
d
&
r
,
inline
void
update_r
(
const
typename
RtMatrixX
<
nVars
,
nVars
>::
d
&
R
,
typename
RtVectorX
<
nIneqCon
+
nEqCon
>::
d
&
r
,
const
typename
RtVectorX
<
nVars
>::
d
&
d
,
int
iq
)
{
r
.
head
(
iq
)
=
d
.
head
(
iq
);
R
.
topLeftCorner
(
iq
,
iq
).
template
triangularView
<
Eigen
::
Upper
>().
solveInPlace
(
r
.
head
(
iq
));
R
.
topLeftCorner
(
iq
,
iq
)
.
template
triangularView
<
Eigen
::
Upper
>()
.
solveInPlace
(
r
.
head
(
iq
));
}
bool
add_constraint
(
typename
RtMatrixX
<
nVars
,
nVars
>::
d
&
R
,
typename
RtMatrixX
<
nVars
,
nVars
>::
d
&
J
,
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
);
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
);
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
);
};
}
/* namespace solvers */
}
/* namespace eiquadprog */
#include "eiquadprog/eiquadprog-rt.hxx"
/* --- Details -------------------------------------------------------------------- */
/* --- Details
* -------------------------------------------------------------------- */
#endif
/* __eiquadprog_rt_hpp__ */
include/eiquadprog/eiquadprog-rt.hxx
View file @
59e13e74
...
...
@@ -29,7 +29,7 @@ template <int nVars, int nEqCon, int nIneqCon>
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
q
=
0
;
// size of the active set A
// (containing the indices of the active constraints)
is_inverse_provided_
=
false
;
}
...
...
@@ -68,8 +68,7 @@ bool RtEiquadprog<nVars, nEqCon, nIneqCon>::add_constraint(
cc
=
d
(
j
-
1
);
ss
=
d
(
j
);
h
=
utils
::
distance
(
cc
,
ss
);
if
(
h
==
0.0
)
continue
;
if
(
h
==
0.0
)
continue
;
d
(
j
)
=
0.0
;
ss
=
ss
/
h
;
cc
=
cc
/
h
;
...
...
@@ -82,8 +81,8 @@ bool RtEiquadprog<nVars, nEqCon, nIneqCon>::add_constraint(
xny
=
ss
/
(
1.0
+
cc
);
// #define OPTIMIZE_ADD_CONSTRAINT
#ifdef OPTIMIZE_ADD_CONSTRAINT // the optimized code is actually slower than
the
// original
#ifdef OPTIMIZE_ADD_CONSTRAINT
// the optimized code is actually slower than
//
the
original
T1
=
J
.
col
(
j
-
1
);
cc_ss
(
0
)
=
cc
;
cc_ss
(
1
)
=
ss
;
...
...
@@ -148,23 +147,20 @@ void RtEiquadprog<nVars, nEqCon, nIneqCon>::delete_constraint(
u
(
iq
-
1
)
=
u
(
iq
);
A
(
iq
)
=
0
;
u
(
iq
)
=
0.0
;
for
(
j
=
0
;
j
<
iq
;
j
++
)
R
(
j
,
iq
-
1
)
=
0.0
;
for
(
j
=
0
;
j
<
iq
;
j
++
)
R
(
j
,
iq
-
1
)
=
0.0
;
/* constraint has been fully removed */
iq
--
;
#ifdef EIQGUADPROG_TRACE_SOLVER
std
::
cerr
<<
'/'
<<
iq
<<
std
::
endl
;
#endif
if
(
iq
==
0
)
return
;
if
(
iq
==
0
)
return
;
for
(
j
=
qq
;
j
<
iq
;
j
++
)
{
cc
=
R
(
j
,
j
);
ss
=
R
(
j
+
1
,
j
);
h
=
utils
::
distance
(
cc
,
ss
);
if
(
h
==
0.0
)
continue
;
if
(
h
==
0.0
)
continue
;
cc
=
cc
/
h
;
ss
=
ss
/
h
;
R
(
j
+
1
,
j
)
=
0.0
;
...
...
@@ -200,20 +196,20 @@ RtEiquadprog_status RtEiquadprog<nVars, nEqCon, nIneqCon>::solve_quadprog(
const
typename
RtMatrixX
<
nIneqCon
,
nVars
>::
d
&
CI
,
const
typename
RtVectorX
<
nIneqCon
>::
d
&
ci0
,
typename
RtVectorX
<
nVars
>::
d
&
x
)
{
int
i
,
k
,
l
;
// indices
int
ip
;
// index of the chosen violated constraint
int
iq
;
// current number of active constraints
double
psi
;
// current sum of constraint violations
double
c1
;
// Hessian trace
double
c2
;
// Hessian Chowlesky factor trace
double
ss
;
// largest constraint violation (negative for violation)
double
R_norm
;
// norm of matrix R
int
i
,
k
,
l
;
// indices
int
ip
;
// index of the chosen violated constraint
int
iq
;
// current number of active constraints
double
psi
;
// current sum of constraint violations
double
c1
;
// Hessian trace
double
c2
;
// Hessian Chowlesky factor trace
double
ss
;
// largest constraint violation (negative for violation)
double
R_norm
;
// norm of matrix R
const
double
inf
=
std
::
numeric_limits
<
double
>::
infinity
();
double
t
,
t1
,
t2
;
/* t is the step length, which is the minimum of the partial step length t1
* and the full step length t2 */
iter
=
0
;
// active-set iteration number
iter
=
0
;
// active-set iteration number
/*
* Preprocessing phase
...
...
@@ -286,8 +282,7 @@ RtEiquadprog_status RtEiquadprog<nVars, nEqCon, nIneqCon>::solve_quadprog(
for
(
i
=
0
;
i
<
nEqCon
;
i
++
)
{
START_PROFILER_EIQUADPROG_RT
(
PROFILE_EIQUADPROG_ADD_EQ_CONSTR_1
);
// np = CE.row(i); // does not compile if nEqCon=0
for
(
int
tmp
=
0
;
tmp
<
nVars
;
tmp
++
)
np
[
tmp
]
=
CE
(
i
,
tmp
);
for
(
int
tmp
=
0
;
tmp
<
nVars
;
tmp
++
)
np
[
tmp
]
=
CE
(
i
,
tmp
);
compute_d
(
d
,
m_J
,
np
);
update_z
(
z
,
m_J
,
d
,
iq
);
update_r
(
R
,
r
,
d
,
iq
);
...
...
@@ -303,7 +298,7 @@ RtEiquadprog_status RtEiquadprog<nVars, nEqCon, nIneqCon>::solve_quadprog(
the contraint becomes feasible */
t2
=
0.0
;
if
(
std
::
abs
(
z
.
dot
(
z
))
>
std
::
numeric_limits
<
double
>::
epsilon
())
// i.e. z != 0
std
::
numeric_limits
<
double
>::
epsilon
())
// i.e. z != 0
t2
=
(
-
np
.
dot
(
x
)
-
ce0
(
i
))
/
z
.
dot
(
np
);
x
+=
t2
*
z
;
...
...
@@ -327,8 +322,7 @@ RtEiquadprog_status RtEiquadprog<nVars, nEqCon, nIneqCon>::solve_quadprog(
STOP_PROFILER_EIQUADPROG_RT
(
PROFILE_EIQUADPROG_ADD_EQ_CONSTR
);
/* set iai = K \ A */
for
(
i
=
0
;
i
<
nIneqCon
;
i
++
)
iai
(
i
)
=
i
;
for
(
i
=
0
;
i
<
nIneqCon
;
i
++
)
iai
(
i
)
=
i
;
#ifdef USE_WARM_START
// DEBUG_STREAM("Gonna warm start using previous active
...
...
@@ -345,7 +339,7 @@ RtEiquadprog_status RtEiquadprog<nVars, nEqCon, nIneqCon>::solve_quadprog(
the contraint becomes feasible */
t2
=
0.0
;
if
(
std
::
abs
(
z
.
dot
(
z
))
>
std
::
numeric_limits
<
double
>::
epsilon
())
// i.e. z != 0
std
::
numeric_limits
<
double
>::
epsilon
())
// i.e. z != 0
t2
=
(
-
np
.
dot
(
x
)
-
ci0
(
ip
))
/
z
.
dot
(
np
);
else
DEBUG_STREAM
(
"[WARM START] z=0
\n
"
)
...
...
@@ -443,8 +437,7 @@ l2: /* Step 2: check for feasibility and determine a new S-pair */
/* set np = n(ip) */
// np = CI.row(ip); // does not compile if nIneqCon=0
for
(
int
tmp
=
0
;
tmp
<
nVars
;
tmp
++
)
np
[
tmp
]
=
CI
(
ip
,
tmp
);
for
(
int
tmp
=
0
;
tmp
<
nVars
;
tmp
++
)
np
[
tmp
]
=
CI
(
ip
,
tmp
);
/* set u = (u 0)^T */
u
(
iq
)
=
0.0
;
/* add ip to the active set A */
...
...
@@ -501,7 +494,7 @@ l2a: /* Step 2a: determine step direction */
/* 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
std
::
numeric_limits
<
double
>::
epsilon
())
// i.e. z != 0
t2
=
-
s
(
ip
)
/
z
.
dot
(
np
);
else
t2
=
inf
;
/* +inf */
...
...
@@ -570,8 +563,7 @@ l2a: /* Step 2a: determine step direction */
utils
::
print_matrix
(
"R"
,
R
,
nVars
);
utils
::
print_vector
(
"A"
,
A
,
iq
);
#endif
for
(
i
=
0
;
i
<
nIneqCon
;
i
++
)
iai
(
i
)
=
i
;
for
(
i
=
0
;
i
<
nIneqCon
;
i
++
)
iai
(
i
)
=
i
;
for
(
i
=
0
;
i
<
iq
;
i
++
)
{
A
(
i
)
=
A_old
(
i
);
iai
(
A
(
i
))
=
-
1
;
...
...
@@ -595,8 +587,7 @@ l2a: /* Step 2a: determine step direction */
delete_constraint
(
R
,
m_J
,
A
,
u
,
iq
,
l
);
// s(ip) = CI.row(ip).dot(x) + ci0(ip); // does not compile if nIneqCon=0
s
(
ip
)
=
ci0
(
ip
);
for
(
int
tmp
=
0
;
tmp
<
nVars
;
tmp
++
)
s
(
ip
)
+=
CI
(
ip
,
tmp
)
*
x
[
tmp
];
for
(
int
tmp
=
0
;
tmp
<
nVars
;
tmp
++
)
s
(
ip
)
+=
CI
(
ip
,
tmp
)
*
x
[
tmp
];
#ifdef EIQGUADPROG_TRACE_SOLVER
std
::
cerr
<<
"Partial step has taken "
<<
t
<<
std
::
endl
;
...
...
include/eiquadprog/eiquadprog-utils.hxx
View file @
59e13e74
...
...
@@ -8,7 +8,8 @@ namespace eiquadprog {
namespace
utils
{
/// Compute sqrt(a^2 + b^2)
template
<
typename
Scalar
>
inline
Scalar
distance
(
Scalar
a
,
Scalar
b
)
{
template
<
typename
Scalar
>
inline
Scalar
distance
(
Scalar
a
,
Scalar
b
)
{
Scalar
a1
,
b1
,
t
;
a1
=
std
::
abs
(
a
);
b1
=
std
::
abs
(
b
);
...
...
@@ -31,6 +32,7 @@ void print_matrix(const char *name, Eigen::MatrixBase<Derived> &x, int /*n*/) {
std
::
cerr
<<
name
<<
std
::
endl
<<
x
<<
std
::
endl
;
}
}}
// namespace eiquadprog::utils
}
// namespace utils
}
// namespace eiquadprog
#endif
include/eiquadprog/eiquadprog.hpp
View file @
59e13e74
...
...
@@ -142,7 +142,7 @@ double solve_quadprog(Eigen::MatrixXd &G, Eigen::VectorXd &g0,
Eigen
::
VectorXi
&
activeSet
,
size_t
&
activeSetSize
);
// }
}
// namespace solvers
}
// namespace eiquadprog
}
// namespace solvers
}
// namespace eiquadprog
#endif
src/eiquadprog-fast.cpp
View file @
59e13e74
...
...
@@ -7,7 +7,7 @@ namespace solvers {
EiquadprogFast
::
EiquadprogFast
()
{
m_maxIter
=
DEFAULT_MAX_ITER
;
q
=
0
;
// size of the active set A (containing the indices
q
=
0
;
// size of the active set A (containing the indices
// of the active constraints)
is_inverse_provided_
=
false
;
m_nVars
=
0
;
...
...
@@ -70,8 +70,7 @@ bool EiquadprogFast::add_constraint(MatrixXd &R, MatrixXd &J, VectorXd &d,
cc
=
d
(
j
-
1
);
ss
=
d
(
j
);
h
=
utils
::
distance
(
cc
,
ss
);
if
(
h
==
0.0
)
continue
;
if
(
h
==
0.0
)
continue
;
d
(
j
)
=
0.0
;
ss
=
ss
/
h
;
cc
=
cc
/
h
;
...
...
@@ -84,8 +83,8 @@ bool EiquadprogFast::add_constraint(MatrixXd &R, MatrixXd &J, VectorXd &d,
xny
=
ss
/
(
1.0
+
cc
);
// #define OPTIMIZE_ADD_CONSTRAINT
#ifdef OPTIMIZE_ADD_CONSTRAINT // the optimized code is actually slower than
the
// original
#ifdef OPTIMIZE_ADD_CONSTRAINT
// the optimized code is actually slower than
//
the
original
T1
=
J
.
col
(
j
-
1
);
cc_ss
(
0
)
=
cc
;
cc_ss
(
1
)
=
ss
;
...
...
@@ -147,23 +146,20 @@ void EiquadprogFast::delete_constraint(MatrixXd &R, MatrixXd &J, VectorXi &A,
u
(
iq
-
1
)
=
u
(
iq
);
A
(
iq
)
=
0
;
u
(
iq
)
=
0.0
;
for
(
j
=
0
;
j
<
iq
;
j
++
)
R
(
j
,
iq
-
1
)
=
0.0
;
for
(
j
=
0
;
j
<
iq
;
j
++
)
R
(
j
,
iq
-
1
)
=
0.0
;
/* constraint has been fully removed */
iq
--
;
#ifdef EIQGUADPROG_TRACE_SOLVER
std
::
cerr
<<
'/'
<<
iq
<<
std
::
endl
;
#endif
if
(
iq
==
0
)
return
;
if
(
iq
==
0
)
return
;
for
(
j
=
qq
;
j
<
iq
;
j
++
)
{
cc
=
R
(
j
,
j
);
ss
=
R
(
j
+
1
,
j
);
h
=
utils
::
distance
(
cc
,
ss
);
if
(
h
==
0.0
)
continue
;
if
(
h
==
0.0
)
continue
;
cc
=
cc
/
h
;
ss
=
ss
/
h
;
R
(
j
+
1
,
j
)
=
0.0
;
...
...
@@ -210,20 +206,20 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(
static_cast
<
size_t
>
(
CI
.
cols
())
==
m_nVars
);
assert
(
static_cast
<
size_t
>
(
ci0
.
size
())
==
m_nIneqCon
);
size_t
i
,
k
,
l
;
// indices
size_t
ip
;
// index of the chosen violated constraint
size_t
iq
;
// current number of active constraints
double
psi
;
// current sum of constraint violations
double
c1
;
// Hessian trace
double
c2
;
// Hessian Cholesky factor trace
double
ss
;
// largest constraint violation (negative for violation)
double
R_norm
;
// norm of matrix R
size_t
i
,
k
,
l
;
// indices
size_t
ip
;
// index of the chosen violated constraint
size_t
iq
;
// current number of active constraints
double
psi
;
// current sum of constraint violations
double
c1
;
// Hessian trace
double
c2
;
// Hessian Cholesky factor trace
double
ss
;
// largest constraint violation (negative for violation)
double
R_norm
;
// norm of matrix R
const
double
inf
=
std
::
numeric_limits
<
double
>::
infinity
();
double
t
,
t1
,
t2
;
/* t is the step length, which is the minimum of the partial step length t1
* and the full step length t2 */
iter
=
0
;
// active-set iteration number
iter
=
0
;
// active-set iteration number
/*
* Preprocessing phase
...
...
@@ -338,8 +334,7 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(
STOP_PROFILER_EIQUADPROG_FAST
(
EIQUADPROG_FAST_ADD_EQ_CONSTR
);
/* set iai = K \ A */
for
(
i
=
0
;
i
<
nIneqCon
;
i
++
)
iai
(
i
)
=
static_cast
<
VectorXi
::
Scalar
>
(
i
);
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
...
...
@@ -357,7 +352,7 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(
becomes feasible */
t2
=
0.0
;
if
(
std
::
abs
(
z
.
dot
(
z
))
>
std
::
numeric_limits
<
double
>::
epsilon
())
// i.e. z != 0
std
::
numeric_limits
<
double
>::
epsilon
())
// i.e. z != 0
t2
=
(
-
np
.
dot
(
x
)
-
ci0
(
ip
))
/
z
.
dot
(
np
);
else
DEBUG_STREAM
(
"[WARM START] z=0
\n
"
)
...
...
@@ -582,8 +577,7 @@ l2a: /* Step 2a: determine step direction */
utils
::
print_matrix
(
"R"
,
R
,
nVars
);
utils
::
print_vector
(
"A"
,
A
,
iq
);
#endif
for
(
i
=
0
;
i
<
nIneqCon
;
i
++
)
iai
(
i
)
=
static_cast
<
VectorXi
::
Scalar
>
(
i
);
for
(
i
=
0
;
i
<
nIneqCon
;
i
++
)
iai
(
i
)
=
static_cast
<
VectorXi
::
Scalar
>
(
i
);
for
(
i
=
0
;
i
<
iq
;
i
++
)
{
A
(
i
)
=
A_old
(
i
);
iai
(
A
(
i
))
=
-
1
;
...
...
src/eiquadprog.cpp
View file @
59e13e74
...
...
@@ -10,7 +10,6 @@ double solve_quadprog(MatrixXd &G, VectorXd &g0, const MatrixXd &CE,
const
VectorXd
&
ce0
,
const
MatrixXd
&
CI
,
const
VectorXd
&
ci0
,
VectorXd
&
x
,
VectorXi
&
activeSet
,
size_t
&
activeSetSize
)
{
Eigen
::
DenseIndex
p
=
CE
.
cols
();
Eigen
::
DenseIndex
m
=
CI
.
cols
();
...
...
@@ -40,7 +39,6 @@ double solve_quadprog(LLT<MatrixXd, Lower> &chol, double c1, VectorXd &g0,
const
MatrixXd
&
CE
,
const
VectorXd
&
ce0
,
const
MatrixXd
&
CI
,
const
VectorXd
&
ci0
,
VectorXd
&
x
,
VectorXi
&
activeSet
,
size_t
&
activeSetSize
)
{
Eigen
::
DenseIndex
p
=
CE
.
cols
();
Eigen
::
DenseIndex
m
=
CI
.
cols
();
...
...
@@ -74,8 +72,7 @@ double solve_quadprog(LLT<MatrixXd, Lower> &chol, double c1, VectorXd &g0,
* step length t1 and the full step length t2 */
// VectorXi A(m + p); // Del Prete: active set is now an output
// parameter
if
(
static_cast
<
size_t
>
(
A
.
size
())
!=
m
+
p
)
A
.
resize
(
m
+
p
);
if
(
static_cast
<
size_t
>
(
A
.
size
())
!=
m
+
p
)
A
.
resize
(
m
+
p
);
VectorXi
A_old
(
m
+
p
),
iai
(
m
+
p
),
iaexcl
(
m
+
p
);
// int q;
size_t
iq
,
iter
=
0
;
...
...
@@ -138,7 +135,7 @@ double solve_quadprog(LLT<MatrixXd, Lower> &chol, double c1, VectorXd &g0,
the contraint becomes feasible */
t2
=
0.0
;
if
(
std
::
abs
(
z
.
dot
(
z
))
>
std
::
numeric_limits
<
double
>::
epsilon
())
// i.e. z != 0
std
::
numeric_limits
<
double
>::
epsilon
())
// i.e. z != 0
t2
=
(
-
np
.
dot
(
x
)
-
ce0
(
i
))
/
z
.
dot
(
np
);
x
+=
t2
*
z
;
...
...
@@ -159,8 +156,7 @@ double solve_quadprog(LLT<MatrixXd, Lower> &chol, double c1, VectorXd &g0,
}
/* set iai = K \ A */
for
(
i
=
0
;
i
<
mi
;
i
++
)
iai
(
i
)
=
static_cast
<
VectorXi
::
Scalar
>
(
i
);
for
(
i
=
0
;
i
<
mi
;
i
++
)
iai
(
i
)
=
static_cast
<
VectorXi
::
Scalar
>
(
i
);
l1:
iter
++
;
...
...
@@ -257,7 +253,7 @@ l2a: /* Step 2a: determine step direction */
/* 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
std
::
numeric_limits
<
double
>::
epsilon
())
// i.e. z != 0
t2
=
-
s
(
ip
)
/
z
.
dot
(
np
);
else
t2
=
inf
;
/* +inf */
...
...
@@ -324,8 +320,7 @@ l2a: /* Step 2a: determine step direction */
utils
::
print_matrix
(
"R"
,
R
,
n
);
utils
::
print_vector
(
"A"
,
A
,
iq
);
#endif
for
(
i
=
0
;
i
<
m
;
i
++
)
iai
(
i
)
=
static_cast
<
VectorXi
::
Scalar
>
(
i
);
for
(
i
=
0
;
i
<
m
;
i
++
)
iai
(
i
)
=
static_cast
<
VectorXi
::
Scalar
>
(
i
);
for
(
i
=
0
;
i
<
iq
;
i
++
)
{
A
(
i
)
=
A_old
(
i
);
iai
(
A
(
i
))
=
-
1
;
...
...
@@ -388,8 +383,7 @@ bool add_constraint(MatrixXd &R, MatrixXd &J, VectorXd &d, size_t &iq,
cc
=
d
(
j
-
1
);
ss
=
d
(
j
);
h
=
utils
::
distance
(
cc
,
ss
);
if
(
h
==
0.0
)
continue
;
if
(
h
==
0.0
)
continue
;
d
(
j
)