#include <hpp/constraints/solver/hierarchical-iterative.hh>
Classes | |
| struct | Data |
Public Types | |
| enum | Status { ERROR_INCREASED , MAX_ITERATION_REACHED , INFEASIBLE , SUCCESS } |
| typedef Eigen::RowBlockIndices | Indices_t |
| typedef lineSearch::FixedSequence | DefaultLineSearch |
| typedef shared_ptr< saturation::Base > | Saturation_t |
Public Member Functions | |
| HierarchicalIterative (const LiegroupSpacePtr_t &configSpace) | |
| HierarchicalIterative (const HierarchicalIterative &other) | |
| virtual | ~HierarchicalIterative () |
| value_type | residualError () const |
| Returns the squared norm of the error vector. More... | |
| void | residualError (vectorOut_t error) const |
| Returns the error vector. More... | |
| bool | definesSubmanifoldOf (const HierarchicalIterative &solver) const |
| virtual std::ostream & | print (std::ostream &os) const |
| template<typename LineSearchType > | |
| solver::HierarchicalIterative::Status | solve (vectorOut_t arg, LineSearchType lineSearch) const |
Problem definition | |
| const LiegroupSpacePtr_t & | configSpace () const |
| Get configuration space on which constraints are defined. More... | |
| virtual bool | contains (const ImplicitPtr_t &numericalConstraint) const |
| virtual bool | add (const ImplicitPtr_t &constraint, const std::size_t &priority) |
| virtual void | merge (const HierarchicalIterative &other) |
| 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 |
| bool | isSatisfied (vectorIn_t arg, value_type errorThreshold) const |
| bool | isConstraintSatisfied (const ImplicitPtr_t &constraint, vectorIn_t arg, vectorOut_t error, bool &constraintFound) const |
| const value_type & | sigma () const |
Parameters | |
| void | freeVariables (const segments_t intervals) |
| void | freeVariables (const Indices_t &indices) |
| const Indices_t & | freeVariables () const |
| Get free velocity variables. 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 |
| void | solveLevelByLevel (bool solveLevelByLevel) |
| bool | solveLevelByLevel () const |
Stack | |
| const ImplicitConstraintSet & | constraints (const std::size_t priority) |
| Get set of constraints for a give priority level. More... | |
| const NumericalConstraints_t & | constraints () const |
| Get constraints (implicit and explicit) More... | |
| std::size_t | numberStacks () const |
| const size_type & | dimension () const |
| const size_type & | reducedDimension () const |
| 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 | rightHandSideFromConfig (ConfigurationIn_t config) |
| virtual bool | rightHandSideFromConfig (const ImplicitPtr_t &constraint, ConfigurationIn_t config) |
| virtual bool | rightHandSide (const ImplicitPtr_t &constraint, vectorIn_t rhs) |
| virtual bool | getRightHandSide (const ImplicitPtr_t &constraint, vectorOut_t rhs) const |
| Get right hand side of a constraints. More... | |
| virtual void | rightHandSide (vectorIn_t rhs) |
| void | rightHandSideAt (const value_type &s) |
| vector_t | rightHandSide () const |
| size_type | rightHandSideSize () const |
Access to internal data | |
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 |
| const vector_t & | lastStep () const |
| Accessor to the last step done. More... | |
| virtual bool | 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 () |
| virtual void | computeActiveRowsOfJ (std::size_t iStack) |
| void | computeDescentDirection () const |
| void | expandDqSmall () const |
| void | saturate (vectorOut_t arg) const |
| HierarchicalIterative () | |
Friends | |
| struct | lineSearch::Backtracking |
Detailed Description
Solve a system of non-linear equations on a robot configuration
The non-linear system of equations is built by adding equations with method HierarchicalIterative::add.
Note that a hierarchy between the equations can be provided. In this case, the solver will try to solve the highest priority equations first, and then to solve the lower priority equations. Note that priorities are in decreasing order: 0 has higher priority than 1.
The algorithm used is a Newton-Raphson like algorithm that works as follows: for a single level of priority, let \(f (\mathbf{q}) = 0\) be the system of equations where \(f\) is a \(C^1\) mapping from the robot configuration space to a Lie group space \(\mathcal{L}\).
Starting from initial guess \(\mathbf{q}_0\), the method HierarchicalIterative::solve builds a sequence of configurations \(\mathbf{q}_i\) as follows:
\begin{eqnarray*} \mathbf{q}_{i+1} = \mathbf{q}_i - \alpha_i \frac{\partial f}{\partial \mathbf{q}}(\mathbf{q}_i)^{+} f (\mathbf{q}_i) \end{eqnarray*}
where
- \(\frac{\partial f}{\partial \mathbf{q}}(\mathbf{q}_i)^{+}\) is the Moore-Penrose pseudo-inverse of the system Jacobian,
- \(\alpha_i\) is a sequence of real numbers depending on the line search strategy. Possible line-search strategies are lineSearch::Constant, lineSearch::Backtracking, lineSearch::FixedSequence, lineSearch::ErrorNormBased. until
- the residual \(\|f(\mathbf{q})\|\) is below an error threshold, or
- the maximal number of iterations has been reached.
The computation of the direction of descent in the case of multiple level of hierarchy is described in this paper.
The error threshold can be accessed by methods HierarchicalIterative::errorThreshold. The maximal number of iterations can be accessed by methods HierarchicalIterative::maxIterations.
- Note
- Solving equations one after the other
For some applications, it can be more efficient to solve a set of equations one after the other. In other words, an equation is ignored until the previous one is solved (norm below the threshold). To do so, introduce the equations using method HierarchicalIterative::add with increasing value of priority, and call method HierarchicalIterative::solveLevelByLevel(true).
- Note
- Lie group
The unknowns \(\mathbf{q}\) may take values in a more general set than the configuration space of a robot. This set should be a Cartesian product of Lie groups: hpp::pinocchio::LiegroupSpace.
- Note
- Saturation
- Right hand side and comparison types
Instead of \(f(\mathbf{q}) = 0\), other constraints can be defined. Several comparison types are available:
- Equality: \(f(\mathbf{q}) = rhs\), where \(rhs\) is a parameterizable right hand side,
- EqualToZero: \(f(\mathbf{q}) = 0\),
- Superior: \(f(\mathbf{q}) > 0\)
- Inferior: \(f(\mathbf{q}) < 0\) If several constraint are of type equality, the right hand side of the system of equations can be modified by methods HierarchicalIterative::rightHandSideFromInput, HierarchicalIterative::rightHandSide.
- Note
- Free variables
Some variables can be locked, or computed explicitely. In this case, the iterative resolution will only change the other variables called free variables.
- See also
- methods
- freeVariables (const Indices_t& indices) and
- freeVariables (const Indices_t& indices).
Member Typedef Documentation
◆ DefaultLineSearch
| typedef lineSearch::FixedSequence hpp::constraints::solver::HierarchicalIterative::DefaultLineSearch |
◆ Indices_t
◆ Saturation_t
| typedef shared_ptr<saturation::Base> hpp::constraints::solver::HierarchicalIterative::Saturation_t |
◆ SVD_t
|
protected |
Member Enumeration Documentation
◆ Status
Constructor & Destructor Documentation
◆ HierarchicalIterative() [1/3]
| hpp::constraints::solver::HierarchicalIterative::HierarchicalIterative | ( | const LiegroupSpacePtr_t & | configSpace | ) |
◆ HierarchicalIterative() [2/3]
| hpp::constraints::solver::HierarchicalIterative::HierarchicalIterative | ( | const HierarchicalIterative & | other | ) |
◆ ~HierarchicalIterative()
|
inlinevirtual |
◆ HierarchicalIterative() [3/3]
|
inlineprotected |
Member Function Documentation
◆ activeDerivativeParameters()
| ArrayXb hpp::constraints::solver::HierarchicalIterative::activeDerivativeParameters | ( | ) | const |
Velocity parameters involved in the constraint resolution.
◆ activeParameters()
| ArrayXb hpp::constraints::solver::HierarchicalIterative::activeParameters | ( | ) | const |
Configuration parameters involved in the constraint resolution.
◆ add()
|
virtual |
Add an implicit constraint
- Parameters
-
constraint implicit constraint priority level of priority of the constraint: priority are in decreasing order: 0 is the highest priority level,
Reimplemented in hpp::constraints::solver::BySubstitution.
◆ computeActiveRowsOfJ()
|
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::solver::BySubstitution.
◆ computeDescentDirection()
|
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::solver::HierarchicalIterative::computeError | ( | ) | const |
If lastIsOptional() is true, then the last level is ignored.
- Warning
- computeValue must have been called first.
◆ computeSaturation()
| void hpp::constraints::solver::HierarchicalIterative::computeSaturation | ( | vectorIn_t | arg | ) | const |
◆ computeValue()
| void hpp::constraints::solver::HierarchicalIterative::computeValue | ( | vectorIn_t | arg | ) | const |
Compute the value of each level, and the jacobian if ComputeJac is true.
◆ configSpace()
|
inline |
Get configuration space on which constraints are defined.
◆ constraints() [1/2]
|
inline |
Get constraints (implicit and explicit)
◆ constraints() [2/2]
|
inline |
Get set of constraints for a give priority level.
◆ contains()
|
virtual |
Check whether a numerical constraint has been added
- Parameters
-
numericalConstraint numerical constraint
- Returns
- true if numerical constraint is already in the solver
◆ definesSubmanifoldOf()
| bool hpp::constraints::solver::HierarchicalIterative::definesSubmanifoldOf | ( | const HierarchicalIterative & | solver | ) | const |
Inclusion of manifolds
- Parameters
-
solver another solver
This function returns true if the solution manifold of the solver given as input is a submanifold of the solution manifold of this solver. The function tests whether constraints of the input solver are also in this solver.
◆ dimension()
|
inline |
◆ errorThreshold() [1/2]
|
inline |
Get error threshold.
◆ errorThreshold() [2/2]
|
inline |
Set error threshold.
◆ expandDqSmall()
|
protected |
◆ freeVariables() [1/3]
|
inline |
Get free velocity variables.
◆ freeVariables() [2/3]
|
inline |
Set free velocity variables
The other variables will be left unchanged by the iterative resolution.
◆ freeVariables() [3/3]
|
inline |
Set free velocity variables
The other variables will be left unchanged by the iterative resolution.
- Parameters
-
intervals set of index intervals
◆ getReducedJacobian()
| void hpp::constraints::solver::HierarchicalIterative::getReducedJacobian | ( | matrixOut_t | J | ) | const |
◆ getRightHandSide()
|
virtual |
Get right hand side of a constraints.
Reimplemented in hpp::constraints::solver::BySubstitution.
◆ getValue()
| void hpp::constraints::solver::HierarchicalIterative::getValue | ( | vectorOut_t | v | ) | const |
◆ inequalityThreshold() [1/2]
|
inline |
Get the inequality threshold.
◆ inequalityThreshold() [2/2]
|
inline |
set the inequality threshold
◆ integrate()
|
virtual |
Reimplemented in hpp::constraints::solver::BySubstitution.
◆ isConstraintSatisfied()
| bool hpp::constraints::solver::HierarchicalIterative::isConstraintSatisfied | ( | const ImplicitPtr_t & | constraint, |
| vectorIn_t | arg, | ||
| vectorOut_t | error, | ||
| bool & | constraintFound | ||
| ) | const |
Whether a constraint is satisfied for an input vector
- Parameters
-
constraint,the constraint in the solver, arg the input vector
- Return values
-
error the error of the constraint. constraintFound whether the constraint belongs to the solver,
- Returns
- true if constraint belongs to solver and error is below the threshold, false otherwise.
◆ isSatisfied() [1/2]
|
inline |
Whether input vector satisfies the constraints of the solver
- Parameters
-
arg input vector Compares to internal error threshold.
◆ isSatisfied() [2/2]
|
inline |
Whether input vector satisfies the constraints of the solver
- Parameters
-
arg input vector errorThreshold threshold to use instead of the value stored in the solver.
◆ lastIsOptional() [1/2]
|
inline |
◆ lastIsOptional() [2/2]
|
inline |
◆ lastStep()
|
inline |
Accessor to the last step done.
◆ maxIterations() [1/2]
|
inline |
Get maximal number of iterations in config projector.
◆ maxIterations() [2/2]
|
inline |
Set maximal number of iterations.
◆ merge()
|
virtual |
add constraints of another solver
- Parameters
-
other other solver
Add constraints of other to this solver.
- Note
- right hand side of other is not copied.
◆ numberStacks()
|
inline |
◆ print()
|
virtual |
Reimplemented in hpp::constraints::solver::BySubstitution.
◆ reducedDimension()
|
inline |
Dimension of the problem after removing the rows of the jacobian which do not influence the error (only zeros along those lines).
◆ residualError() [1/2]
|
inline |
Returns the squared norm of the error vector.
◆ residualError() [2/2]
| void hpp::constraints::solver::HierarchicalIterative::residualError | ( | vectorOut_t | error | ) | const |
Returns the error vector.
◆ rightHandSide() [1/3]
| vector_t hpp::constraints::solver::HierarchicalIterative::rightHandSide | ( | ) | const |
Get the right hand side
- Returns
- the right hand side
- Note
- size of result is equal to total dimension of parameterizable constraints (type Equality).
◆ rightHandSide() [2/3]
|
virtual |
Set right hand side of a constraints
- Parameters
-
constraint the constraint, rhs right hand side.
- Note
- Size of rhs should be equal to the total dimension of parameterizable constraints (type Equality) .
Reimplemented in hpp::constraints::solver::BySubstitution.
◆ rightHandSide() [3/3]
|
virtual |
Set the right hand side
- Parameters
-
rhs the right hand side
- Note
- Size of rhs should be equal to the total dimension of parameterizable constraints (type Equality).
Reimplemented in hpp::constraints::solver::BySubstitution.
◆ rightHandSideAt()
| void hpp::constraints::solver::HierarchicalIterative::rightHandSideAt | ( | const value_type & | s | ) |
Set the right hand side at a given parameter.
- Parameters
-
s parameter passed to Implicit::rightHandSideAt
◆ rightHandSideFromConfig() [1/2]
| vector_t hpp::constraints::solver::HierarchicalIterative::rightHandSideFromConfig | ( | ConfigurationIn_t | config | ) |
Compute right hand side of equality constraints from a configuration
- Parameters
-
config a configuration.
for each constraint of type Equality, set right hand side as \(rhs = f(\mathbf{q})\).
- Note
- Only parameterizable constraints (type Equality) are set
◆ rightHandSideFromConfig() [2/2]
|
virtual |
Compute right hand side of a constraint from a configuration
- Parameters
-
constraint the constraint, config a configuration.
Set right hand side as \(rhs = f(\mathbf{q})\).
- Note
- Only parameterizable constraints (type Equality) are set
Reimplemented in hpp::constraints::solver::BySubstitution.
◆ rightHandSideSize()
| size_type hpp::constraints::solver::HierarchicalIterative::rightHandSideSize | ( | ) | const |
Get size of the right hand side
- Returns
- sum of dimensions of parameterizable constraints (type Equality)
◆ saturate()
|
protected |
◆ saturation() [1/2]
|
inline |
Get the saturation function.
◆ saturation() [2/2]
|
inline |
Set the saturation function.
◆ sigma()
|
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/3]
|
inline |
Solve the system of non linear equations
- Parameters
-
arg initial guess,
Use Newton Rhapson like iterative method until the error is below the threshold, or until the maximal number of iterations has been reached. Use the default line search method (fixed sequence of \(\alpha_i\)).
- Note
- Explicit constraints are expressed in their implicit form: \(\mathbf{q}_2 = f (\mathbf{q}_1)\) is replaced by \(\mathbf{q}_2 - f (\mathbf{q}_1) = 0\), where \(\mathbf{q}_1\) and \(\mathbf{q}_2\) are vectors composed of the components of \(\mathbf{q}\).
◆ solve() [2/3]
|
inline |
◆ solve() [3/3]
| Status hpp::constraints::solver::HierarchicalIterative::solve | ( | vectorOut_t | arg, |
| LineSearchType | ls = LineSearchType() |
||
| ) | const |
Solve the system of non linear equations
- Parameters
-
arg initial guess, ls line search method used.
Use Newton Rhapson like iterative method until the error is below the threshold, or until the maximal number of iterations has been reached.
- Note
- Explicit constraints are expressed in their implicit form: \(\mathbf{q}_2 = f (\mathbf{q}_1)\) is replaced by \(\mathbf{q}_2 - f (\mathbf{q}_1) = 0\), where \(\mathbf{q}_1\) and \(\mathbf{q}_2\) are vectors composed of the components of \(\mathbf{q}\).
◆ solveLevelByLevel() [1/2]
|
inline |
◆ solveLevelByLevel() [2/2]
|
inline |
◆ squaredErrorThreshold()
|
inline |
Get error threshold.
◆ 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 |
Member Data Documentation
◆ configSpace_
|
protected |
◆ constraints_
|
protected |
Members moved from core::ConfigProjector.
◆ datas_
|
mutableprotected |
◆ dimension_
|
protected |
◆ dq_
|
mutableprotected |
◆ dqSmall_
|
protected |
◆ freeVariables_
|
protected |
Unknown of the set of implicit constraints.
◆ inequalityThreshold_
|
protected |
◆ iq_
|
protected |
Value rank of constraint in its priority level.
◆ iv_
|
protected |
Derivative rank of constraint in its priority level.
◆ lastIsOptional_
|
protected |
◆ maxIterations_
|
protected |
◆ OM_
|
mutableprotected |
◆ OP_
|
mutableprotected |
◆ priority_
|
protected |
Priority level of constraint.
◆ qSat_
|
mutableprotected |
◆ reducedDimension_
|
protected |
◆ reducedJ_
|
mutableprotected |
◆ reducedSaturation_
|
protected |
◆ saturate_
|
protected |
◆ saturation_
|
mutableprotected |
◆ sigma_
|
mutableprotected |
The smallest non-zero singular value.
◆ solveLevelByLevel_
|
protected |
◆ squaredErrorThreshold_
|
protected |
◆ squaredNorm_
|
mutableprotected |
◆ stacks_
|
protected |
◆ svd_
|
mutableprotected |
◆ tmpSat_
|
mutableprotected |
The documentation for this class was generated from the following file:
- include/hpp/constraints/solver/hierarchical-iterative.hh
