hpp-constraints: hpp::constraints::solver::BySubstitution Class Reference

hpp-constraints 9.0.2 Definition of basic geometric constraints for motion planning

Loading...
Searching...
No Matches
Public Member Functions | Protected Member Functions | List of all members
hpp::constraints::solver::BySubstitution Class Reference
Constraint solvers

`#include <hpp/constraints/solver/by-substitution.hh>`

Inheritance diagram for hpp::constraints::solver::BySubstitution:
[legend]
Collaboration diagram for hpp::constraints::solver::BySubstitution:
[legend]

## Public Member Functions
| BySubstitution (const LiegroupSpacePtr_t &configSpace)
| BySubstitution (const BySubstitution &other)
virtual | ~BySubstitution ()
bool | add (const ImplicitPtr_t &numericalConstraint, const std::size_t &priority=0)
BySubstitution | extract (interval_t interval)
const NumericalConstraints_t & | numericalConstraints () const
| Get the numerical constraints implicit and explicit.
ExplicitConstraintSet & | explicitConstraintSet ()
| Get explicit constraint set.
const ExplicitConstraintSet & | explicitConstraintSet () const
| Set explicit constraint set.
size_type | numberFreeVariables () const
| Return the number of free variables.
void | explicitConstraintSetHasChanged ()
| Should be called whenever explicit solver is modified.
template<typename LineSearchType >
Status | solve (vectorOut_t arg, LineSearchType ls=LineSearchType()) const
template<typename LineSearchType >
Status | solve (vectorOut_t arg, bool optimize, LineSearchType ls=LineSearchType()) const
void | projectVectorOnKernel (ConfigurationIn_t from, vectorIn_t velocity, vectorOut_t result) const
virtual void | projectOnKernel (ConfigurationIn_t from, ConfigurationIn_t to, ConfigurationOut_t result)
Status | solve (vectorOut_t arg) const
size_type | errorSize () const
bool | isSatisfied (vectorIn_t arg) const
bool | isSatisfied (vectorIn_t arg, value_type errorThreshold) const
bool | isSatisfied (vectorIn_t arg, vectorOut_t error) const
bool | isConstraintSatisfied (const ImplicitPtr_t &constraint, vectorIn_t arg, vectorOut_t error, bool &constraintFound) const
template<typename LineSearchType >
bool | oneStep (vectorOut_t arg, LineSearchType &lineSearch) const
void | updateJacobian (vectorIn_t arg) const
void | errorThreshold (const value_type &threshold)
| Set error threshold.
value_type | errorThreshold () const
| Get error threshold.
segments_t | implicitDof () const
virtual std::ostream & | print (std::ostream &os) const
bool | integrate (vectorIn_t from, vectorIn_t velocity, vectorOut_t result) const
template<typename LineSearchType >
HierarchicalIterative::Status | impl_solve (vectorOut_t arg, bool _optimize, LineSearchType lineSearch) const
Right hand side accessors
vector_t | rightHandSideFromConfig (ConfigurationIn_t config)
bool | rightHandSideFromConfig (const ImplicitPtr_t &constraint, ConfigurationIn_t config)
bool | rightHandSide (const ImplicitPtr_t &constraint, vectorIn_t rhs)
bool | getRightHandSide (const ImplicitPtr_t &constraint, vectorOut_t rhs) const
void | rightHandSide (vectorIn_t rhs)
vector_t | rightHandSide () const
size_type | rightHandSideSize () const
Public Member Functions inherited from hpp::constraints::solver::HierarchicalIterative
| HierarchicalIterative (const LiegroupSpacePtr_t &configSpace)
| HierarchicalIterative (const HierarchicalIterative &other)
virtual | ~HierarchicalIterative ()
value_type | residualError () const
| Returns the squared norm of the error vector.
void | residualError (vectorOut_t error) const
| Returns the error vector.
bool | definesSubmanifoldOf (const HierarchicalIterative &solver) const
template<typename LineSearchType >
solver::HierarchicalIterative::Status | solve (vectorOut_t arg, LineSearchType lineSearch) const
const LiegroupSpacePtr_t & | configSpace () const
| Get configuration space on which constraints are defined.
virtual bool | contains (const ImplicitPtr_t &numericalConstraint) const
virtual void | merge (const HierarchicalIterative &other)
void | saturation (const Saturation_t &saturate)
| Set the saturation function.
const Saturation_t & | saturation () const
| Get the saturation function.
HierarchicalIterative | extract (interval_t interval) const
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
void | freeVariables (const segments_t intervals)
void | freeVariables (const Indices_t &indices)
const Indices_t & | freeVariables () const
| Get free velocity variables.
void | maxIterations (size_type iterations)
| Set maximal number of iterations.
size_type | maxIterations () const
| Get maximal number of iterations in config projector.
void | errorThreshold (const value_type &threshold)
| Set error threshold.
value_type | errorThreshold () const
| Get error threshold.
value_type | squaredErrorThreshold () const
| Get error threshold.
value_type | inequalityThreshold () const
| Get the inequality threshold.
void | inequalityThreshold (const value_type &it)
| set the inequality threshold
void | lastIsOptional (bool optional)
bool | lastIsOptional () const
void | solveLevelByLevel (bool solveLevelByLevel)
bool | solveLevelByLevel () const
const ImplicitConstraintSet & | constraints (const std::size_t priority)
| Get set of constraints for a give priority level.
const NumericalConstraints_t & | constraints () const
| Get constraints (implicit and explicit)
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.
ArrayXb | activeDerivativeParameters () const
| Velocity parameters involved in the constraint resolution.
vector_t | rightHandSideFromConfig (ConfigurationIn_t config)
void | rightHandSideAt (const value_type &s)
vector_t | rightHandSide () const
size_type | rightHandSideSize () const
template<bool ComputeJac>
void | computeValue (vectorIn_t arg) const
| Compute the value of each level, and the jacobian if ComputeJac is true.
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.

## Protected Member Functions
void | computeActiveRowsOfJ (std::size_t iStack)
Protected Member Functions inherited from hpp::constraints::solver::HierarchicalIterative
void | update ()
void | computeDescentDirection () const
void | expandDqSmall () const
void | saturate (vectorOut_t arg) const
| HierarchicalIterative ()

## Additional Inherited Members
Public Types inherited from hpp::constraints::solver::HierarchicalIterative
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
Protected Types inherited from hpp::constraints::solver::HierarchicalIterative
typedef Eigen::JacobiSVD< matrix_t > | SVD_t
Protected Attributes inherited from hpp::constraints::solver::HierarchicalIterative
value_type | squaredErrorThreshold_
value_type | inequalityThreshold_
size_type | maxIterations_
std::vector< ImplicitConstraintSet > | stacks_
LiegroupSpacePtr_t | configSpace_
size_type | dimension_
size_type | reducedDimension_
bool | lastIsOptional_
bool | solveLevelByLevel_
Indices_t | freeVariables_
| Unknown of the set of implicit constraints.
Saturation_t | saturate_
NumericalConstraints_t | constraints_
| Members moved from core::ConfigProjector.
std::map< DifferentiableFunctionPtr_t, size_type > | iq_
| Value rank of constraint in its priority level.
std::map< DifferentiableFunctionPtr_t, size_type > | iv_
| Derivative rank of constraint in its priority level.
std::map< DifferentiableFunctionPtr_t, std::size_t > | priority_
| Priority level of constraint.
value_type | sigma_
| The smallest non-zero singular value.
vector_t | dq_
vector_t | dqSmall_
matrix_t | reducedJ_
Eigen::VectorXi | saturation_
Eigen::VectorXi | reducedSaturation_
Configuration_t | qSat_
ArrayXb | tmpSat_
value_type | squaredNorm_
std::vector< Data > | datas_
SVD_t | svd_
vector_t | OM_
vector_t | OP_

## Detailed Description

Solve a non-linear system equations with explicit and implicit constraints

This solver is defined in paper https://hal.archives-ouvertes.fr/hal-01804774/file/paper.pdf. We give here only a brief description

The unknows (denoted by ) of the system of equations is a Lie group. It is usually a robot configuration space or the Cartesian product of robot configuration spaces.

The solver stores a set of implicit numerical constraints: . These implicit constraints are added using method HierarchicalIterative::add.

The solver also stores explicit numerical constraints (constraints where some configuration variables depend on others) in an instance of class ExplicitConstraintSet. This instance is accessible via method BySubstitution::explicitConstraintSet.

When an explicit constraint is added using method ExplicitConstraintSet::add, this method checks that the explicit constraint is compatible with the previously added ones. If so, the constraint is stored in the explicit constraint set. Otherwise, it has to be added as an implicit constraint.

See Section III of the above mentioned paper for the description of the constraint resolution.

## Constructor & Destructor Documentation

## ◆ BySubstitution() [1/2]

hpp::constraints::solver::BySubstitution::BySubstitution | ( | const LiegroupSpacePtr_t & | configSpace | ) |

## ◆ BySubstitution() [2/2]

hpp::constraints::solver::BySubstitution::BySubstitution | ( | const BySubstitution & | other | ) |

## ◆ ~BySubstitution()

virtual hpp::constraints::solver::BySubstitution::~BySubstitution | ( |  | ) |
| inline virtual

## Member Function Documentation

## ◆ add()

bool hpp::constraints::solver::BySubstitution::add | ( | const ImplicitPtr_t & | numericalConstraint,
|  | const std::size_t & | priority = 0 )
| 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,

If the constraint is explicit and compatible with previously inserted constraints, it is added as explicit. Otherwise, it is added as implicit.

Reimplemented from hpp::constraints::solver::HierarchicalIterative.

## ◆ computeActiveRowsOfJ()

void hpp::constraints::solver::BySubstitution::computeActiveRowsOfJ | ( | std::size_t | iStack | ) |
| protected virtual

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 from hpp::constraints::solver::HierarchicalIterative.

## ◆ errorSize()

size_type hpp::constraints::solver::BySubstitution::errorSize | ( |  | ) | const
| inline

Return the size of the error as computed by isSatisfied

Size of error is different from size of right hand side (rightHandSideSize) if the solver contains constraints with values in a Lie group.

## ◆ errorThreshold() [1/2]

value_type hpp::constraints::solver::BySubstitution::errorThreshold | ( |  | ) | const
| inline

Get error threshold.

## ◆ errorThreshold() [2/2]

void hpp::constraints::solver::BySubstitution::errorThreshold | ( | const value_type & | threshold | ) |
| inline

Set error threshold.

## ◆ explicitConstraintSet() [1/2]

ExplicitConstraintSet & hpp::constraints::solver::BySubstitution::explicitConstraintSet | ( |  | ) |
| inline

Get explicit constraint set.

## ◆ explicitConstraintSet() [2/2]

const ExplicitConstraintSet & hpp::constraints::solver::BySubstitution::explicitConstraintSet | ( |  | ) | const
| inline

Set explicit constraint set.

## ◆ explicitConstraintSetHasChanged()

void hpp::constraints::solver::BySubstitution::explicitConstraintSetHasChanged | ( |  | ) |

Should be called whenever explicit solver is modified.

## ◆ extract()

BySubstitution hpp::constraints::solver::BySubstitution::extract | ( | interval_t | interval | ) |

Return another solver defined over a sub-interval of time

If some constraints have a time varying right hand side, this method will build another solver with the same constraints, except that the right hand side is defined over a sub-interval of time

See also
  DifferentiableFunction::extract.

Parameters

  interval | sub-interval of definition

## ◆ getRightHandSide()

bool hpp::constraints::solver::BySubstitution::getRightHandSide | ( | const ImplicitPtr_t & | constraint,
|  | vectorOut_t | rhs ) const
| virtual

Reimplemented from hpp::constraints::solver::HierarchicalIterative.

## ◆ impl_solve()

template<typename LineSearchType >

HierarchicalIterative::Status hpp::constraints::solver::BySubstitution::impl_solve | ( | vectorOut_t | arg,
|  | bool | _optimize,
|  | LineSearchType | lineSearch ) const
| inline

## ◆ implicitDof()

segments_t hpp::constraints::solver::BySubstitution::implicitDof | ( |  | ) | const

Return the indices in the input vector which are solved implicitely.

The other dof which are modified are solved explicitely.

## ◆ integrate()

bool hpp::constraints::solver::BySubstitution::integrate | ( | vectorIn_t | from,
|  | vectorIn_t | velocity,
|  | vectorOut_t | result ) const
| inline virtual

Reimplemented from hpp::constraints::solver::HierarchicalIterative.

## ◆ isConstraintSatisfied()

bool hpp::constraints::solver::BySubstitution::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/3]

bool hpp::constraints::solver::BySubstitution::isSatisfied | ( | vectorIn_t | arg | ) | const
| inline

Whether input vector satisfies the constraints of the solver

Parameters

  arg | input vector. Compares to internal error threshold.

## ◆ isSatisfied() [2/3]

bool hpp::constraints::solver::BySubstitution::isSatisfied | ( | vectorIn_t | arg,
|  | value_type | errorThreshold ) const
| 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.

## ◆ isSatisfied() [3/3]

bool hpp::constraints::solver::BySubstitution::isSatisfied | ( | vectorIn_t | arg,
|  | vectorOut_t | error ) const
| inline

Whether input vector satisfies the constraints of the solver

Parameters

  arg | input vector

Return values

  error | the constraint errors dispatched in a vector, the head of the vector corresponds to implicit constraints, the tail of the vector corresponds to explicit constraints.

## ◆ numberFreeVariables()

size_type hpp::constraints::solver::BySubstitution::numberFreeVariables | ( |  | ) | const
| inline

Return the number of free variables.

## ◆ numericalConstraints()

const NumericalConstraints_t & hpp::constraints::solver::BySubstitution::numericalConstraints | ( |  | ) | const
| inline

Get the numerical constraints implicit and explicit.

## ◆ oneStep()

template<typename LineSearchType >

bool hpp::constraints::solver::BySubstitution::oneStep | ( | vectorOut_t | arg,
|  | LineSearchType & | lineSearch ) const
| inline

## ◆ print()

virtual std::ostream & hpp::constraints::solver::BySubstitution::print | ( | std::ostream & | os | ) | const
| virtual

Reimplemented from hpp::constraints::solver::HierarchicalIterative.

## ◆ projectOnKernel()

virtual void hpp::constraints::solver::BySubstitution::projectOnKernel | ( | ConfigurationIn_t | from,
|  | ConfigurationIn_t | to,
|  | ConfigurationOut_t | result )
| virtual

Project configuration "to" on constraint tangent space in "from"

Parameters

  from | configuration,
  to | configuration to project

## ◆ projectVectorOnKernel()

void hpp::constraints::solver::BySubstitution::projectVectorOnKernel | ( | ConfigurationIn_t | from,
|  | vectorIn_t | velocity,
|  | vectorOut_t | result ) const

Project velocity on constraint tangent space in "from"

Parameters

  from | configuration,
  velocity | velocity to project

## ◆ rightHandSide() [1/3]

vector_t hpp::constraints::solver::BySubstitution::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]

bool hpp::constraints::solver::BySubstitution::rightHandSide | ( | const ImplicitPtr_t & | constraint,
|  | vectorIn_t | rhs )
| 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 from hpp::constraints::solver::HierarchicalIterative.

## ◆ rightHandSide() [3/3]

void hpp::constraints::solver::BySubstitution::rightHandSide | ( | vectorIn_t | rhs | ) |
| 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 from hpp::constraints::solver::HierarchicalIterative.

## ◆ rightHandSideFromConfig() [1/2]

vector_t hpp::constraints::solver::BySubstitution::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 .

Note
  Only parameterizable constraints (type Equality) are set

## ◆ rightHandSideFromConfig() [2/2]

bool hpp::constraints::solver::BySubstitution::rightHandSideFromConfig | ( | const ImplicitPtr_t & | constraint,
|  | ConfigurationIn_t | config )
| virtual

Reimplemented from hpp::constraints::solver::HierarchicalIterative.

## ◆ rightHandSideSize()

size_type hpp::constraints::solver::BySubstitution::rightHandSideSize | ( |  | ) | const

Get size of the right hand side

Returns
  sum of dimensions of parameterizable constraints (type Equality)

## ◆ solve() [1/3]

Status hpp::constraints::solver::BySubstitution::solve | ( | vectorOut_t | arg | ) | const
| inline

## ◆ solve() [2/3]

template<typename LineSearchType >

Status hpp::constraints::solver::BySubstitution::solve | ( | vectorOut_t | arg,
|  | bool | optimize,
|  | LineSearchType | ls = LineSearchType() ) const
| inline

## ◆ solve() [3/3]

template<typename LineSearchType >

Status hpp::constraints::solver::BySubstitution::solve | ( | vectorOut_t | arg,
|  | LineSearchType | ls = LineSearchType() ) const
| inline

## ◆ updateJacobian()

void hpp::constraints::solver::BySubstitution::updateJacobian | ( | vectorIn_t | arg | ) | const

Computes the jacobian of the explicit functions and updates the jacobian of the problem using the chain rule.

The documentation for this class was generated from the following files:

- include/hpp/constraints/solver/by-substitution.hh
- include/hpp/constraints/solver/impl/by-substitution.hh