#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 boost::function< bool(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi &saturation)> | 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 |
| void | add (const DifferentiableFunctionPtr_t &f, const std::size_t &priority) HPP_CONSTRAINTS_DEPRECATED |
| void | add (const DifferentiableFunctionPtr_t &f, const std::size_t &priority, const ComparisonTypes_t &comp) HPP_CONSTRAINTS_DEPRECATED |
| void | 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 | 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 |
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 |
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: 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 error threshold can be accessed by methods HierarchicalIterative::errorThreshold. The maximal number of iterations can be accessed by methods HierarchicalIterative::maxIterations.
- 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
To prevent configuration variables to get out of joint limits during Newton Raphson iterations, the user may provide a method of type HierarchicalIterative::Saturation_t using setter and getter HierarchicalIterative::saturation.
- Note
- 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 boost::function<bool (vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi& saturation)> hpp::constraints::solver::HierarchicalIterative::Saturation_t |
This function checks which degrees of freedom are saturated.
- Parameters
-
q a configuration,
- Return values
-
qSat configuration after saturing values out of bounds saturation vector: for each degree of freedom, saturation is set to - -1 if the lower bound is reached,
- 1 if the upper bound is reached,
- 0 otherwise.
- Returns
- true if and only if at least one degree of freedom has been saturated
◆ SVD_t
|
protected |
Member Enumeration Documentation
◆ Status
Constructor & Destructor Documentation
◆ HierarchicalIterative() [1/2]
| hpp::constraints::solver::HierarchicalIterative::HierarchicalIterative | ( | const LiegroupSpacePtr_t & | configSpace | ) |
◆ HierarchicalIterative() [2/2]
| hpp::constraints::solver::HierarchicalIterative::HierarchicalIterative | ( | const HierarchicalIterative & | other | ) |
◆ ~HierarchicalIterative()
|
inlinevirtual |
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() [1/3]
| void hpp::constraints::solver::HierarchicalIterative::add | ( | const DifferentiableFunctionPtr_t & | f, |
| const std::size_t & | priority | ||
| ) |
Add an implicit equality constraint
- Parameters
-
f differentiable function from the robot configuration space to a Lie group (See hpp::pinocchio::LiegroupSpace), priority level of priority of the constraint: priority are in decreasing order: 0 is the highest priority level.
Constraint is defined by \(f (\mathbf{q}) = 0\).
◆ add() [2/3]
| void hpp::constraints::solver::HierarchicalIterative::add | ( | const DifferentiableFunctionPtr_t & | f, |
| const std::size_t & | priority, | ||
| const ComparisonTypes_t & | comp | ||
| ) |
Add an implicit constraint
- Parameters
-
f differentiable function from the robot configuration space to a Lie group (See hpp::pinocchio::LiegroupSpace), priority level of priority of the constraint: priority are in decreasing order: 0 is the highest priority level, comp comparison type. See class documentation for details.
◆ add() [3/3]
| void hpp::constraints::solver::HierarchicalIterative::add | ( | const ImplicitPtr_t & | constraint, |
| const std::size_t & | priority | ||
| ) |
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,
◆ 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 set of constraints for a give priority level.
◆ constraints() [2/2]
|
inline |
Get constraints (implicit and explicit)
◆ contains()
|
virtual |
Check whether a numerical constraint has been added
- Parameters
-
numericalConstraint numerical constraint
- Returns
- true if numerical constraint is already in the solver whatever the passive dofs are.
- Note
- Comparison between constraints is performed by function names. This means that two constraints with the same function names are considered as equal.
Reimplemented in hpp::constraints::solver::BySubstitution.
◆ 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 |
Set error threshold.
◆ errorThreshold() [2/2]
|
inline |
Get error threshold.
◆ expandDqSmall()
|
protected |
◆ freeVariables() [1/3]
|
inline |
Set free velocity variables
The other variables will be left unchanged by the iterative resolution.
- Parameters
-
intervals set of index intervals
◆ freeVariables() [2/3]
|
inline |
Set free velocity variables
The other variables will be left unchanged by the iterative resolution.
◆ freeVariables() [3/3]
|
inline |
Get free velocity variables.
◆ 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()
|
inline |
◆ lastIsOptional() [1/2]
|
inline |
◆ lastIsOptional() [2/2]
|
inline |
◆ lastStep()
|
inline |
Accessor to the last step done.
◆ maxIterations() [1/2]
|
inline |
Set maximal number of iterations.
◆ maxIterations() [2/2]
|
inline |
Get maximal number of iterations in config projector.
◆ 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]
|
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() [2/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.
◆ rightHandSide() [3/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).
◆ 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 |
Set the saturation function.
◆ saturation() [2/2]
|
inline |
Get 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() [2/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}\).
◆ solve() [3/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}\).
◆ 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_
|
mutableprotected |
◆ 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_
|
mutableprotected |
◆ saturate_
|
protected |
◆ saturation_
|
mutableprotected |
◆ sigma_
|
mutableprotected |
The smallest non-zero singular value.
◆ squaredErrorThreshold_
|
protected |
◆ squaredNorm_
|
mutableprotected |
◆ stacks_
|
protected |
◆ statistics_
|
protected |
◆ svd_
|
mutableprotected |
◆ tmpSat_
|
mutableprotected |
The documentation for this class was generated from the following file:
- include/hpp/constraints/solver/hierarchical-iterative.hh
