 |
Visual Servoing Platform
version 3.3.0
|
40 #include <visp3/core/vpMatrixException.h>
41 #include <visp3/core/vpQuadProg.h>
43 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
118 for(
unsigned int i = 0; i < n; ++i)
130 D.diag(d.extract(k,n-k));
131 for(
unsigned int i = 0; i < n-k; ++i)
138 "should install GSL 3rd party"));
162 std::cout <<
"vpQuadProg::setEqualityConstraint: equality constraint infeasible" << std::endl;
193 if(A.
getCols() && (Q*A).infinityNorm() > tol)
194 x = b + A*(Q*A).solveBySVD(r - Q*b);
263 std::cout <<
"vpQuadProg::solveQPe: wrong dimension\n" <<
270 if((Q*
Z).infinityNorm() > tol)
271 x =
x1 +
Z*(Q*
Z).solveBySVD(r - Q*
x1);
332 std::cout <<
"vpQuadProg::solveQPe: equality constraint infeasible" << std::endl;
396 return solveQPi(Q, r, C, d, x,
false, tol);
402 std::cout <<
"vpQuadProg::solveQP: equality constraint infeasible" << std::endl;
416 std::cout <<
"vpQuadProg::solveQP: inequality constraint infeasible" << std::endl;
470 unsigned int n =
checkDimensions(Q, r,
nullptr,
nullptr, &C, &d,
"solveQPi");
487 std::cout <<
"vpQuadProg::solveQPi: inequality constraint infeasible" << std::endl;
491 std::cout <<
"vpQuadProg::solveQPi: use_equality before setEqualityConstraint" << std::endl;
494 const unsigned int p = C.
getRows();
515 std::cout <<
"vpQuadProg::solveQPi: some constraints have been removed since last call\n";
523 for(
unsigned int i = 0; i <
active.size(); ++i)
525 for(
unsigned int j = 0; j < n; ++j)
526 A[i][j] = C[
active[i]][j];
545 for(
unsigned int i = 0; i < p; ++i)
552 for(
unsigned int i = 0; i < k; ++i)
559 for(
unsigned int i = 0; i < p; ++i)
562 for(
unsigned int j = 0; j < n; ++j)
564 A_lp[i][j] = C[i][j];
565 A_lp[i][n+j] = -C[i][j];
572 A_lp[i][2*n+p+l] = -1;
584 std::cout <<
"vpQuadProg::solveQPi: inequality constraints not feasible" << std::endl;
594 for(
unsigned int i = 0; i < p; ++i)
596 if(C.
getRow(i)*x - d[i] < -tol)
608 for(
unsigned int i = 0; i < p; ++i)
617 bool update_Ap =
true;
618 unsigned int last_active = C.
getRows();
627 for(
unsigned int i = 0; i <
active.size(); ++i)
629 for(
unsigned int j = 0; j < n; ++j)
630 A[i][j] = C[
active[i]][j];
633 if(update_Ap &&
active.size())
638 std::cout <<
"vpQuadProg::solveQPi: QP seems infeasible, too many constraints activated\n";
646 unsigned int ineqInd = (
unsigned int)
active.size();
651 double ineqMax = -tol;
652 for(
unsigned int i = 0; i < mu.getRows(); ++i)
654 if(mu[i] < ineqMax &&
active[i] != last_active)
662 if(ineqInd ==
active.size())
675 unsigned int ineqInd = 0;
678 for(
unsigned int i = 0; i <
inactive.size(); ++i)
733 void dummy_vpQuadProg(){};
bool solveQP(const vpMatrix &Q, const vpColVector &r, vpMatrix A, vpColVector b, const vpMatrix &C, const vpColVector &d, vpColVector &x, const double &tol=1e-6)
bool solveQPi(const vpMatrix &Q, const vpColVector &r, const vpMatrix &C, const vpColVector &d, vpColVector &x, bool use_equality=false, const double &tol=1e-6)
bool solveQPe(const vpMatrix &Q, const vpColVector &r, vpColVector &x, const double &tol=1e-6) const
vpRowVector getRow(unsigned int i) const
std::vector< unsigned int > inactive
std::vector< unsigned int > active
static bool allLesser(const vpMatrix &C, const vpColVector &x, const vpColVector &d, const double &thr=1e-6)
void solveByQR(const vpColVector &b, vpColVector &x) const
vpColVector extract(unsigned int r, unsigned int colsize) const
static bool allZero(const vpColVector &x, const double &tol=1e-6)
@ dimensionError
Bad dimension.
Implementation of column vector and the associated operations.
void solveBySVD(const vpColVector &B, vpColVector &x) const
static void fromCanonicalCost(const vpMatrix &H, const vpColVector &c, vpMatrix &Q, vpColVector &r, const double &tol=1e-6)
static vpColVector solveSVDorQR(const vpMatrix &A, const vpColVector &b)
Implementation of a matrix and operations on matrices.
static bool allGreater(const vpColVector &x, const double &thr=1e-6)
unsigned int getCols() const
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
vpColVector eigenValues() const
void resize(unsigned int i, bool flagNullify=true)
static bool colReduction(vpMatrix &A, vpColVector &b, bool full_rank=false, const double &tol=1e-6)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
bool setEqualityConstraint(const vpMatrix &A, const vpColVector &b, const double &tol=1e-6)
static unsigned int checkDimensions(const vpMatrix &Q, const vpColVector &r, const vpMatrix *A, const vpColVector *b, const vpMatrix *C, const vpColVector *d, const std::string fct)
@ functionNotImplementedError
Function not implemented.
error that can be emited by ViSP classes.
vpMatrix transpose() const
static bool solveByProjection(const vpMatrix &Q, const vpColVector &r, vpMatrix &A, vpColVector &b, vpColVector &x, const double &tol=1e-6)
static bool simplex(const vpColVector &c, vpMatrix A, vpColVector b, vpColVector &x, const double &tol=1e-6)
unsigned int getRows() const