Classes | Public Types | Public Member Functions | Protected Types | Protected Member Functions | Protected Attributes | Friends | List of all members
hpp::constraints::HierarchicalIterativeSolver Class Reference

#include <hpp/constraints/iterative-solver.hh>

Inheritance diagram for hpp::constraints::HierarchicalIterativeSolver:
[legend]
Collaboration diagram for hpp::constraints::HierarchicalIterativeSolver:
[legend]

Classes

struct  Data
 

Public Types

enum  Status {
  ERROR_INCREASED,
  MAX_ITERATION_REACHED,
  INFEASIBLE,
  SUCCESS
}
 
typedef Eigen::ColBlockIndices Reduction_t
 
typedef lineSearch::FixedSequence DefaultLineSearch
 
typedef boost::function< void(vectorIn_t from, vectorIn_t velocity, vectorOut_t result)> Integration_t
 This function integrates velocity during unit time, from argument. More...
 
typedef boost::function< bool(vectorIn_t result, Eigen::VectorXi &saturation)> Saturation_t
 This function checks what DoF are saturated. More...
 

Public Member Functions

 HierarchicalIterativeSolver (const std::size_t &argSize, const std::size_t derSize)
 
virtual ~HierarchicalIterativeSolver ()
 
value_type residualError () const
 Returns the squared norm of the error vector. More...
 
void residualError (vectorOut_t error) const
 Returns the error vector. More...
 
virtual std::ostream & print (std::ostream &os) const
 
Problem definition
void add (const DifferentiableFunctionPtr_t &f, const std::size_t &priority)
 Add an equality constraint to a priority. More...
 
void add (const DifferentiableFunctionPtr_t &f, const std::size_t &priority, const ComparisonTypes_t &comp)
 Add a constraint. More...
 
void integration (const Integration_t &integrate)
 Set the integration function. More...
 
const Integration_tintegration () const
 Get the integration function. More...
 
void saturation (const Saturation_t &saturate)
 Set the saturation function. More...
 
const Saturation_tsaturation () const
 Get the saturation function. More...
 
Problem resolution
template<typename LineSearchType >
Status solve (vectorOut_t arg, LineSearchType ls=LineSearchType()) const
 
Status solve (vectorOut_t arg) const
 
bool isSatisfied (vectorIn_t arg) const
 
const value_typesigma () const
 Returns the lowest singular value. More...
 
Parameters
void reduction (const segments_t intervals)
 Set the velocity variable that must be changed. More...
 
void reduction (const Reduction_t &reduction)
 Set the velocity variable that must be changed. More...
 
void maxIterations (size_type iterations)
 Set maximal number of iterations. More...
 
size_type maxIterations () const
 Get maximal number of iterations in config projector. More...
 
void errorThreshold (const value_type &threshold)
 Set error threshold. More...
 
value_type errorThreshold () const
 Get error threshold. More...
 
value_type squaredErrorThreshold () const
 Get error threshold. More...
 
value_type inequalityThreshold () const
 Get the inequality threshold. More...
 
void inequalityThreshold (const value_type &it)
 set the inequality threshold More...
 
void lastIsOptional (bool optional)
 
bool lastIsOptional () const
 
Stack
const DifferentiableFunctionStackstack (const std::size_t priority)
 
std::size_t numberStacks () const
 
const size_typedimension () const
 
const size_typereducedDimension () const
 Dimension of the problem after removing the rows of the jacobian which do not influence the error (only zeros along those lines). More...
 
ArrayXb activeParameters () const
 Configuration parameters involved in the constraint resolution. More...
 
ArrayXb activeDerivativeParameters () const
 Velocity parameters involved in the constraint resolution. More...
 
Right hand side accessors
vector_t rightHandSideFromInput (vectorIn_t arg)
 Compute a right hand side using the input arg. More...
 
bool rightHandSideFromInput (const DifferentiableFunctionPtr_t &f, vectorIn_t arg)
 
bool rightHandSide (const DifferentiableFunctionPtr_t &f, vectorIn_t rhs)
 
