Skip to content
GitLab
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
aef9db43
Commit
aef9db43
authored
Apr 22, 2020
by
Olivier Stasse
Committed by
olivier stasse
May 12, 2020
Browse files
Reorganization to have a library for:
* eiquadprog-fast * eiquadprog Associated tests + integration tests.
parent
1b32975f
Changes
16
Hide whitespace changes
Inline
Side-by-side
.gitignore
View file @
aef9db43
build*/
*~
CMakeLists.txt
View file @
aef9db43
...
...
@@ -48,12 +48,16 @@ SEARCH_FOR_BOOST()
SET
(
${
PROJECT_NAME
}
_HEADERS
include/
${
PROJECT_NAME
}
/eiquadprog.hpp
include/
${
PROJECT_NAME
}
/eiquadprog-fast.hpp
include/
${
PROJECT_NAME
}
/eiquadprog-fast.hxx
include/
${
PROJECT_NAME
}
/eiquadprog-rt.hpp
include/
${
PROJECT_NAME
}
/eiquadprog-rt.hxx
)
ADD_LIBRARY
(
${
PROJECT_NAME
}
INTERFACE
)
ADD_LIBRARY
(
${
PROJECT_NAME
}
SHARED
src/eiquadprog-fast.cpp
src/eiquadprog.cpp
)
TARGET_INCLUDE_DIRECTORIES
(
${
PROJECT_NAME
}
SYSTEM PRIVATE
${
EIGEN3_INCLUDE_DIR
}
)
TARGET_INCLUDE_DIRECTORIES
(
${
PROJECT_NAME
}
SYSTEM INTERFACE
${
EIGEN3_INCLUDE_DIR
}
)
TARGET_INCLUDE_DIRECTORIES
(
${
PROJECT_NAME
}
INTERFACE $<INSTALL_INTERFACE:include>
)
INSTALL
(
TARGETS
${
PROJECT_NAME
}
EXPORT
${
TARGETS_EXPORT_NAME
}
DESTINATION lib
)
...
...
include/eiquadprog/eiquadprog-fast.hpp
View file @
aef9db43
...
...
@@ -61,10 +61,12 @@ namespace eiquadprog {
namespace
solvers
{
#include
"eiquadprog/eiquadprog-utils.hxx"
/**
* Possible states of the solver.
*/
enum
EiquadprogFast_status
{
enum
EiquadprogFast_status
{
EIQUADPROG_FAST_OPTIMAL
=
0
,
EIQUADPROG_FAST_INFEASIBLE
=
1
,
EIQUADPROG_FAST_UNBOUNDED
=
2
,
...
...
@@ -228,7 +230,5 @@ class EiquadprogFast {
}
/* namespace solvers */
}
/* namespace eiquadprog */
/* --- Details -------------------------------------------------------------------- */
#include
"eiquadprog/eiquadprog-fast.hxx"
#endif
/* EIQUADPROGFAST_HPP_ */
include/eiquadprog/eiquadprog-rt.hpp
View file @
aef9db43
...
...
@@ -71,6 +71,7 @@ namespace eiquadprog {
namespace
solvers
{
#include
"eiquadprog/eiquadprog-utils.hxx"
/**
* Possible states of the solver.
*/
...
...
@@ -135,14 +136,14 @@ class RtEiquadprog {
* s.t. CE x + ce0 = 0
* CI x + ci0 >= 0
*/
inline
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_
;
...
...
@@ -244,18 +245,19 @@ class RtEiquadprog {
R
.
topLeftCorner
(
iq
,
iq
).
template
triangularView
<
Eigen
::
Upper
>().
solveInPlace
(
r
.
head
(
iq
));
}
inline
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
);
inline
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 */
/* --- Details -------------------------------------------------------------------- */
#include
"eiquadprog/eiquadprog-rt.hxx"
/* --- Details -------------------------------------------------------------------- */
#endif
/* __eiquadprog_rt_hpp__ */
include/eiquadprog/eiquadprog-rt.hxx
View file @
aef9db43
...
...
@@ -26,9 +26,11 @@ 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)
q
=
0
;
// size of the active set A
// (containing the indices of the active constraints)
is_inverse_provided_
=
false
;
}
...
...
@@ -36,9 +38,10 @@ 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-utils.hxx
View file @
aef9db43
#ifndef EIQUADPROG_UTILS_HPP_
#define EIQUADPROG_UTILS_HPP_
#include
<Eigen/Core>
/// Compute sqrt(a^2 + b^2)
template
<
typename
Scalar
>
inline
Scalar
distance
(
Scalar
a
,
Scalar
b
)
{
...
...
include/eiquadprog/eiquadprog.hpp
View file @
aef9db43
...
...
@@ -82,480 +82,58 @@
#include
<iostream>
namespace
Eigen
{
#include
"eiquadprog/eiquadprog-utils.hxx"
// namespace internal {
namespace
eiquadprog
{
namespace
solvers
{
template
<
typename
Scalar
>
inline
Scalar
distance
(
Scalar
a
,
Scalar
b
)
{
Scalar
a1
,
b1
,
t
;
a1
=
std
::
abs
(
a
);
b1
=
std
::
abs
(
b
);
if
(
a1
>
b1
)
{
t
=
(
b1
/
a1
);
return
a1
*
std
::
sqrt
(
1.0
+
t
*
t
);
}
else
if
(
b1
>
a1
)
{
t
=
(
a1
/
b1
);
return
b1
*
std
::
sqrt
(
1.0
+
t
*
t
);
}
return
a1
*
std
::
sqrt
(
2.0
);
}
// }
inline
void
compute_d
(
Eigen
::
VectorXd
&
d
,
const
Eigen
::
MatrixXd
&
J
,
const
Eigen
::
VectorXd
&
np
)
{
d
=
J
.
adjoint
()
*
np
;
}
inline
void
compute_d
(
VectorXd
&
d
,
const
MatrixXd
&
J
,
const
VectorXd
&
np
)
{
d
=
J
.
adjoint
()
*
np
;
}
inline
void
update_z
(
VectorXd
&
z
,
const
MatrixXd
&
J
,
const
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
MatrixXd
&
R
,
VectorXd
&
r
,
const
VectorXd
&
d
,
size_t
iq
)
{
r
.
head
(
iq
)
=
R
.
topLeftCorner
(
iq
,
iq
).
triangularView
<
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
(
MatrixXd
&
R
,
MatrixXd
&
J
,
VectorXd
&
d
,
size_t
&
iq
,
double
&
R_norm
);
void
delete_constraint
(
MatrixXd
&
R
,
MatrixXd
&
J
,
VectorXi
&
A
,
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
(
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
);
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
);
/* solve_quadprog is used for on-demand QP solving */
inline
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
)
{
LLT
<
MatrixXd
,
Lower
>
chol
(
G
.
cols
());
double
c1
;
/* compute the trace of the original matrix G */
c1
=
G
.
trace
();
/* decompose the matrix G in the form LL^T */
chol
.
compute
(
G
);
return
solve_quadprog2
(
chol
,
c1
,
g0
,
CE
,
ce0
,
CI
,
ci0
,
x
,
activeSet
,
activeSetSize
);
}
/* solve_quadprog2 is used for when the Cholesky decomposition of G is pre-computed
* @param A Output vector containing the indexes of the active constraints.
* @param q Output value representing the size of the active set.
*/
inline
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
();
size_t
p
=
CE
.
cols
();
size_t
m
=
CI
.
cols
();
MatrixXd
R
(
g0
.
size
(),
g0
.
size
()),
J
(
g0
.
size
(),
g0
.
size
());
VectorXd
s
(
m
+
p
),
z
(
n
),
r
(
m
+
p
),
d
(
n
),
np
(
n
),
u
(
m
+
p
);
VectorXd
x_old
(
n
),
u_old
(
m
+
p
);
double
f_value
,
psi
,
c2
,
sum
,
ss
,
R_norm
;
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 */
// 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
);
VectorXi
A_old
(
m
+
p
),
iai
(
m
+
p
),
iaexcl
(
m
+
p
);
// int q;
size_t
iq
,
iter
=
0
;
me
=
p
;
/* number of equality constraints */
mi
=
m
;
/* number of inequality constraints */
q
=
0
;
/* size of the active set A (containing the indices of the active constraints) */
/*
* Preprocessing phase
*/
/* initialize the matrix R */
d
.
setZero
();
R
.
setZero
();
R_norm
=
1.0
;
/* this variable will hold the norm of the matrix R */
/* compute the inverse of the factorized matrix G^-1, this is the initial value for H */
// J = L^-T
J
.
setIdentity
();
J
=
chol
.
matrixU
().
solve
(
J
);
c2
=
J
.
trace
();
#ifdef TRACE_SOLVER
print_matrix
(
"J"
,
J
,
n
);
#endif
/* c1 * c2 is an estimate for cond(G) */
/*
* Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x
* this is a feasible point in the dual space
* x = G^-1 * g0
*/
x
=
chol
.
solve
(
g0
);
x
=
-
x
;
/* and compute the current solution value */
f_value
=
0.5
*
g0
.
dot
(
x
);
#ifdef TRACE_SOLVER
std
::
cerr
<<
"Unconstrained solution: "
<<
f_value
<<
std
::
endl
;
print_vector
(
"x"
,
x
,
n
);
#endif
/* Add equality constraints to the working set A */
iq
=
0
;
for
(
i
=
0
;
i
<
me
;
i
++
)
{
np
=
CE
.
col
(
i
);
compute_d
(
d
,
J
,
np
);
update_z
(
z
,
J
,
d
,
iq
);
update_r
(
R
,
r
,
d
,
iq
);
#ifdef TRACE_SOLVER
print_matrix
(
"R"
,
R
,
iq
);
print_vector
(
"z"
,
z
,
n
);
print_vector
(
"r"
,
r
,
iq
);
print_vector
(
"d"
,
d
,
n
);
#endif
/* 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
)
-
ce0
(
i
))
/
z
.
dot
(
np
);
x
+=
t2
*
z
;
/* set u = u+ */
u
(
iq
)
=
t2
;
u
.
head
(
iq
)
-=
t2
*
r
.
head
(
iq
);
/* compute the new solution value */
f_value
+=
0.5
*
(
t2
*
t2
)
*
z
.
dot
(
np
);
A
(
i
)
=
static_cast
<
VectorXi
::
Scalar
>
(
-
i
-
1
);
if
(
!
add_constraint
(
R
,
J
,
d
,
iq
,
R_norm
))
{
// FIXME: it should raise an error
// Equality constraints are linearly dependent
return
f_value
;
}
}
/* set iai = K \ A */
for
(
i
=
0
;
i
<
mi
;
i
++
)
iai
(
i
)
=
static_cast
<
VectorXi
::
Scalar
>
(
i
);
l1:
iter
++
;
#ifdef TRACE_SOLVER
print_vector
(
"x"
,
x
,
n
);
#endif
/* step 1: choose a violated constraint */
for
(
i
=
me
;
i
<
iq
;
i
++
)
{
ip
=
A
(
i
);
iai
(
ip
)
=
-
1
;
}
/* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
ss
=
0.0
;
psi
=
0.0
;
/* this value will contain the sum of all infeasibilities */
ip
=
0
;
/* ip will be the index of the chosen violated constraint */
for
(
i
=
0
;
i
<
mi
;
i
++
)
{
iaexcl
(
i
)
=
1
;
sum
=
CI
.
col
(
i
).
dot
(
x
)
+
ci0
(
i
);
s
(
i
)
=
sum
;
psi
+=
std
::
min
(
0.0
,
sum
);
}
#ifdef TRACE_SOLVER
print_vector
(
"s"
,
s
,
mi
);
#endif
if
(
std
::
abs
(
psi
)
<=
static_cast
<
double
>
(
mi
)
*
std
::
numeric_limits
<
double
>::
epsilon
()
*
c1
*
c2
*
100.0
)
{
/* numerically there are not infeasibilities anymore */
q
=
iq
;
return
f_value
;
}
/* save old values for u, x and A */
u_old
.
head
(
iq
)
=
u
.
head
(
iq
);
A_old
.
head
(
iq
)
=
A
.
head
(
iq
);
x_old
=
x
;
l2:
/* Step 2: check for feasibility and determine a new S-pair */
for
(
i
=
0
;
i
<
mi
;
i
++
)
{
if
(
s
(
i
)
<
ss
&&
iai
(
i
)
!=
-
1
&&
iaexcl
(
i
))
{
ss
=
s
(
i
);
ip
=
i
;
}
}
if
(
ss
>=
0.0
)
{
q
=
iq
;
return
f_value
;
}
/* set np = n(ip) */
np
=
CI
.
col
(
ip
);
/* set u = (u 0)^T */
u
(
iq
)
=
0.0
;
/* add ip to the active set A */
A
(
iq
)
=
static_cast
<
VectorXi
::
Scalar
>
(
ip
);
#ifdef TRACE_SOLVER
std
::
cerr
<<
"Trying with constraint "
<<
ip
<<
std
::
endl
;
print_vector
(
"np"
,
np
,
n
);
#endif
l2a:
/* Step 2a: determine step direction */
/* compute z = H np: the step direction in the primal space (through J, see the paper) */
compute_d
(
d
,
J
,
np
);
update_z
(
z
,
J
,
d
,
iq
);
/* 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
;
print_vector
(
"z"
,
z
,
n
);
print_vector
(
"r"
,
r
,
iq
+
1
);
print_vector
(
"u"
,
u
,
iq
+
1
);
print_vector
(
"d"
,
d
,
n
);
print_ivector
(
"A"
,
A
,
iq
+
1
);
#endif
/* Step 2b: compute step length */
l
=
0
;
/* 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 */
for
(
k
=
me
;
k
<
iq
;
k
++
)
{
double
tmp
;
if
(
r
(
k
)
>
0.0
&&
((
tmp
=
u
(
k
)
/
r
(
k
))
<
t1
))
{
t1
=
tmp
;
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
t2
=
-
s
(
ip
)
/
z
.
dot
(
np
);
else
t2
=
inf
;
/* +inf */
/* the step is chosen as the minimum of t1 and t2 */
t
=
std
::
min
(
t1
,
t2
);
#ifdef TRACE_SOLVER
std
::
cerr
<<
"Step sizes: "
<<
t
<<
" (t1 = "
<<
t1
<<
", t2 = "
<<
t2
<<
") "
;
#endif
/* Step 2c: determine new S-pair and take step: */
/* case (i): no step in primal or dual space */
if
(
t
>=
inf
)
{
/* QPP is infeasible */
// FIXME: unbounded to raise
q
=
iq
;
return
inf
;
}
/* case (ii): step in dual space */
if
(
t2
>=
inf
)
{
/* set u = u + t * [-r 1) and drop constraint l from the active set A */
u
.
head
(
iq
)
-=
t
*
r
.
head
(
iq
);
u
(
iq
)
+=
t
;
iai
(
l
)
=
static_cast
<
VectorXi
::
Scalar
>
(
l
);
delete_constraint
(
R
,
J
,
A
,
u
,
p
,
iq
,
l
);
#ifdef TRACE_SOLVER
std
::
cerr
<<
" in dual space: "
<<
f_value
<<
std
::
endl
;
print_vector
(
"x"
,
x
,
n
);
print_vector
(
"z"
,
z
,
n
);
print_ivector
(
"A"
,
A
,
iq
+
1
);
#endif
goto
l2a
;
}
/* case (iii): step in primal and dual space */
x
+=
t
*
z
;
/* update the solution value */
f_value
+=
t
*
z
.
dot
(
np
)
*
(
0.5
*
t
+
u
(
iq
));
u
.
head
(
iq
)
-=
t
*
r
.
head
(
iq
);
u
(
iq
)
+=
t
;
#ifdef TRACE_SOLVER
std
::
cerr
<<
" in both spaces: "
<<
f_value
<<
std
::
endl
;
print_vector
(
"x"
,
x
,
n
);
print_vector
(
"u"
,
u
,
iq
+
1
);
print_vector
(
"r"
,
r
,
iq
+
1
);
print_ivector
(
"A"
,
A
,
iq
+
1
);
#endif
if
(
t
==
t2
)
{
#ifdef TRACE_SOLVER
std
::
cerr
<<
"Full step has taken "
<<
t
<<
std
::
endl
;
print_vector
(
"x"
,
x
,
n
);
#endif
/* full step has taken */
/* add constraint ip to the active set*/
if
(
!
add_constraint
(
R
,
J
,
d
,
iq
,
R_norm
))
{
iaexcl
(
ip
)
=
0
;
delete_constraint
(
R
,
J
,
A
,
u
,
p
,
iq
,
ip
);
#ifdef TRACE_SOLVER
print_matrix
(
"R"
,
R
,
n
);
print_ivector
(
"A"
,
A
,
iq
);
#endif
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
;
u
(
i
)
=
u_old
(
i
);
}
x
=
x_old
;
goto
l2
;
/* go to step 2 */
}
else
iai
(
ip
)
=
-
1
;
#ifdef TRACE_SOLVER
print_matrix
(
"R"
,
R
,
n
);
print_ivector
(
"A"
,
A
,
iq
);
#endif
goto
l1
;
}
/* a patial step has taken */
#ifdef TRACE_SOLVER
std
::
cerr
<<
"Partial step has taken "
<<
t
<<
std
::
endl
;
print_vector
(
"x"
,
x
,
n
);
#endif
/* drop constraint l */
iai
(
l
)
=
static_cast
<
VectorXi
::
Scalar
>
(
l
);
delete_constraint
(
R
,
J
,
A
,
u
,
p
,
iq
,
l
);
#ifdef TRACE_SOLVER
print_matrix
(
"R"
,
R
,
n
);
print_ivector
(
"A"
,
A
,
iq
);
#endif
s
(
ip
)
=
CI
.
col
(
ip
).
dot
(
x
)
+
ci0
(
ip
);
#ifdef TRACE_SOLVER
print_vector
(
"s"
,
s
,
mi
);
#endif
goto
l2a
;
}
inline
bool
add_constraint
(
MatrixXd
&
R
,
MatrixXd
&
J
,
VectorXd
&
d
,
size_t
&
iq
,
double
&
R_norm
)
{
size_t
n
=
J
.
rows
();
#ifdef TRACE_SOLVER
std
::
cerr
<<
"Add constraint "
<<
iq
<<
'/'
;
#endif
size_t
j
,
k
;
double
cc
,
ss
,
h
,
t1
,
t2
,
xny
;
/* we have to find the Givens rotation which will reduce the element
d(j) to zero.
if it is already zero we don't have to do anything, except of
decreasing j */
for
(
j
=
n
-
1
;
j
>=
iq
+
1
;
j
--
)
{
/* The Givens rotation is done with the matrix (cc cs, cs -cc).
If cc is one, then element (j) of d is zero compared with element
(j - 1). Hence we don't have to do anything.
If cc is zero, then we just have to switch column (j) and column (j - 1)
of J. Since we only switch columns in J, we have to be careful how we
update d depending on the sign of gs.
Otherwise we have to apply the Givens rotation to these columns.
The i - 1 element of d has to be updated to h. */
cc
=
d
(
j
-
1
);
ss
=
d
(
j
);
h
=
distance
(
cc
,
ss
);
if
(
h
==
0.0
)
continue
;
d
(
j
)
=
0.0
;
ss
=
ss
/
h
;
cc
=
cc
/
h
;
if
(
cc
<
0.0
)
{
cc
=
-
cc
;
ss
=
-
ss
;
d
(
j
-
1
)
=
-
h
;
}
else
d
(
j
-
1
)
=
h
;
xny
=
ss
/
(
1.0
+
cc
);
for
(
k
=
0
;
k
<
n
;
k
++
)
{
t1
=
J
(
k
,
j
-
1
);
t2
=
J
(
k
,
j
);
J
(
k
,
j
-
1
)
=
t1
*
cc
+
t2
*
ss
;
J
(
k
,
j
)
=
xny
*
(
t1
+
J
(
k
,
j
-
1
))
-
t2
;
}
}
/* update the number of constraints added*/
iq
++
;
/* To update R we have to put the iq components of the d vector
into column iq - 1 of R
*/
R
.
col
(
iq
-
1
).
head
(
iq
)
=
d
.
head
(
iq
);
#ifdef TRACE_SOLVER
std
::
cerr
<<
iq
<<
std
::
endl
;
#endif
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
);
// }
if
(
std
::
abs
(
d
(
iq
-
1
))
<=
std
::
numeric_limits
<
double
>::
epsilon
()
*
R_norm
)
// problem degenerate
return
false
;
R_norm
=
std
::
max
<
double
>
(
R_norm
,
std
::
abs
(
d
(
iq
-
1
)));