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< bubMatrixQRWidebubMatrixQR
 
typedef bub::matrix_range< const bubMatrixQRWidebubMatrixQRConst
 
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< bubMatrixbubMatrixOrdered
 
typedef bub::matrix_indirect< bubMatrixQRWidebubMatrixQRWideOrdered
 
typedef bub::matrix_indirect< bubMatrixQRbubMatrixQROrdered
 
typedef bub::triangular_adaptor< bubMatrixQROrdered, bub::upper > bubMatrixQROrderedTri
 
typedef bub::matrix_indirect< const bubMatrixQRWidebubMatrixQRWideOrderedConst
 
typedef bub::matrix_indirect< bubMatrixQRConstbubMatrixQROrderedConst
 
typedef bub::triangular_adaptor< bubMatrixQROrderedConst, bub::upper > bubMatrixQROrderedTriConst
 
typedef std::vector< ConstraintMemConstraintList
 
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

◆ bubMatrixOrdered

◆ bubMatrixQR

◆ bubMatrixQRConst

◆ bubMatrixQROrdered

◆ bubMatrixQROrderedConst

◆ bubMatrixQROrderedTri

◆ bubMatrixQROrderedTriConst

◆ bubMatrixQRTri

◆ bubMatrixQRTriConst

◆ bubMatrixQRWide

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

◆ bubMatrixQRWideOrdered

◆ bubMatrixQRWideOrderedConst

◆ bubOrder

◆ ConstraintList

◆ ConstraintRefList

Member Enumeration Documentation

◆ ActivationTodoType

Enumerator
TODO_NOTHING 
TODO_ACTIVATE 
TODO_INACTIVATE 

Constructor & Destructor Documentation

◆ SolverHierarchicalInequalities()

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

References constraintH, nJ, Qh, and Rh.

Member Function Documentation

◆ accessRh()

bub::triangular_adaptor<bub::matrix_range< bubMatrix >,bub::upper> dynamicgraph::sot::SolverHierarchicalInequalities::accessRh ( void  )

◆ accessRhConst()

bub::triangular_adaptor<bub::matrix_range< const bubMatrix >,bub::upper> dynamicgraph::sot::SolverHierarchicalInequalities::accessRhConst ( void  ) const

◆ accessRsConst()

bubMatrixQROrderedTriConst dynamicgraph::sot::SolverHierarchicalInequalities::accessRsConst ( void  ) const

◆ applyFreeSpaceMotion()

void dynamicgraph::sot::SolverHierarchicalInequalities::applyFreeSpaceMotion ( const bubVector _du)

◆ computeDifferentialCondition()

void dynamicgraph::sot::SolverHierarchicalInequalities::computeDifferentialCondition ( void  )

◆ computeGradient()

void dynamicgraph::sot::SolverHierarchicalInequalities::computeGradient ( bubVector gradientWide)

◆ computeLagrangian()

void dynamicgraph::sot::SolverHierarchicalInequalities::computeLagrangian ( void  )

◆ computePrimal()

void dynamicgraph::sot::SolverHierarchicalInequalities::computePrimal ( void  )

◆ computeSlack()

void dynamicgraph::sot::SolverHierarchicalInequalities::computeSlack ( void  )

◆ displayConstraint()

static void dynamicgraph::sot::SolverHierarchicalInequalities::displayConstraint ( ConstraintList cs)
static

◆ downdateConstraintHierarchic()

void dynamicgraph::sot::SolverHierarchicalInequalities::downdateConstraintHierarchic ( const unsigned int  kdown)

◆ downdateConstraintSlack()

void dynamicgraph::sot::SolverHierarchicalInequalities::downdateConstraintSlack ( const unsigned int  kdown)

◆ forceDowndateHierachic()

void dynamicgraph::sot::SolverHierarchicalInequalities::forceDowndateHierachic ( ConstraintRefList toDowndate)

◆ forceUpdateHierachic()

void dynamicgraph::sot::SolverHierarchicalInequalities::forceUpdateHierachic ( ConstraintRefList toUpdate,
const ConstraintMem::BoundSideVector boundSide 
)

◆ freerange()

bub::range dynamicgraph::sot::SolverHierarchicalInequalities::freerange ( void  ) const
inline

◆ freeranges()

bub::range dynamicgraph::sot::SolverHierarchicalInequalities::freeranges ( void  ) const
inline

◆ fullrange()

bub::range dynamicgraph::sot::SolverHierarchicalInequalities::fullrange ( void  ) const
inline

◆ getDifferentialU0()

const bubVector& dynamicgraph::sot::SolverHierarchicalInequalities::getDifferentialU0 ( void  ) const
inline

◆ getNbDof()

unsigned int dynamicgraph::sot::SolverHierarchicalInequalities::getNbDof ( void  )
inline

◆ getSlackActiveSet()

const std::vector<ConstraintRef>& dynamicgraph::sot::SolverHierarchicalInequalities::getSlackActiveSet ( void  ) const
inline

◆ getToActivateList()

const std::vector<ConstraintRef>& dynamicgraph::sot::SolverHierarchicalInequalities::getToActivateList ( void  ) const
inline

◆ getToInactivateList()

const std::vector<ConstraintRef>& dynamicgraph::sot::SolverHierarchicalInequalities::getToInactivateList ( void  ) const
inline

◆ initConstraintSize()

void dynamicgraph::sot::SolverHierarchicalInequalities::initConstraintSize ( const unsigned int  size)

◆ initializeConstraintMemory()

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 
)

◆ initializeDecompositionSlack()

void dynamicgraph::sot::SolverHierarchicalInequalities::initializeDecompositionSlack ( void  )

◆ printDebug()

void dynamicgraph::sot::SolverHierarchicalInequalities::printDebug ( void  )

◆ printDifferentialCondition()

void dynamicgraph::sot::SolverHierarchicalInequalities::printDifferentialCondition ( std::ostream &  os) const

◆ pushBackSlackToHierarchy()

void dynamicgraph::sot::SolverHierarchicalInequalities::pushBackSlackToHierarchy ( void  )

◆ rangeh()

bub::range dynamicgraph::sot::SolverHierarchicalInequalities::rangeh ( void  ) const
inline

◆ rangehs()

bub::range dynamicgraph::sot::SolverHierarchicalInequalities::rangehs ( void  ) const
inline

◆ rankDetermination()

template<typename bubTemplateMatrix >
unsigned int dynamicgraph::sot::SolverHierarchicalInequalities::rankDetermination ( const bubTemplateMatrix &  A,
const double  threshold = THRESHOLD_ZERO 
)

◆ recordInitialConditions()

void dynamicgraph::sot::SolverHierarchicalInequalities::recordInitialConditions ( void  )

◆ regularizeQhJs()

void dynamicgraph::sot::SolverHierarchicalInequalities::regularizeQhJs ( void  )

◆ regularizeQhJsU()

void dynamicgraph::sot::SolverHierarchicalInequalities::regularizeQhJsU ( void  )

◆ selecActivationHierarchic()

bool dynamicgraph::sot::SolverHierarchicalInequalities::selecActivationHierarchic ( double &  tau)

◆ selecActivationSlack()

bool dynamicgraph::sot::SolverHierarchicalInequalities::selecActivationSlack ( void  )

◆ selecInactivationHierarchic()

bool dynamicgraph::sot::SolverHierarchicalInequalities::selecInactivationHierarchic ( void  )

◆ selecInactivationSlack()

bool dynamicgraph::sot::SolverHierarchicalInequalities::selecInactivationSlack ( void  )

◆ setInitialCondition()

void dynamicgraph::sot::SolverHierarchicalInequalities::setInitialCondition ( const bubVector _u0,
const unsigned int  _rankh 
)

◆ setInitialConditionVoid()

void dynamicgraph::sot::SolverHierarchicalInequalities::setInitialConditionVoid ( void  )

◆ setNbDof()

void dynamicgraph::sot::SolverHierarchicalInequalities::setNbDof ( const unsigned int  nJ)

◆ solve() [1/2]

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 
)

◆ solve() [2/2]

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 
)

◆ updateConstraintHierarchic()

void dynamicgraph::sot::SolverHierarchicalInequalities::updateConstraintHierarchic ( const unsigned int  constraintId,
const ConstraintMem::BoundSideType  side 
)

◆ updateConstraintSlack()

void dynamicgraph::sot::SolverHierarchicalInequalities::updateConstraintSlack ( const unsigned int  kup,
const ConstraintMem::BoundSideType  activeSide 
)

◆ updateRankOneDowndate()

void dynamicgraph::sot::SolverHierarchicalInequalities::updateRankOneDowndate ( void  )

◆ updateRankOneUpdate()

void dynamicgraph::sot::SolverHierarchicalInequalities::updateRankOneUpdate ( void  )

◆ warmStart()

void dynamicgraph::sot::SolverHierarchicalInequalities::warmStart ( void  )

Member Data Documentation

◆ constraintH

ConstraintList& dynamicgraph::sot::SolverHierarchicalInequalities::constraintH

◆ constraintS

ConstraintList dynamicgraph::sot::SolverHierarchicalInequalities::constraintS

◆ constraintSactive

ConstraintRefList dynamicgraph::sot::SolverHierarchicalInequalities::constraintSactive

◆ du

bubVector dynamicgraph::sot::SolverHierarchicalInequalities::du

◆ du0

bubVector dynamicgraph::sot::SolverHierarchicalInequalities::du0

◆ freeRank

unsigned int dynamicgraph::sot::SolverHierarchicalInequalities::freeRank

◆ freeRankChange

int dynamicgraph::sot::SolverHierarchicalInequalities::freeRankChange

◆ Hactivation

bool dynamicgraph::sot::SolverHierarchicalInequalities::Hactivation

◆ HactivationRef

unsigned int dynamicgraph::sot::SolverHierarchicalInequalities::HactivationRef

◆ HactivationScore

double dynamicgraph::sot::SolverHierarchicalInequalities::HactivationScore

◆ HactivationSide

ConstraintMem::BoundSideType dynamicgraph::sot::SolverHierarchicalInequalities::HactivationSide

◆ Hinactivation

bool dynamicgraph::sot::SolverHierarchicalInequalities::Hinactivation

◆ HinactivationRef

unsigned int dynamicgraph::sot::SolverHierarchicalInequalities::HinactivationRef

◆ HinactivationScore

double dynamicgraph::sot::SolverHierarchicalInequalities::HinactivationScore

◆ initialActiveH

std::vector<bool> dynamicgraph::sot::SolverHierarchicalInequalities::initialActiveH

◆ initialSideH

ConstraintMem::BoundSideVector dynamicgraph::sot::SolverHierarchicalInequalities::initialSideH

◆ lagrangian

bubVector dynamicgraph::sot::SolverHierarchicalInequalities::lagrangian

◆ lastRotation

sotRotationComposed dynamicgraph::sot::SolverHierarchicalInequalities::lastRotation

◆ nJ

unsigned int dynamicgraph::sot::SolverHierarchicalInequalities::nJ

◆ orderS

bubOrder dynamicgraph::sot::SolverHierarchicalInequalities::orderS

◆ Qh

sotRotationComposedInExtenso& dynamicgraph::sot::SolverHierarchicalInequalities::Qh

◆ QhJs

bubMatrixQRWide dynamicgraph::sot::SolverHierarchicalInequalities::QhJs

◆ QhJsU

bubMatrixQRWide dynamicgraph::sot::SolverHierarchicalInequalities::QhJsU

◆ rankh

unsigned int dynamicgraph::sot::SolverHierarchicalInequalities::rankh

◆ ranks

unsigned int dynamicgraph::sot::SolverHierarchicalInequalities::ranks

◆ Rh

bubMatrix& dynamicgraph::sot::SolverHierarchicalInequalities::Rh

◆ Sactivation

bool dynamicgraph::sot::SolverHierarchicalInequalities::Sactivation

◆ SactivationRef

unsigned int dynamicgraph::sot::SolverHierarchicalInequalities::SactivationRef

◆ SactivationScore

double dynamicgraph::sot::SolverHierarchicalInequalities::SactivationScore

◆ SactivationSide

ConstraintMem::BoundSideType dynamicgraph::sot::SolverHierarchicalInequalities::SactivationSide

◆ Sinactivation

bool dynamicgraph::sot::SolverHierarchicalInequalities::Sinactivation

◆ SinactivationRef

unsigned int dynamicgraph::sot::SolverHierarchicalInequalities::SinactivationRef

◆ SinactivationScore

double dynamicgraph::sot::SolverHierarchicalInequalities::SinactivationScore

◆ sizes

unsigned int dynamicgraph::sot::SolverHierarchicalInequalities::sizes

◆ slackActiveSet

std::vector<ConstraintRef> dynamicgraph::sot::SolverHierarchicalInequalities::slackActiveSet

◆ slackInf

bubVector dynamicgraph::sot::SolverHierarchicalInequalities::slackInf

◆ slackSup

bubVector dynamicgraph::sot::SolverHierarchicalInequalities::slackSup

◆ THRESHOLD_ZERO

double dynamicgraph::sot::SolverHierarchicalInequalities::THRESHOLD_ZERO
static

◆ toActivate

std::vector<ConstraintRef> dynamicgraph::sot::SolverHierarchicalInequalities::toActivate

◆ toInactivate

std::vector<ConstraintRef> dynamicgraph::sot::SolverHierarchicalInequalities::toInactivate

◆ u0

bubVector dynamicgraph::sot::SolverHierarchicalInequalities::u0

◆ warmStartReady

bool dynamicgraph::sot::SolverHierarchicalInequalities::warmStartReady