void rightHandSide (vectorIn_t rhs)
 Set the level set parameter. More...
 
vector_t rightHandSide () const
 Get the level set parameter. More...
 
size_type rightHandSideSize () const
 Get size of the level set parameter. More...
 
Access to internal datas

You should know what you do when you call these functions

template<bool ComputeJac>
void computeValue (vectorIn_t arg) const
 Compute the value of each level, and the jacobian if ComputeJac is true. More...
 
void computeSaturation (vectorIn_t arg) const
 
void getValue (vectorOut_t v) const
 
void getReducedJacobian (matrixOut_t J) const
 
void computeError () const
 If lastIsOptional() is true, then the last level is ignored. More...
 
const vector_tlastStep () const
 Accessor to the last step done. More...
 
virtual void integrate (vectorIn_t from, vectorIn_t velocity, vectorOut_t result) const
 

Protected Types

typedef Eigen::JacobiSVD< matrix_tSVD_t
 

Protected Member Functions

void update ()
 Allocate datas and update sizes of the problem Should be called whenever the stack is modified. More...
 
virtual void computeActiveRowsOfJ (std::size_t iStack)
 Compute which rows of the jacobian of stack_[iStack] are not zero, using the activeDerivativeParameters of the functions. More...
 
void computeDescentDirection () const
 Compute a SVD decomposition of each level and find the best descent direction at the first order. More...
 
void expandDqSmall () const
 
void saturate (vectorOut_t arg) const
 

Protected Attributes

value_type squaredErrorThreshold_
 
value_type inequalityThreshold_
 
size_type maxIterations_
 
std::vector< DifferentiableFunctionStackstacks_
 
size_type argSize_
 
size_type derSize_
 
size_type dimension_
 
size_type reducedDimension_
 
bool lastIsOptional_
 
Reduction_t reduction_
 
Integration_t integrate_
 
Saturation_t saturate_
 
value_type sigma_
 The smallest non-zero singular value. More...
 
vector_t dq_
 
vector_t dqSmall_
 
matrix_t projector_
 
matrix_t reducedJ_
 
Eigen::VectorXi saturation_
 
Eigen::VectorXi reducedSaturation_
 
ArrayXb tmpSat_
 
value_type squaredNorm_
 
std::vector< Datadatas_
 
SVD_t svd_
 
mutable ::hpp::statistics::SuccessStatistics statistics_
 

Friends

struct lineSearch::Backtracking
 

Member Typedef Documentation

◆ DefaultLineSearch

◆ Integration_t

This function integrates velocity during unit time, from argument.

It should be robust to cases where from and result points to the same vector in memory (aliasing)

◆ Reduction_t

◆ Saturation_t

typedef boost::function<bool (vectorIn_t result, Eigen::VectorXi& saturation)> hpp::constraints::HierarchicalIterativeSolver::Saturation_t

This function checks what DoF are saturated.

For each DoF, saturation is set to

  • -1 if the lower bound is reached.
  • 1 if the upper bound is reached.
  • 0 otherwise saturates velocity during unit time, from argument.

◆ SVD_t

Member Enumeration Documentation

◆ Status

Enumerator
ERROR_INCREASED 
MAX_ITERATION_REACHED 
INFEASIBLE 
SUCCESS 

Constructor & Destructor Documentation

◆ HierarchicalIterativeSolver()

hpp::constraints::HierarchicalIterativeSolver::HierarchicalIterativeSolver ( const std::size_t &  argSize,
const std::size_t  derSize 
)

◆ ~HierarchicalIterativeSolver()

virtual hpp::constraints::HierarchicalIterativeSolver::~HierarchicalIterativeSolver ( )
inlinevirtual

Member Function Documentation

◆ activeDerivativeParameters()

ArrayXb hpp::constraints::HierarchicalIterativeSolver::activeDerivativeParameters ( ) const

Velocity parameters involved in the constraint resolution.

◆ activeParameters()

ArrayXb hpp::constraints::HierarchicalIterativeSolver::activeParameters ( ) const

