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
ff42abad
Commit
ff42abad
authored
Oct 24, 2019
by
Guilhem Saurel
Browse files
Format
parent
57d5f735
Changes
3
Expand all
Hide whitespace changes
Inline
Side-by-side
cmake
@
efa25a99
Compare
f389aae2
...
efa25a99
Subproject commit
f389aae203c4d92649cd5eb66289fd6a17c03fde
Subproject commit
efa25a9976b8a6fc9f51d26924d4238d0d4820b1
include/eiquadprog/eiquadprog-rt.hpp
View file @
ff42abad
...
...
@@ -33,261 +33,225 @@
#ifdef PROFILE_EIQUADPROG
#define START_PROFILER_EIQUADPROG_RT(x) START_PROFILER(x)
#define STOP_PROFILER_EIQUADPROG_RT(x)
STOP_PROFILER(x)
#define STOP_PROFILER_EIQUADPROG_RT(x) STOP_PROFILER(x)
#else
#define START_PROFILER_EIQUADPROG_RT(x)
#define STOP_PROFILER_EIQUADPROG_RT(x)
#endif
#define PROFILE_EIQUADPROG_CHOWLESKY_DECOMPOSITION "EIQUADPROG_RT Chowlesky dec"
#define PROFILE_EIQUADPROG_CHOWLESKY_INVERSE
"EIQUADPROG_RT Chowlesky inv"
#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR
"EIQUADPROG_RT ADD_EQ_CONSTR"
#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR_1
"EIQUADPROG_RT ADD_EQ_CONSTR_1"
#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR_2
"EIQUADPROG_RT ADD_EQ_CONSTR_2"
#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_2
"EIQUADPROG_RT STEP_2"
#define PROFILE_EIQUADPROG_STEP_2A
"EIQUADPROG_RT STEP_2A"
#define PROFILE_EIQUADPROG_STEP_2B
"EIQUADPROG_RT STEP_2B"
#define PROFILE_EIQUADPROG_STEP_2C
"EIQUADPROG_RT STEP_2C"
#define PROFILE_EIQUADPROG_CHOWLESKY_INVERSE "EIQUADPROG_RT Chowlesky inv"
#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR "EIQUADPROG_RT ADD_EQ_CONSTR"
#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR_1 "EIQUADPROG_RT ADD_EQ_CONSTR_1"
#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR_2 "EIQUADPROG_RT ADD_EQ_CONSTR_2"
#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_2 "EIQUADPROG_RT STEP_2"
#define PROFILE_EIQUADPROG_STEP_2A "EIQUADPROG_RT STEP_2A"
#define PROFILE_EIQUADPROG_STEP_2B "EIQUADPROG_RT STEP_2B"
#define PROFILE_EIQUADPROG_STEP_2C "EIQUADPROG_RT STEP_2C"
#define DEFAULT_MAX_ITER 1000
template
<
int
Rows
,
int
Cols
>
struct
RtMatrixX
{
template
<
int
Rows
,
int
Cols
>
struct
RtMatrixX
{
typedef
Eigen
::
Matrix
<
double
,
Rows
,
Cols
>
d
;
};
template
<
int
Rows
>
struct
RtVectorX
{
template
<
int
Rows
>
struct
RtVectorX
{
typedef
Eigen
::
Matrix
<
double
,
Rows
,
1
>
d
;
typedef
Eigen
::
Matrix
<
int
,
Rows
,
1
>
i
;
typedef
Eigen
::
Matrix
<
int
,
Rows
,
1
>
i
;
};
namespace
eiquadprog
{
namespace
solvers
{
/**
* Possible states of the solver.
*/
enum
RtEiquadprog_status
{
RT_EIQUADPROG_OPTIMAL
=
0
,
RT_EIQUADPROG_INFEASIBLE
=
1
,
RT_EIQUADPROG_UNBOUNDED
=
2
,
RT_EIQUADPROG_MAX_ITER_REACHED
=
3
,
RT_EIQUADPROG_REDUNDANT_EQUALITIES
=
4
};
template
<
int
nVars
,
int
nEqCon
,
int
nIneqCon
>
class
RtEiquadprog
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
RtEiquadprog
();
virtual
~
RtEiquadprog
();
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).
*/
int
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
typename
RtVectorX
<
nIneqCon
+
nEqCon
>::
d
&
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
typename
RtVectorX
<
nIneqCon
+
nEqCon
>::
i
&
getActiveSet
()
const
{
return
A
;
}
/**
* solves the problem
* min. x' Hess x + 2 g0' x
* 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
);
typename
RtMatrixX
<
nVars
,
nVars
>::
d
m_J
;
// J * J' = Hessian
bool
is_inverse_provided_
;
private:
int
m_maxIter
;
/// max number of active-set iterations
double
f_value
;
/// current value of cost function
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
typename
RtMatrixX
<
nVars
,
nVars
>::
d
R
;
/// CI*x+ci0
typename
RtVectorX
<
nIneqCon
>::
d
s
;
/// infeasibility multipliers, i.e. negative step direction in dual space
typename
RtVectorX
<
nIneqCon
+
nEqCon
>::
d
r
;
/// Lagrange multipliers
typename
RtVectorX
<
nIneqCon
+
nEqCon
>::
d
u
;
/// step direction in primal space
typename
RtVectorX
<
nVars
>::
d
z
;
/// J' np
typename
RtVectorX
<
nVars
>::
d
d
;
/// current constraint normal
typename
RtVectorX
<
nVars
>::
d
np
;
/// 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
typename
RtVectorX
<
nIneqCon
+
nEqCon
>::
i
A
;
/// initialized as K \ A
/// iai(i)=-1 iff inequality constraint i is in the active set
/// iai(i)=i otherwise
typename
RtVectorX
<
nIneqCon
>::
i
iai
;
/// 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
typename
RtVectorX
<
nIneqCon
>::
i
iaexcl
;
typename
RtVectorX
<
nVars
>::
d
x_old
;
// old value of x
typename
RtVectorX
<
nIneqCon
+
nEqCon
>::
d
u_old
;
// old value of u
typename
RtVectorX
<
nIneqCon
+
nEqCon
>::
i
A_old
;
// old value of A
namespace
eiquadprog
{
namespace
solvers
{
/**
* Possible states of the solver.
*/
enum
RtEiquadprog_status
{
RT_EIQUADPROG_OPTIMAL
=
0
,
RT_EIQUADPROG_INFEASIBLE
=
1
,
RT_EIQUADPROG_UNBOUNDED
=
2
,
RT_EIQUADPROG_MAX_ITER_REACHED
=
3
,
RT_EIQUADPROG_REDUNDANT_EQUALITIES
=
4
};
template
<
int
nVars
,
int
nEqCon
,
int
nIneqCon
>
class
RtEiquadprog
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
RtEiquadprog
();
virtual
~
RtEiquadprog
();
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).
*/
int
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
typename
RtVectorX
<
nIneqCon
+
nEqCon
>::
d
&
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
typename
RtVectorX
<
nIneqCon
+
nEqCon
>::
i
&
getActiveSet
()
const
{
return
A
;
}
/**
* solves the problem
* min. x' Hess x + 2 g0' x
* 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
);
typename
RtMatrixX
<
nVars
,
nVars
>::
d
m_J
;
// J * J' = Hessian
bool
is_inverse_provided_
;
private:
int
m_maxIter
;
/// max number of active-set iterations
double
f_value
;
/// current value of cost function
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
typename
RtMatrixX
<
nVars
,
nVars
>::
d
R
;
/// CI*x+ci0
typename
RtVectorX
<
nIneqCon
>::
d
s
;
/// infeasibility multipliers, i.e. negative step direction in dual space
typename
RtVectorX
<
nIneqCon
+
nEqCon
>::
d
r
;
/// Lagrange multipliers
typename
RtVectorX
<
nIneqCon
+
nEqCon
>::
d
u
;
/// step direction in primal space
typename
RtVectorX
<
nVars
>::
d
z
;
/// J' np
typename
RtVectorX
<
nVars
>::
d
d
;
/// current constraint normal
typename
RtVectorX
<
nVars
>::
d
np
;
/// 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
typename
RtVectorX
<
nIneqCon
+
nEqCon
>::
i
A
;
/// initialized as K \ A
/// iai(i)=-1 iff inequality constraint i is in the active set
/// iai(i)=i otherwise
typename
RtVectorX
<
nIneqCon
>::
i
iai
;
/// 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
typename
RtVectorX
<
nIneqCon
>::
i
iaexcl
;
typename
RtVectorX
<
nVars
>::
d
x_old
;
// old value of x
typename
RtVectorX
<
nIneqCon
+
nEqCon
>::
d
u_old
;
// old value of u
typename
RtVectorX
<
nIneqCon
+
nEqCon
>::
i
A_old
;
// old value of A
#ifdef OPTIMIZE_ADD_CONSTRAINT
typename
RtVectorX
<
nVars
>::
d
T1
;
// tmp vector
typename
RtVectorX
<
nVars
>::
d
T1
;
// tmp vector
#endif
/// size of the active set A (containing the indices of the active constraints)
int
q
;
/// number of active-set iterations
int
iter
;
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
(
typename
RtVectorX
<
nVars
>::
d
&
d
,
const
typename
RtMatrixX
<
nVars
,
nVars
>::
d
&
J
,
const
typename
RtVectorX
<
nVars
>::
d
&
np
)
{
/// size of the active set A (containing the indices of the active constraints)
int
q
;
/// number of active-set iterations
int
iter
;
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
(
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
;
d
.
noalias
()
=
J
.
adjoint
()
*
np
;
#else
d
=
J
.
adjoint
()
*
np
;
d
=
J
.
adjoint
()
*
np
;
#endif
}
}
inline
void
update_z
(
typename
RtVectorX
<
nVars
>::
d
&
z
,
const
typename
RtMatrixX
<
nVars
,
nVars
>::
d
&
J
,
const
typename
RtVectorX
<
nVars
>::
d
&
d
,
int
iq
)
{
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
);
z
.
noalias
()
=
J
.
rightCols
(
nVars
-
iq
)
*
d
.
tail
(
nVars
-
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
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
));
}
inline
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
);
};
}
/* namespace solvers */
}
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
));
}
inline
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
);
};
}
/* namespace solvers */
}
/* namespace eiquadprog */
/* --- Details -------------------------------------------------------------------- */
...
...
include/eiquadprog/eiquadprog-rt.hxx
View file @
ff42abad
This diff is collapsed.
Click to expand it.
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment