#include <hpp/constraints/iterative-solver.hh>
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_t & | integration () const |
Get the integration function. More... | |
void | saturation (const Saturation_t &saturate) |
Set the saturation function. More... | |
const Saturation_t & | saturation () 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_type & | sigma () 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 DifferentiableFunctionStack & | stack (const std::size_t priority) |
std::size_t | numberStacks () const |
const size_type & | dimension () const |
const size_type & | reducedDimension () 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_t & | lastStep () 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_t > | SVD_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< DifferentiableFunctionStack > | stacks_ |
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< Data > | datas_ |
SVD_t | svd_ |
mutable ::hpp::statistics::SuccessStatistics | statistics_ |
Friends | |
struct | lineSearch::Backtracking |
typedef boost::function<void (vectorIn_t from, vectorIn_t velocity, vectorOut_t result)> hpp::constraints::HierarchicalIterativeSolver::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)
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
|
protected |
hpp::constraints::HierarchicalIterativeSolver::HierarchicalIterativeSolver | ( | const std::size_t & | argSize, |
const std::size_t | derSize | ||
) |
|
inlinevirtual |
ArrayXb hpp::constraints::HierarchicalIterativeSolver::activeDerivativeParameters | ( | ) | const |
Velocity parameters involved in the constraint resolution.
ArrayXb hpp::constraints::HierarchicalIterativeSolver::activeParameters | ( | ) | const |
Configuration parameters involved in the constraint resolution.
|
inline |
Add an equality constraint to a priority.
References hpp::constraints::EqualToZero.
void hpp::constraints::HierarchicalIterativeSolver::add | ( | const DifferentiableFunctionPtr_t & | f, |
const std::size_t & | priority, | ||
const ComparisonTypes_t & | comp | ||
) |
Add a constraint.
|
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.
|
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} )
void hpp::constraints::HierarchicalIterativeSolver::computeError | ( | ) | const |
If lastIsOptional() is true, then the last level is ignored.
void hpp::constraints::HierarchicalIterativeSolver::computeSaturation | ( | vectorIn_t | arg | ) | const |
void hpp::constraints::HierarchicalIterativeSolver::computeValue | ( | vectorIn_t | arg | ) | const |
Compute the value of each level, and the jacobian if ComputeJac is true.
|
inline |
|
inline |
Set error threshold.
|
inline |
Get error threshold.
Referenced by hpp::constraints::HybridSolver::errorThreshold().
|
protected |
void hpp::constraints::HierarchicalIterativeSolver::getReducedJacobian | ( | matrixOut_t | J | ) | const |
void hpp::constraints::HierarchicalIterativeSolver::getValue | ( | vectorOut_t | v | ) | const |
|
inline |
Get the inequality threshold.
|
inline |
set the inequality threshold
|
inlinevirtual |
Reimplemented in hpp::constraints::HybridSolver.
Referenced by hpp::constraints::HybridSolver::integrate().
|
inline |
Set the integration function.
|
inline |
Get the integration function.
|
inline |
|
inline |
|
inline |
|
inline |
Accessor to the last step done.
|
inline |
Set maximal number of iterations.
|
inline |
Get maximal number of iterations in config projector.
|
inline |
|
virtual |
Reimplemented in hpp::constraints::HybridSolver.
|
inline |
Dimension of the problem after removing the rows of the jacobian which do not influence the error (only zeros along those lines).
|
inline |
Set the velocity variable that must be changed.
The other variables will be left unchanged by the iterative algorithm.
|
inline |
Set the velocity variable that must be changed.
The other variables will be left unchanged by the iterative algorithm.
|
inline |
Returns the squared norm of the error vector.
void hpp::constraints::HierarchicalIterativeSolver::residualError | ( | vectorOut_t | error | ) | const |
Returns the error vector.
bool hpp::constraints::HierarchicalIterativeSolver::rightHandSide | ( | const DifferentiableFunctionPtr_t & | f, |
vectorIn_t | rhs | ||
) |
void hpp::constraints::HierarchicalIterativeSolver::rightHandSide | ( | vectorIn_t | rhs | ) |
Set the level set parameter.
rhs | the level set parameter. |
vector_t hpp::constraints::HierarchicalIterativeSolver::rightHandSide | ( | ) | const |
Get the level set parameter.
vector_t hpp::constraints::HierarchicalIterativeSolver::rightHandSideFromInput | ( | vectorIn_t | arg | ) |
Compute a right hand side using the input arg.
bool hpp::constraints::HierarchicalIterativeSolver::rightHandSideFromInput | ( | const DifferentiableFunctionPtr_t & | f, |
vectorIn_t | arg | ||
) |
size_type hpp::constraints::HierarchicalIterativeSolver::rightHandSideSize | ( | ) | const |
Get size of the level set parameter.
|
protected |
|
inline |
Set the saturation function.
|
inline |
Get the saturation function.
|
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.
|
inline |
References Eigen::assert().
|
inline |
|
inline |
Get error threshold.
|
inline |
References Eigen::assert().
|
protected |
Allocate datas and update sizes of the problem Should be called whenever the stack is modified.
|
friend |
|
protected |
|
mutableprotected |
|
protected |
|
protected |
|
mutableprotected |
|
mutableprotected |
|
protected |
|
protected |
|
protected |
|
protected |
|
mutableprotected |
|
protected |
|
mutableprotected |
|
mutableprotected |
|
protected |
|
protected |
|
mutableprotected |
|
mutableprotected |
The smallest non-zero singular value.
|
protected |
|
mutableprotected |
|
protected |
|
protected |
|
mutableprotected |
|
mutableprotected |