Configuration parameters involved in the constraint resolution.

◆ add() [1/2]

void hpp::constraints::HierarchicalIterativeSolver::add ( const DifferentiableFunctionPtr_t f,
const std::size_t &  priority 
)
inline

Add an equality constraint to a priority.

References hpp::constraints::EqualToZero.

◆ add() [2/2]

void hpp::constraints::HierarchicalIterativeSolver::add ( const DifferentiableFunctionPtr_t f,
const std::size_t &  priority,
const ComparisonTypes_t comp 
)

Add a constraint.

◆ computeActiveRowsOfJ()

virtual void hpp::constraints::HierarchicalIterativeSolver::computeActiveRowsOfJ ( std::size_t  iStack)
protectedvirtual

Compute which rows of the jacobian of stack_[iStack] are not zero, using the activeDerivativeParameters of the functions.

The result is stored in datas_[i].activeRowsOfJ

Reimplemented in hpp::constraints::HybridSolver.

◆ computeDescentDirection()

void hpp::constraints::HierarchicalIterativeSolver::computeDescentDirection ( ) const
protected

Compute a SVD decomposition of each level and find the best descent direction at the first order.

Linearization of the system of equations rhs - v_{i} = J (q_i) (dq_{i+1} - q_{i}) q_{i+1} - q_{i} = J(q_i)^{+} ( rhs - v_{i} ) dq = J(q_i)^{+} ( rhs - v_{i} )

Warning
computeValue<true> must have been called first.

◆ computeError()

void hpp::constraints::HierarchicalIterativeSolver::computeError ( ) const

If lastIsOptional() is true, then the last level is ignored.

Warning
computeValue must have been called first.

◆ computeSaturation()

void hpp::constraints::HierarchicalIterativeSolver::computeSaturation ( vectorIn_t  arg) const

◆ computeValue()

template<bool ComputeJac>
void hpp::constraints::HierarchicalIterativeSolver::computeValue ( vectorIn_t  arg) const

Compute the value of each level, and the jacobian if ComputeJac is true.

◆ dimension()

const size_type& hpp::constraints::HierarchicalIterativeSolver::dimension ( ) const
inline

◆ errorThreshold() [1/2]

void hpp::constraints::HierarchicalIterativeSolver::errorThreshold ( const value_type threshold)
inline

Set error threshold.

◆ errorThreshold() [2/2]

value_type hpp::constraints::HierarchicalIterativeSolver::errorThreshold ( ) const
inline

Get error threshold.

Referenced by hpp::constraints::HybridSolver::errorThreshold().

◆ expandDqSmall()

void hpp::constraints::HierarchicalIterativeSolver::expandDqSmall ( ) const
protected

◆ getReducedJacobian()

void hpp::constraints::HierarchicalIterativeSolver::getReducedJacobian ( matrixOut_t  J) const

◆ getValue()

void hpp::constraints::HierarchicalIterativeSolver::getValue ( vectorOut_t  v) const

◆ inequalityThreshold() [1/2]

value_type hpp::constraints::HierarchicalIterativeSolver::inequalityThreshold ( ) const
inline

Get the inequality threshold.

◆ inequalityThreshold() [2/2]

void hpp::constraints::HierarchicalIterativeSolver::inequalityThreshold ( const value_type it)
inline

set the inequality threshold

◆ integrate()

virtual void hpp::constraints::HierarchicalIterativeSolver::integrate ( vectorIn_t  from,
vectorIn_t  velocity,
vectorOut_t  result 
) const
inlinevirtual

◆ integration() [1/2]

void hpp::constraints::HierarchicalIterativeSolver::integration ( const Integration_t integrate)
inline

Set the integration function.

◆ integration() [2/2]

const Integration_t& hpp::constraints::HierarchicalIterativeSolver::integration ( ) const
inline

Get the integration function.

◆ isSatisfied()

bool hpp::constraints::HierarchicalIterativeSolver::isSatisfied ( vectorIn_t  arg) const
inline

◆ lastIsOptional() [1/2]

void hpp::constraints::HierarchicalIterativeSolver::lastIsOptional ( bool  optional)
inline

◆ lastIsOptional() [2/2]

bool hpp::constraints::HierarchicalIterativeSolver::lastIsOptional ( ) const
inline

◆ lastStep()

const vector_t& hpp::constraints::HierarchicalIterativeSolver::lastStep ( ) const
inline

Accessor to the last step done.

◆ maxIterations() [1/2]

void hpp::constraints::HierarchicalIterativeSolver::maxIterations ( size_type  iterations)
inline

Set maximal number of iterations.

◆ maxIterations() [2/2]

size_type hpp::constraints::HierarchicalIterativeSolver::maxIterations ( ) const
inline

Get maximal number of iterations in config projector.

◆ numberStacks()

std::size_t hpp::constraints::HierarchicalIterativeSolver::numberStacks ( ) const
inline

◆ print()

virtual std::ostream& hpp::constraints::HierarchicalIterativeSolver::print ( std::ostream &  os) const
virtual

Reimplemented in hpp::constraints::HybridSolver.

◆ reducedDimension()

const size_type& hpp::constraints::HierarchicalIterativeSolver::reducedDimension ( ) const
inline

Dimension of the problem after removing the rows of the jacobian which do not influence the error (only zeros along those lines).

◆ reduction() [1/2]

void hpp::constraints::HierarchicalIterativeSolver::reduction ( const segments_t  intervals)
inline

Set the velocity variable that must be changed.

The other variables will be left unchanged by the iterative algorithm.

◆ reduction() [2/2]

void hpp::constraints::HierarchicalIterativeSolver::reduction ( const Reduction_t reduction)
inline

Set the velocity variable that must be changed.

The other variables will be left unchanged by the iterative algorithm.

◆ residualError() [1/2]

value_type hpp::constraints::HierarchicalIterativeSolver::residualError ( ) const
inline

Returns the squared norm of the error vector.

◆ residualError() [2/2]

void hpp::constraints::HierarchicalIterativeSolver::residualError ( vectorOut_t  error) const

Returns the error vector.

◆ rightHandSide() [1/3]

bool hpp::constraints::HierarchicalIterativeSolver::rightHandSide ( const DifferentiableFunctionPtr_t f,
vectorIn_t  rhs 
)

◆ rightHandSide() [2/3]

void hpp::constraints::HierarchicalIterativeSolver::rightHandSide ( vectorIn_t  rhs)

Set the level set parameter.

Parameters
rhsthe level set parameter.

◆ rightHandSide() [3/3]

vector_t hpp::constraints::HierarchicalIterativeSolver::rightHandSide ( ) const

Get the level set parameter.

Returns
the parameter.

◆ rightHandSideFromInput() [1/2]

vector_t hpp::constraints::HierarchicalIterativeSolver::rightHandSideFromInput ( vectorIn_t  arg)

Compute a right hand side using the input arg.

◆ rightHandSideFromInput() [2/2]

bool hpp::constraints::HierarchicalIterativeSolver::rightHandSideFromInput ( const DifferentiableFunctionPtr_t f,
vectorIn_t  arg 
)

◆ rightHandSideSize()

size_type hpp::constraints::HierarchicalIterativeSolver::rightHandSideSize ( ) const

Get size of the level set parameter.

◆ saturate()

void hpp::constraints::HierarchicalIterativeSolver::saturate ( vectorOut_t  arg) const
protected

◆ saturation() [1/2]

void hpp::constraints::HierarchicalIterativeSolver::saturation ( const Saturation_t saturate)
inline

Set the saturation function.

◆ saturation() [2/2]

const Saturation_t& hpp::constraints::HierarchicalIterativeSolver::saturation ( ) const
inline

Get the saturation function.

◆ sigma()

const value_type& hpp::constraints::HierarchicalIterativeSolver::sigma ( ) const
inline

Returns the lowest singular value.

If the jacobian has maximum rank r, then it corresponds to r-th greatest singular value. This value is zero when the jacobian is singular.

