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
a75e7dbd
Commit
a75e7dbd
authored
Sep 17, 2019
by
Guilhem Saurel
Browse files
[Format]
parent
fb6bbcd5
Changes
5
Expand all
Hide whitespace changes
Inline
Side-by-side
include/eiquadprog/eiquadprog-fast.hpp
View file @
a75e7dbd
...
...
@@ -7,7 +7,7 @@
#include
<Eigen/Dense>
#define OPTIMIZE_STEP_1_2
// compute s(x) = ci^T * x + ci0
#define OPTIMIZE_STEP_1_2 // compute s(x) = ci^T * x + ci0
#define OPTIMIZE_COMPUTE_D
#define OPTIMIZE_UPDATE_Z
#define OPTIMIZE_HESSIAN_INVERSE
...
...
@@ -21,227 +21,197 @@
#ifdef PROFILE_EIQUADPROG
#define START_PROFILER_EIQUADPROG_FAST(x) START_PROFILER(x)
#define STOP_PROFILER_EIQUADPROG_FAST(x)
STOP_PROFILER(x)
#define STOP_PROFILER_EIQUADPROG_FAST(x) STOP_PROFILER(x)
#else
#define START_PROFILER_EIQUADPROG_FAST(x)
#define STOP_PROFILER_EIQUADPROG_FAST(x)
#endif
#define EIQUADPROG_FAST_CHOLESKY_DECOMPOSITION "EIQUADPROG_FAST Cholesky dec"
#define EIQUADPROG_FAST_CHOLESKY_INVERSE
"EIQUADPROG_FAST Cholesky inv"
#define EIQUADPROG_FAST_ADD_EQ_CONSTR
"EIQUADPROG_FAST ADD_EQ_CONSTR"
#define EIQUADPROG_FAST_ADD_EQ_CONSTR_1
"EIQUADPROG_FAST ADD_EQ_CONSTR_1"
#define EIQUADPROG_FAST_ADD_EQ_CONSTR_2
"EIQUADPROG_FAST ADD_EQ_CONSTR_2"
#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_2
"EIQUADPROG_FAST STEP_2"
#define EIQUADPROG_FAST_STEP_2A
"EIQUADPROG_FAST STEP_2A"
#define EIQUADPROG_FAST_STEP_2B
"EIQUADPROG_FAST STEP_2B"
#define EIQUADPROG_FAST_STEP_2C
"EIQUADPROG_FAST STEP_2C"
#define EIQUADPROG_FAST_CHOLESKY_INVERSE "EIQUADPROG_FAST Cholesky inv"
#define EIQUADPROG_FAST_ADD_EQ_CONSTR "EIQUADPROG_FAST ADD_EQ_CONSTR"
#define EIQUADPROG_FAST_ADD_EQ_CONSTR_1 "EIQUADPROG_FAST ADD_EQ_CONSTR_1"
#define EIQUADPROG_FAST_ADD_EQ_CONSTR_2 "EIQUADPROG_FAST ADD_EQ_CONSTR_2"
#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_2 "EIQUADPROG_FAST STEP_2"
#define EIQUADPROG_FAST_STEP_2A "EIQUADPROG_FAST STEP_2A"
#define EIQUADPROG_FAST_STEP_2B "EIQUADPROG_FAST STEP_2B"
#define EIQUADPROG_FAST_STEP_2C "EIQUADPROG_FAST STEP_2C"
#define DEFAULT_MAX_ITER 1000
namespace
eiquadprog
{
namespace
solvers
{
/**
* Possible states of the solver.
*/
enum
EiquadprogFast_status
{
EIQUADPROG_FAST_OPTIMAL
=
0
,
EIQUADPROG_FAST_INFEASIBLE
=
1
,
EIQUADPROG_FAST_UNBOUNDED
=
2
,
EIQUADPROG_FAST_MAX_ITER_REACHED
=
3
,
EIQUADPROG_FAST_REDUNDANT_EQUALITIES
=
4
};
class
EiquadprogFast
{
typedef
Eigen
::
MatrixXd
MatrixXd
;
typedef
Eigen
::
VectorXd
VectorXd
;
typedef
Eigen
::
VectorXi
VectorXi
;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EiquadprogFast
();
virtual
~
EiquadprogFast
();
void
reset
(
size_t
dim_qp
,
size_t
num_eq
,
size_t
num_ineq
);
int
getMaxIter
()
const
{
return
m_maxIter
;
}
bool
setMaxIter
(
int
maxIter
)
{
if
(
maxIter
<
0
)
return
false
;
m_maxIter
=
maxIter
;
return
true
;
}
/**
* @return The size of the active set, namely the number of
* active constraints (including the equalities).
*/
size_t
getActiveSetSize
()
const
{
return
q
;
}
/**
* @return The number of active-set iteratios.
*/
int
getIteratios
()
const
{
return
iter
;
}
/**
* @return The value of the objective function.
*/
double
getObjValue
()
const
{
return
f_value
;
}
/**
* @return The Lagrange multipliers
*/
const
VectorXd
&
getLagrangeMultipliers
()
const
{
return
u
;
}
/**
* Return the active set, namely the indeces of active constraints.
* The first nEqCon indexes are for the equalities and are negative.
* The last nIneqCon indexes are for the inequalities and start from 0.
* Only the first q elements of the return vector are valid, where q
* is the size of the active set.
* @return The set of indexes of the active constraints.
*/
const
VectorXi
&
getActiveSet
()
const
{
return
A
;
}
/**
* solves the problem
* min. x' Hess x + 2 g0' x
* 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
);
MatrixXd
m_J
;
// J * J' = Hessian <nVars,nVars>::d
bool
is_inverse_provided_
;
private:
size_t
m_nVars
;
size_t
m_nEqCon
;
size_t
m_nIneqCon
;
int
m_maxIter
;
/// max number of active-set iterations
double
f_value
;
/// current value of cost function
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
MatrixXd
R
;
// <nVars,nVars>::d
/// CI*x+ci0
VectorXd
s
;
// <nIneqCon>::d
/// infeasibility multipliers, i.e. negative step direction in dual space
VectorXd
r
;
// <nIneqCon+nEqCon>::d
/// Lagrange multipliers
VectorXd
u
;
// <nIneqCon+nEqCon>::d
/// step direction in primal space
VectorXd
z
;
// <nVars>::d
/// J' np
VectorXd
d
;
//<nVars>::d
/// current constraint normal
VectorXd
np
;
//<nVars>::d
/// active set (indeces of active constraints)
/// the first nEqCon indeces are for the equalities and are negative
/// the last nIneqCon indeces are for the inequalities are start from 0
VectorXi
A
;
// <nIneqCon+nEqCon>
/// initialized as K \ A
/// iai(i)=-1 iff inequality constraint i is in the active set
/// iai(i)=i otherwise
VectorXi
iai
;
// <nIneqCon>::i
/// 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
VectorXi
iaexcl
;
//<nIneqCon>::i
VectorXd
x_old
;
// old value of x <nVars>::d
VectorXd
u_old
;
// old value of u <nIneqCon+nEqCon>::d
VectorXi
A_old
;
// old value of A <nIneqCon+nEqCon>::i
namespace
eiquadprog
{
namespace
solvers
{
/**
* Possible states of the solver.
*/
enum
EiquadprogFast_status
{
EIQUADPROG_FAST_OPTIMAL
=
0
,
EIQUADPROG_FAST_INFEASIBLE
=
1
,
EIQUADPROG_FAST_UNBOUNDED
=
2
,
EIQUADPROG_FAST_MAX_ITER_REACHED
=
3
,
EIQUADPROG_FAST_REDUNDANT_EQUALITIES
=
4
};
class
EiquadprogFast
{
typedef
Eigen
::
MatrixXd
MatrixXd
;
typedef
Eigen
::
VectorXd
VectorXd
;
typedef
Eigen
::
VectorXi
VectorXi
;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EiquadprogFast
();
virtual
~
EiquadprogFast
();
void
reset
(
size_t
dim_qp
,
size_t
num_eq
,
size_t
num_ineq
);
int
getMaxIter
()
const
{
return
m_maxIter
;
}
bool
setMaxIter
(
int
maxIter
)
{
if
(
maxIter
<
0
)
return
false
;
m_maxIter
=
maxIter
;
return
true
;
}
/**
* @return The size of the active set, namely the number of
* active constraints (including the equalities).
*/
size_t
getActiveSetSize
()
const
{
return
q
;
}
/**
* @return The number of active-set iteratios.
*/
int
getIteratios
()
const
{
return
iter
;
}
/**
* @return The value of the objective function.
*/
double
getObjValue
()
const
{
return
f_value
;
}
/**
* @return The Lagrange multipliers
*/
const
VectorXd
&
getLagrangeMultipliers
()
const
{
return
u
;
}
/**
* Return the active set, namely the indeces of active constraints.
* The first nEqCon indexes are for the equalities and are negative.
* The last nIneqCon indexes are for the inequalities and start from 0.
* Only the first q elements of the return vector are valid, where q
* is the size of the active set.
* @return The set of indexes of the active constraints.
*/
const
VectorXi
&
getActiveSet
()
const
{
return
A
;
}
/**
* solves the problem
* min. x' Hess x + 2 g0' x
* 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
);
MatrixXd
m_J
;
// J * J' = Hessian <nVars,nVars>::d
bool
is_inverse_provided_
;
private:
size_t
m_nVars
;
size_t
m_nEqCon
;
size_t
m_nIneqCon
;
int
m_maxIter
;
/// max number of active-set iterations
double
f_value
;
/// current value of cost function
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
MatrixXd
R
;
// <nVars,nVars>::d
/// CI*x+ci0
VectorXd
s
;
// <nIneqCon>::d
/// infeasibility multipliers, i.e. negative step direction in dual space
VectorXd
r
;
// <nIneqCon+nEqCon>::d
/// Lagrange multipliers
VectorXd
u
;
// <nIneqCon+nEqCon>::d
/// step direction in primal space
VectorXd
z
;
// <nVars>::d
/// J' np
VectorXd
d
;
//<nVars>::d
/// current constraint normal
VectorXd
np
;
//<nVars>::d
/// active set (indeces of active constraints)
/// the first nEqCon indeces are for the equalities and are negative
/// the last nIneqCon indeces are for the inequalities are start from 0
VectorXi
A
;
// <nIneqCon+nEqCon>
/// initialized as K \ A
/// iai(i)=-1 iff inequality constraint i is in the active set
/// iai(i)=i otherwise
VectorXi
iai
;
// <nIneqCon>::i
/// 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
VectorXi
iaexcl
;
//<nIneqCon>::i
VectorXd
x_old
;
// old value of x <nVars>::d
VectorXd
u_old
;
// old value of u <nIneqCon+nEqCon>::d
VectorXi
A_old
;
// old value of A <nIneqCon+nEqCon>::i
#ifdef OPTIMIZE_ADD_CONSTRAINT
VectorXd
T1
;
/// tmp variable used in add_constraint
VectorXd
T1
;
/// tmp variable used in add_constraint
#endif
/// size of the active set A (containing the indices of the active constraints)
size_t
q
;
/// size of the active set A (containing the indices of the active constraints)
size_t
q
;
/// number of active-set iterations
int
iter
;
/// number of active-set iterations
int
iter
;
inline
void
compute_d
(
VectorXd
&
d
,
const
MatrixXd
&
J
,
const
VectorXd
&
np
)
{
inline
void
compute_d
(
VectorXd
&
d
,
const
MatrixXd
&
J
,
const
VectorXd
&
np
)
{
#ifdef OPTIMIZE_COMPUTE_D
d
.
noalias
()
=
J
.
adjoint
()
*
np
;
d
.
noalias
()
=
J
.
adjoint
()
*
np
;
#else
d
=
J
.
adjoint
()
*
np
;
d
=
J
.
adjoint
()
*
np
;
#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
);
z
.
noalias
()
=
J
.
rightCols
(
z
.
size
()
-
iq
)
*
d
.
tail
(
z
.
size
()
-
iq
);
#else
z
=
J
.
rightCols
(
J
.
cols
()
-
iq
)
*
d
.
tail
(
J
.
cols
()
-
iq
);
z
=
J
.
rightCols
(
J
.
cols
()
-
iq
)
*
d
.
tail
(
J
.
cols
()
-
iq
);
#endif
}
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
));
}
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
,
size_t
l
);
};
}
/* namespace solvers */
}
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
));
}
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
,
size_t
l
);
};
}
/* namespace solvers */
}
/* namespace eiquadprog */
/* --- Details -------------------------------------------------------------------- */
...
...
include/eiquadprog/eiquadprog-fast.hxx
View file @
a75e7dbd
This diff is collapsed.
Click to expand it.
include/eiquadprog/eiquadprog.hpp
View file @
a75e7dbd
This diff is collapsed.
Click to expand it.
unittest/eiquadprog-basic.cpp
View file @
a75e7dbd
...
...
@@ -20,25 +20,24 @@
// ci0: m
// x: n
BOOST_AUTO_TEST_SUITE
(
BOOST_TEST_MODULE
)
BOOST_AUTO_TEST_SUITE
(
BOOST_TEST_MODULE
)
// min ||x||^2
BOOST_AUTO_TEST_CASE
(
test_unbiased
)
{
Eigen
::
MatrixXd
Q
(
2
,
2
);
BOOST_AUTO_TEST_CASE
(
test_unbiased
)
{
Eigen
::
MatrixXd
Q
(
2
,
2
);
Q
.
setZero
();
Q
(
0
,
0
)
=
1.0
;
Q
(
1
,
1
)
=
1.0
;
Q
(
0
,
0
)
=
1.0
;
Q
(
1
,
1
)
=
1.0
;
Eigen
::
VectorXd
C
(
2
);
C
.
setZero
();
Eigen
::
MatrixXd
Aeq
(
2
,
0
);
Eigen
::
MatrixXd
Aeq
(
2
,
0
);
Eigen
::
VectorXd
Beq
(
0
);
Eigen
::
MatrixXd
Aineq
(
2
,
0
);
Eigen
::
MatrixXd
Aineq
(
2
,
0
);
Eigen
::
VectorXd
Bineq
(
0
);
...
...
@@ -53,29 +52,28 @@ BOOST_AUTO_TEST_CASE ( test_unbiased )
double
out
=
Eigen
::
solve_quadprog
(
Q
,
C
,
Aeq
,
Beq
,
Aineq
,
Bineq
,
x
,
activeSet
,
activeSetSize
);
BOOST_CHECK_CLOSE
(
out
,
val
,
1e-6
);
BOOST_CHECK_CLOSE
(
out
,
val
,
1e-6
);
BOOST_CHECK
(
x
.
isApprox
(
solution
));
}
// min ||x-x_0||^2, x_0 = (1 1)^T
BOOST_AUTO_TEST_CASE
(
test_biased
)
{
Eigen
::
MatrixXd
Q
(
2
,
2
);
BOOST_AUTO_TEST_CASE
(
test_biased
)
{
Eigen
::
MatrixXd
Q
(
2
,
2
);
Q
.
setZero
();
Q
(
0
,
0
)
=
1.0
;
Q
(
1
,
1
)
=
1.0
;
Q
(
0
,
0
)
=
1.0
;
Q
(
1
,
1
)
=
1.0
;
Eigen
::
VectorXd
C
(
2
);
C
(
0
)
=
-
1.
;
C
(
1
)
=
-
1.
;
Eigen
::
MatrixXd
Aeq
(
2
,
0
);
Eigen
::
MatrixXd
Aeq
(
2
,
0
);
Eigen
::
VectorXd
Beq
(
0
);
Eigen
::
MatrixXd
Aineq
(
2
,
0
);
Eigen
::
MatrixXd
Aineq
(
2
,
0
);
Eigen
::
VectorXd
Bineq
(
0
);
...
...
@@ -91,7 +89,7 @@ BOOST_AUTO_TEST_CASE ( test_biased )
double
out
=
Eigen
::
solve_quadprog
(
Q
,
C
,
Aeq
,
Beq
,
Aineq
,
Bineq
,
x
,
activeSet
,
activeSetSize
);
BOOST_CHECK_CLOSE
(
out
,
val
,
1e-6
);
BOOST_CHECK_CLOSE
(
out
,
val
,
1e-6
);
BOOST_CHECK
(
x
.
isApprox
(
solution
));
}
...
...
@@ -100,24 +98,23 @@ BOOST_AUTO_TEST_CASE ( test_biased )
// s.t.
// x[1] = 1 - x[0]
BOOST_AUTO_TEST_CASE
(
test_equality_constraints
)
{
Eigen
::
MatrixXd
Q
(
2
,
2
);
BOOST_AUTO_TEST_CASE
(
test_equality_constraints
)
{
Eigen
::
MatrixXd
Q
(
2
,
2
);
Q
.
setZero
();
Q
(
0
,
0
)
=
1.0
;
Q
(
1
,
1
)
=
1.0
;
Q
(
0
,
0
)
=
1.0
;
Q
(
1
,
1
)
=
1.0
;
Eigen
::
VectorXd
C
(
2
);
C
.
setZero
();
Eigen
::
MatrixXd
Aeq
(
2
,
1
);
Aeq
(
0
,
0
)
=
1.
;
Aeq
(
1
,
0
)
=
1.
;
Eigen
::
MatrixXd
Aeq
(
2
,
1
);
Aeq
(
0
,
0
)
=
1.
;
Aeq
(
1
,
0
)
=
1.
;
Eigen
::
VectorXd
Beq
(
1
);
Beq
(
0
)
=
-
1.
;
Eigen
::
MatrixXd
Aineq
(
2
,
0
);
Eigen
::
MatrixXd
Aineq
(
2
,
0
);
Eigen
::
VectorXd
Bineq
(
0
);
...
...
@@ -133,7 +130,7 @@ BOOST_AUTO_TEST_CASE ( test_equality_constraints )
double
out
=
Eigen
::
solve_quadprog
(
Q
,
C
,
Aeq
,
Beq
,
Aineq
,
Bineq
,
x
,
activeSet
,
activeSetSize
);
BOOST_CHECK_CLOSE
(
out
,
val
,
1e-6
);
BOOST_CHECK_CLOSE
(
out
,
val
,
1e-6
);
BOOST_CHECK
(
x
.
isApprox
(
solution
));
}
...
...
@@ -142,24 +139,23 @@ BOOST_AUTO_TEST_CASE ( test_equality_constraints )
// s.t.
// x[i] >= 1
BOOST_AUTO_TEST_CASE
(
test_inequality_constraints
)
{
Eigen
::
MatrixXd
Q
(
2
,
2
);
BOOST_AUTO_TEST_CASE
(
test_inequality_constraints
)
{
Eigen
::
MatrixXd
Q
(
2
,
2
);
Q
.
setZero
();
Q
(
0
,
0
)
=
1.0
;
Q
(
1
,
1
)
=
1.0
;
Q
(
0
,
0
)
=
1.0
;
Q
(
1
,
1
)
=
1.0
;
Eigen
::
VectorXd
C
(
2
);
C
.
setZero
();
Eigen
::
MatrixXd
Aeq
(
2
,
0
);
Eigen
::
MatrixXd
Aeq
(
2
,
0
);
Eigen
::
VectorXd
Beq
(
0
);
Eigen
::
MatrixXd
Aineq
(
2
,
2
);
Eigen
::
MatrixXd
Aineq
(
2
,
2
);
Aineq
.
setZero
();
Aineq
(
0
,
0
)
=
1.
;
Aineq
(
1
,
1
)
=
1.
;
Aineq
(
0
,
0
)
=
1.
;
Aineq
(
1
,
1
)
=
1.
;
Eigen
::
VectorXd
Bineq
(
2
);
Bineq
(
0
)
=
-
1.
;
...
...
@@ -177,7 +173,7 @@ BOOST_AUTO_TEST_CASE ( test_inequality_constraints )
double
out
=
Eigen
::
solve_quadprog
(
Q
,
C
,
Aeq
,
Beq
,
Aineq
,
Bineq
,
x
,
activeSet
,
activeSetSize
);
BOOST_CHECK_CLOSE
(
out
,
val
,
1e-6
);
BOOST_CHECK_CLOSE
(
out
,
val
,
1e-6
);
BOOST_CHECK
(
x
.
isApprox
(
solution
));
}
...
...
@@ -187,27 +183,26 @@ BOOST_AUTO_TEST_CASE ( test_inequality_constraints )
// x[1] = 5 - x[0]
// x[1] >= 3
BOOST_AUTO_TEST_CASE
(
test_full
)
{
Eigen
::
MatrixXd
Q
(
2
,
2
);
BOOST_AUTO_TEST_CASE
(
test_full
)
{
Eigen
::
MatrixXd
Q
(
2
,
2
);
Q
.
setZero
();
Q
(
0
,
0
)
=
1.0
;
Q
(
1
,
1
)
=
1.0
;
Q
(
0
,
0
)
=
1.0
;
Q
(
1
,
1
)
=
1.0
;
Eigen
::
VectorXd
C
(
2
);
C
(
0
)
=
-
1.
;
C
(
1
)
=
-
1.
;
Eigen
::
MatrixXd
Aeq
(
2
,
1
);
Aeq
(
0
,
0
)
=
1.
;
Aeq
(
1
,
0
)
=
1.
;
Eigen
::
MatrixXd
Aeq
(
2
,
1
);
Aeq
(
0
,
0
)
=
1.
;
Aeq
(
1
,
0
)
=
1.
;
Eigen
::
VectorXd
Beq
(
1
);
Beq
(
0
)
=
-
5.
;
Eigen
::
MatrixXd
Aineq
(
2
,
1
);
Eigen
::
MatrixXd
Aineq
(
2
,
1
);
Aineq
.
setZero
();
Aineq
(
1
,
0
)
=
1.
;
Aineq
(
1
,
0
)
=
1.
;
Eigen
::
VectorXd
Bineq
(
1
);
Bineq
(
0
)
=
-
3.
;
...
...
@@ -224,7 +219,7 @@ BOOST_AUTO_TEST_CASE ( test_full )
double
out
=
Eigen
::
solve_quadprog
(
Q
,
C
,
Aeq
,
Beq
,
Aineq
,
Bineq
,
x
,
activeSet
,
activeSetSize
);
BOOST_CHECK_CLOSE
(
out
,
val
,
1e-6
);
BOOST_CHECK_CLOSE
(
out
,
val
,
1e-6
);
BOOST_CHECK
(
x
.
isApprox
(
solution
));