#include <sot/core/solver-hierarchical-inequalities.hh>
Public Types | |
enum | ActivationTodoType { TODO_NOTHING, TODO_ACTIVATE, TODO_INACTIVATE } |
typedef bub::matrix< double, bub::column_major > | bubMatrixQRWide |
typedef bub::matrix_range< bubMatrixQRWide > | bubMatrixQR |
typedef bub::matrix_range< const bubMatrixQRWide > | bubMatrixQRConst |
typedef bub::triangular_adaptor< bubMatrixQR, bub::upper > | bubMatrixQRTri |
typedef bub::triangular_adaptor< bubMatrixQRConst, bub::upper > | bubMatrixQRTriConst |
typedef bub::indirect_array | bubOrder |
typedef bub::matrix_indirect< bubMatrix > | bubMatrixOrdered |
typedef bub::matrix_indirect< bubMatrixQRWide > | bubMatrixQRWideOrdered |
typedef bub::matrix_indirect< bubMatrixQR > | bubMatrixQROrdered |
typedef bub::triangular_adaptor< bubMatrixQROrdered, bub::upper > | bubMatrixQROrderedTri |
typedef bub::matrix_indirect< const bubMatrixQRWide > | bubMatrixQRWideOrderedConst |
typedef bub::matrix_indirect< bubMatrixQRConst > | bubMatrixQROrderedConst |
typedef bub::triangular_adaptor< bubMatrixQROrderedConst, bub::upper > | bubMatrixQROrderedTriConst |
typedef std::vector< ConstraintMem > | ConstraintList |
typedef std::vector< ConstraintMem * > | ConstraintRefList |
Public Member Functions | |
SolverHierarchicalInequalities (unsigned int _nJ, sotRotationComposedInExtenso &_Qh, bubMatrix &_Rh, ConstraintList &_cH) | |
void | initConstraintSize (const unsigned int size) |
void | setInitialCondition (const bubVector &_u0, const unsigned int _rankh) |
void | setInitialConditionVoid (void) |
void | setNbDof (const unsigned int nJ) |
unsigned int | getNbDof (void) |
void | recordInitialConditions (void) |
void | computeDifferentialCondition (void) |
const std::vector< ConstraintRef > & | getToActivateList (void) const |
const std::vector< ConstraintRef > & | getToInactivateList (void) const |
const bubVector & | getDifferentialU0 (void) const |
const std::vector< ConstraintRef > & | getSlackActiveSet (void) const |
void | printDifferentialCondition (std::ostream &os) const |
bub::range | fullrange (void) const |
bub::range | rangeh (void) const |
bub::range | rangehs (void) const |
bub::range | freerange (void) const |
bub::range | freeranges (void) const |
bubMatrixQROrderedTriConst | accessRsConst (void) const |
bub::triangular_adaptor< bub::matrix_range< const bubMatrix >, bub::upper > | accessRhConst (void) const |
bub::triangular_adaptor< bub::matrix_range< bubMatrix >, bub::upper > | accessRh (void) |
template<typename bubTemplateMatrix > | |
unsigned int | rankDetermination (const bubTemplateMatrix &A, const double threshold=THRESHOLD_ZERO) |
void | printDebug (void) |
void | warmStart (void) |
void | applyFreeSpaceMotion (const bubVector &_du) |
void | forceUpdateHierachic (ConstraintRefList &toUpdate, const ConstraintMem::BoundSideVector &boundSide) |
void | forceDowndateHierachic (ConstraintRefList &toDowndate) |
void | solve (const bubMatrix &Jse, const bubVector &ese, const bubMatrix &Jsi, const bubVector &esiInf, const bubVector &esiSup, const std::vector< ConstraintMem::BoundSideType > esiBoundSide, bool pushBackAtTheEnd=true) |
void | solve (const bubMatrix &Jse, const bubVector &ese, const bubMatrix &Jsi, const bubVector &esiInf, const bubVector &esiSup, const ConstraintMem::BoundSideVector &esiBoundSide, const std::vector< ConstraintRef > &slackActiveWarmStart, bool pushBackAtTheEnd=true) |
void | initializeConstraintMemory (const bubMatrix &Jse, const bubVector &ese, const bubMatrix &Jsi, const bubVector &esiInf, const bubVector &esiSup, const ConstraintMem::BoundSideVector &esiBoundSide, const std::vector< ConstraintRef > &warmStartSide) |
void | initializeDecompositionSlack (void) |
void | updateConstraintHierarchic (const unsigned int constraintId, const ConstraintMem::BoundSideType side) |
void | downdateConstraintHierarchic (const unsigned int kdown) |
void | updateRankOneDowndate (void) |
void | updateRankOneUpdate (void) |
void | updateConstraintSlack (const unsigned int kup, const ConstraintMem::BoundSideType activeSide) |
void | regularizeQhJs (void) |
void | regularizeQhJsU (void) |
void | downdateConstraintSlack (const unsigned int kdown) |
void | computeGradient (bubVector &gradientWide) |
void | computePrimal (void) |
void | computeSlack (void) |
void | computeLagrangian (void) |
bool | selecActivationHierarchic (double &tau) |
bool | selecInactivationHierarchic (void) |
bool | selecActivationSlack (void) |
bool | selecInactivationSlack (void) |
void | pushBackSlackToHierarchy (void) |
Static Public Member Functions | |
static void | displayConstraint (ConstraintList &cs) |
Static Public Attributes | |
static double | THRESHOLD_ZERO |
typedef bub::matrix_indirect<bubMatrix> dynamicgraph::sot::SolverHierarchicalInequalities::bubMatrixOrdered |
typedef bub::matrix_range<bubMatrixQRWide> dynamicgraph::sot::SolverHierarchicalInequalities::bubMatrixQR |
typedef bub::matrix_range<const bubMatrixQRWide> dynamicgraph::sot::SolverHierarchicalInequalities::bubMatrixQRConst |
typedef bub::matrix_indirect<bubMatrixQR> dynamicgraph::sot::SolverHierarchicalInequalities::bubMatrixQROrdered |
typedef bub::matrix_indirect<bubMatrixQRConst> dynamicgraph::sot::SolverHierarchicalInequalities::bubMatrixQROrderedConst |
typedef bub::triangular_adaptor<bubMatrixQROrdered,bub::upper> dynamicgraph::sot::SolverHierarchicalInequalities::bubMatrixQROrderedTri |
typedef bub::triangular_adaptor<bubMatrixQROrderedConst,bub::upper> dynamicgraph::sot::SolverHierarchicalInequalities::bubMatrixQROrderedTriConst |
typedef bub::triangular_adaptor<bubMatrixQR,bub::upper> dynamicgraph::sot::SolverHierarchicalInequalities::bubMatrixQRTri |
typedef bub::triangular_adaptor<bubMatrixQRConst,bub::upper> dynamicgraph::sot::SolverHierarchicalInequalities::bubMatrixQRTriConst |
typedef bub::matrix<double,bub::column_major> dynamicgraph::sot::SolverHierarchicalInequalities::bubMatrixQRWide |
typedef bub::matrix_indirect<bubMatrixQRWide> dynamicgraph::sot::SolverHierarchicalInequalities::bubMatrixQRWideOrdered |
typedef bub::matrix_indirect<const bubMatrixQRWide> dynamicgraph::sot::SolverHierarchicalInequalities::bubMatrixQRWideOrderedConst |
typedef bub::indirect_array dynamicgraph::sot::SolverHierarchicalInequalities::bubOrder |
typedef std::vector<ConstraintMem> dynamicgraph::sot::SolverHierarchicalInequalities::ConstraintList |
typedef std::vector<ConstraintMem*> dynamicgraph::sot::SolverHierarchicalInequalities::ConstraintRefList |
|
inline |
References constraintH, nJ, Qh, and Rh.
bub::triangular_adaptor<bub::matrix_range< bubMatrix >,bub::upper> dynamicgraph::sot::SolverHierarchicalInequalities::accessRh | ( | void | ) |
bub::triangular_adaptor<bub::matrix_range< const bubMatrix >,bub::upper> dynamicgraph::sot::SolverHierarchicalInequalities::accessRhConst | ( | void | ) | const |
bubMatrixQROrderedTriConst dynamicgraph::sot::SolverHierarchicalInequalities::accessRsConst | ( | void | ) | const |
void dynamicgraph::sot::SolverHierarchicalInequalities::applyFreeSpaceMotion | ( | const bubVector & | _du | ) |
void dynamicgraph::sot::SolverHierarchicalInequalities::computeDifferentialCondition | ( | void | ) |
void dynamicgraph::sot::SolverHierarchicalInequalities::computeGradient | ( | bubVector & | gradientWide | ) |
void dynamicgraph::sot::SolverHierarchicalInequalities::computeLagrangian | ( | void | ) |
void dynamicgraph::sot::SolverHierarchicalInequalities::computePrimal | ( | void | ) |
void dynamicgraph::sot::SolverHierarchicalInequalities::computeSlack | ( | void | ) |
|
static |
void dynamicgraph::sot::SolverHierarchicalInequalities::downdateConstraintHierarchic | ( | const unsigned int | kdown | ) |
void dynamicgraph::sot::SolverHierarchicalInequalities::downdateConstraintSlack | ( | const unsigned int | kdown | ) |
void dynamicgraph::sot::SolverHierarchicalInequalities::forceDowndateHierachic | ( | ConstraintRefList & | toDowndate | ) |
void dynamicgraph::sot::SolverHierarchicalInequalities::forceUpdateHierachic | ( | ConstraintRefList & | toUpdate, |
const ConstraintMem::BoundSideVector & | boundSide | ||
) |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
void dynamicgraph::sot::SolverHierarchicalInequalities::initConstraintSize | ( | const unsigned int | size | ) |
void dynamicgraph::sot::SolverHierarchicalInequalities::initializeConstraintMemory | ( | const bubMatrix & | Jse, |
const bubVector & | ese, | ||
const bubMatrix & | Jsi, | ||
const bubVector & | esiInf, | ||
const bubVector & | esiSup, | ||
const ConstraintMem::BoundSideVector & | esiBoundSide, | ||
const std::vector< ConstraintRef > & | warmStartSide | ||
) |
void dynamicgraph::sot::SolverHierarchicalInequalities::initializeDecompositionSlack | ( | void | ) |
void dynamicgraph::sot::SolverHierarchicalInequalities::printDebug | ( | void | ) |
void dynamicgraph::sot::SolverHierarchicalInequalities::printDifferentialCondition | ( | std::ostream & | os | ) | const |
void dynamicgraph::sot::SolverHierarchicalInequalities::pushBackSlackToHierarchy | ( | void | ) |
|
inline |
|
inline |
unsigned int dynamicgraph::sot::SolverHierarchicalInequalities::rankDetermination | ( | const bubTemplateMatrix & | A, |
const double | threshold = THRESHOLD_ZERO |
||
) |
void dynamicgraph::sot::SolverHierarchicalInequalities::recordInitialConditions | ( | void | ) |
void dynamicgraph::sot::SolverHierarchicalInequalities::regularizeQhJs | ( | void | ) |
void dynamicgraph::sot::SolverHierarchicalInequalities::regularizeQhJsU | ( | void | ) |
bool dynamicgraph::sot::SolverHierarchicalInequalities::selecActivationHierarchic | ( | double & | tau | ) |
bool dynamicgraph::sot::SolverHierarchicalInequalities::selecActivationSlack | ( | void | ) |
bool dynamicgraph::sot::SolverHierarchicalInequalities::selecInactivationHierarchic | ( | void | ) |
bool dynamicgraph::sot::SolverHierarchicalInequalities::selecInactivationSlack | ( | void | ) |
void dynamicgraph::sot::SolverHierarchicalInequalities::setInitialCondition | ( | const bubVector & | _u0, |
const unsigned int | _rankh | ||
) |
void dynamicgraph::sot::SolverHierarchicalInequalities::setInitialConditionVoid | ( | void | ) |
void dynamicgraph::sot::SolverHierarchicalInequalities::setNbDof | ( | const unsigned int | nJ | ) |
void dynamicgraph::sot::SolverHierarchicalInequalities::solve | ( | const bubMatrix & | Jse, |
const bubVector & | ese, | ||
const bubMatrix & | Jsi, | ||
const bubVector & | esiInf, | ||
const bubVector & | esiSup, | ||
const std::vector< ConstraintMem::BoundSideType > | esiBoundSide, | ||
bool | pushBackAtTheEnd = true |
||
) |
void dynamicgraph::sot::SolverHierarchicalInequalities::solve | ( | const bubMatrix & | Jse, |
const bubVector & | ese, | ||
const bubMatrix & | Jsi, | ||
const bubVector & | esiInf, | ||
const bubVector & | esiSup, | ||
const ConstraintMem::BoundSideVector & | esiBoundSide, | ||
const std::vector< ConstraintRef > & | slackActiveWarmStart, | ||
bool | pushBackAtTheEnd = true |
||
) |
void dynamicgraph::sot::SolverHierarchicalInequalities::updateConstraintHierarchic | ( | const unsigned int | constraintId, |
const ConstraintMem::BoundSideType | side | ||
) |
void dynamicgraph::sot::SolverHierarchicalInequalities::updateConstraintSlack | ( | const unsigned int | kup, |
const ConstraintMem::BoundSideType | activeSide | ||
) |
void dynamicgraph::sot::SolverHierarchicalInequalities::updateRankOneDowndate | ( | void | ) |
void dynamicgraph::sot::SolverHierarchicalInequalities::updateRankOneUpdate | ( | void | ) |
void dynamicgraph::sot::SolverHierarchicalInequalities::warmStart | ( | void | ) |
ConstraintList& dynamicgraph::sot::SolverHierarchicalInequalities::constraintH |
Referenced by SolverHierarchicalInequalities().
ConstraintList dynamicgraph::sot::SolverHierarchicalInequalities::constraintS |
ConstraintRefList dynamicgraph::sot::SolverHierarchicalInequalities::constraintSactive |
bubVector dynamicgraph::sot::SolverHierarchicalInequalities::du |
bubVector dynamicgraph::sot::SolverHierarchicalInequalities::du0 |
unsigned int dynamicgraph::sot::SolverHierarchicalInequalities::freeRank |
int dynamicgraph::sot::SolverHierarchicalInequalities::freeRankChange |
bool dynamicgraph::sot::SolverHierarchicalInequalities::Hactivation |
unsigned int dynamicgraph::sot::SolverHierarchicalInequalities::HactivationRef |
double dynamicgraph::sot::SolverHierarchicalInequalities::HactivationScore |
ConstraintMem::BoundSideType dynamicgraph::sot::SolverHierarchicalInequalities::HactivationSide |
bool dynamicgraph::sot::SolverHierarchicalInequalities::Hinactivation |
unsigned int dynamicgraph::sot::SolverHierarchicalInequalities::HinactivationRef |
double dynamicgraph::sot::SolverHierarchicalInequalities::HinactivationScore |
std::vector<bool> dynamicgraph::sot::SolverHierarchicalInequalities::initialActiveH |
ConstraintMem::BoundSideVector dynamicgraph::sot::SolverHierarchicalInequalities::initialSideH |
bubVector dynamicgraph::sot::SolverHierarchicalInequalities::lagrangian |
sotRotationComposed dynamicgraph::sot::SolverHierarchicalInequalities::lastRotation |
unsigned int dynamicgraph::sot::SolverHierarchicalInequalities::nJ |
Referenced by SolverHierarchicalInequalities().
bubOrder dynamicgraph::sot::SolverHierarchicalInequalities::orderS |
sotRotationComposedInExtenso& dynamicgraph::sot::SolverHierarchicalInequalities::Qh |
Referenced by SolverHierarchicalInequalities().
bubMatrixQRWide dynamicgraph::sot::SolverHierarchicalInequalities::QhJs |
bubMatrixQRWide dynamicgraph::sot::SolverHierarchicalInequalities::QhJsU |
unsigned int dynamicgraph::sot::SolverHierarchicalInequalities::rankh |
unsigned int dynamicgraph::sot::SolverHierarchicalInequalities::ranks |
bubMatrix& dynamicgraph::sot::SolverHierarchicalInequalities::Rh |
Referenced by SolverHierarchicalInequalities().
bool dynamicgraph::sot::SolverHierarchicalInequalities::Sactivation |
unsigned int dynamicgraph::sot::SolverHierarchicalInequalities::SactivationRef |
double dynamicgraph::sot::SolverHierarchicalInequalities::SactivationScore |
ConstraintMem::BoundSideType dynamicgraph::sot::SolverHierarchicalInequalities::SactivationSide |
bool dynamicgraph::sot::SolverHierarchicalInequalities::Sinactivation |
unsigned int dynamicgraph::sot::SolverHierarchicalInequalities::SinactivationRef |
double dynamicgraph::sot::SolverHierarchicalInequalities::SinactivationScore |
unsigned int dynamicgraph::sot::SolverHierarchicalInequalities::sizes |
std::vector<ConstraintRef> dynamicgraph::sot::SolverHierarchicalInequalities::slackActiveSet |
bubVector dynamicgraph::sot::SolverHierarchicalInequalities::slackInf |
bubVector dynamicgraph::sot::SolverHierarchicalInequalities::slackSup |
|
static |
std::vector<ConstraintRef> dynamicgraph::sot::SolverHierarchicalInequalities::toActivate |
std::vector<ConstraintRef> dynamicgraph::sot::SolverHierarchicalInequalities::toInactivate |
bubVector dynamicgraph::sot::SolverHierarchicalInequalities::u0 |
bool dynamicgraph::sot::SolverHierarchicalInequalities::warmStartReady |