◆ solve() [1/2]

template<typename LineSearchType >
HierarchicalIterativeSolver::Status hpp::constraints::HierarchicalIterativeSolver::solve ( vectorOut_t  arg,
LineSearchType  ls = LineSearchType() 
) const
inline

References Eigen::assert().

◆ solve() [2/2]

Status hpp::constraints::HierarchicalIterativeSolver::solve ( vectorOut_t  arg) const
inline

◆ squaredErrorThreshold()

value_type hpp::constraints::HierarchicalIterativeSolver::squaredErrorThreshold ( ) const
inline

Get error threshold.

◆ stack()

const DifferentiableFunctionStack& hpp::constraints::HierarchicalIterativeSolver::stack ( const std::size_t  priority)
inline

References Eigen::assert().

◆ update()

void hpp::constraints::HierarchicalIterativeSolver::update ( )
protected

Allocate datas and update sizes of the problem Should be called whenever the stack is modified.

Friends And Related Function Documentation

◆ lineSearch::Backtracking

friend struct lineSearch::Backtracking
friend

Member Data Documentation

◆ argSize_

size_type hpp::constraints::HierarchicalIterativeSolver::argSize_
protected

◆ datas_

std::vector<Data> hpp::constraints::HierarchicalIterativeSolver::datas_
mutableprotected

◆ derSize_

size_type hpp::constraints::HierarchicalIterativeSolver::derSize_
protected

◆ dimension_

size_type hpp::constraints::HierarchicalIterativeSolver::dimension_
protected

◆ dq_

vector_t hpp::constraints::HierarchicalIterativeSolver::dq_
mutableprotected

◆ dqSmall_

vector_t hpp::constraints::HierarchicalIterativeSolver::dqSmall_
mutableprotected

◆ inequalityThreshold_

value_type hpp::constraints::HierarchicalIterativeSolver::inequalityThreshold_
protected

◆ integrate_

Integration_t hpp::constraints::HierarchicalIterativeSolver::integrate_
protected

◆ lastIsOptional_

bool hpp::constraints::HierarchicalIterativeSolver::lastIsOptional_
protected

◆ maxIterations_

size_type hpp::constraints::HierarchicalIterativeSolver::maxIterations_
protected

◆ projector_

matrix_t hpp::constraints::HierarchicalIterativeSolver::projector_
mutableprotected

◆ reducedDimension_

size_type hpp::constraints::HierarchicalIterativeSolver::reducedDimension_
protected

◆ reducedJ_

matrix_t hpp::constraints::HierarchicalIterativeSolver::reducedJ_
mutableprotected

◆ reducedSaturation_

Eigen::VectorXi hpp::constraints::HierarchicalIterativeSolver::reducedSaturation_
mutableprotected

◆ reduction_

Reduction_t hpp::constraints::HierarchicalIterativeSolver::reduction_
protected

◆ saturate_

Saturation_t hpp::constraints::HierarchicalIterativeSolver::saturate_
protected

◆ saturation_

Eigen::VectorXi hpp::constraints::HierarchicalIterativeSolver::saturation_
mutableprotected

◆ sigma_

value_type hpp::constraints::HierarchicalIterativeSolver::sigma_
mutableprotected

The smallest non-zero singular value.

◆ squaredErrorThreshold_

value_type hpp::constraints::HierarchicalIterativeSolver::squaredErrorThreshold_
protected

◆ squaredNorm_

value_type hpp::constraints::HierarchicalIterativeSolver::squaredNorm_
mutableprotected

◆ stacks_

std::vector<DifferentiableFunctionStack> hpp::constraints::HierarchicalIterativeSolver::stacks_
protected

◆ statistics_

mutable ::hpp::statistics::SuccessStatistics hpp::constraints::HierarchicalIterativeSolver::statistics_
protected

◆ svd_

SVD_t hpp::constraints::HierarchicalIterativeSolver::svd_
mutableprotected

◆ tmpSat_

ArrayXb hpp::constraints::HierarchicalIterativeSolver::tmpSat_
mutableprotected