All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
dynamicgraph::sot::SolverHierarchicalInequalities Class Reference

#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 bubVectorgetDifferentialU0 (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)
 

Public Attributes

unsigned int nJ
 
bubVector u0
 
sotRotationComposedInExtensoQh
 
bubMatrixRh
 
unsigned int rankh
 
unsigned int freeRank
 
ConstraintListconstraintH
 
bool Hactivation
 
bool Hinactivation
 
unsigned int HactivationRef
 
unsigned int HinactivationRef
 
ConstraintMem::BoundSideType HactivationSide
 
bubMatrixQRWide QhJsU
 
bubMatrixQRWide QhJs
 
unsigned int ranks
 
unsigned int sizes
 
bubOrder orderS
 
ConstraintList constraintS
 
ConstraintRefList constraintSactive
 
bool Sactivation
 
bool Sinactivation
 
unsigned int SactivationRef
 
unsigned int SinactivationRef
 
ConstraintMem::BoundSideType SactivationSide
 
double SactivationScore
 
double SinactivationScore
 
double HactivationScore
 
double HinactivationScore
 
bubVector du
 
bubVector slackInf
 
bubVector slackSup
 
bubVector lagrangian
 
sotRotationComposed lastRotation
 
int freeRankChange
 
std::vector< bool > initialActiveH
 
ConstraintMem::BoundSideVector initialSideH
 
bubVector du0
 
std::vector< ConstraintRefslackActiveSet
 
std::vector< ConstraintReftoActivate
 
std::vector< ConstraintReftoInactivate
 
bool warmStartReady
 

Static Public Attributes

static double THRESHOLD_ZERO
 

Member Typedef Documentation

typedef bub::matrix<double,bub::column_major> dynamicgraph::sot::SolverHierarchicalInequalities::bubMatrixQRWide

Member Enumeration Documentation

Enumerator
TODO_NOTHING 
TODO_ACTIVATE 
TODO_INACTIVATE 

Constructor & Destructor Documentation

dynamicgraph::sot::SolverHierarchicalInequalities::SolverHierarchicalInequalities ( unsigned int  _nJ,
sotRotationComposedInExtenso _Qh,
bubMatrix _Rh,
ConstraintList _cH 
)
inline

Member Function Documentation

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::displayConstraint ( ConstraintList cs)
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 
)
bub::range dynamicgraph::sot::SolverHierarchicalInequalities::freerange ( void  ) const
inline
bub::range dynamicgraph::sot::SolverHierarchicalInequalities::freeranges ( void  ) const
inline
bub::range dynamicgraph::sot::SolverHierarchicalInequalities::fullrange ( void  ) const
inline
const bubVector& dynamicgraph::sot::SolverHierarchicalInequalities::getDifferentialU0 ( void  ) const
inline
unsigned int dynamicgraph::sot::SolverHierarchicalInequalities::getNbDof ( void  )
inline
const std::vector<ConstraintRef>& dynamicgraph::sot::SolverHierarchicalInequalities::getSlackActiveSet ( void  ) const
inline
const std::vector<ConstraintRef>& dynamicgraph::sot::SolverHierarchicalInequalities::getToActivateList ( void  ) const
inline
const std::vector<ConstraintRef>& dynamicgraph::sot::SolverHierarchicalInequalities::getToInactivateList ( void  ) const
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  )
bub::range dynamicgraph::sot::SolverHierarchicalInequalities::rangeh ( void  ) const
inline
bub::range dynamicgraph::sot::SolverHierarchicalInequalities::rangehs ( void  ) const
inline
template<typename bubTemplateMatrix >
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  )

Member Data Documentation

ConstraintList& dynamicgraph::sot::SolverHierarchicalInequalities::constraintH
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
bubOrder dynamicgraph::sot::SolverHierarchicalInequalities::orderS
sotRotationComposedInExtenso& dynamicgraph::sot::SolverHierarchicalInequalities::Qh
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
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
double dynamicgraph::sot::SolverHierarchicalInequalities::THRESHOLD_ZERO
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