hpp-constraints: hpp::constraints::ExplicitConstraintSet Class Reference

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

Loading...
Searching...
No Matches
Public Types | Public Member Functions | Friends | List of all members
hpp::constraints::ExplicitConstraintSet Class Reference
Constraint solvers

`#include <hpp/constraints/explicit-constraint-set.hh>`

## Public Types
typedef Eigen::RowBlockIndices | RowBlockIndices
typedef Eigen::ColBlockIndices | ColBlockIndices
typedef Eigen::MatrixBlockView< matrix_t, Eigen::Dynamic, Eigen::Dynamic, false, false > | MatrixBlockView

## Public Member Functions
MatrixBlockView | jacobianNotOutToOut (matrix_t &jacobian) const
void | jacobian (matrixOut_t jacobian, vectorIn_t q) const
std::ostream & | print (std::ostream &os) const
Resolution
bool | solve (vectorOut_t arg) const
size_type | errorSize () const
bool | isSatisfied (vectorIn_t arg, value_type errorThreshold=-1) const
bool | isSatisfied (vectorIn_t arg, vectorOut_t error, value_type errorThreshold=-1) const
bool | isConstraintSatisfied (const ImplicitPtr_t &constraint, vectorIn_t arg, vectorOut_t error, bool &constraintFound) const
Construction of the problem
size_type | add (const ExplicitPtr_t &constraint)
bool | contains (const ExplicitPtr_t &numericalConstraint) const
| ExplicitConstraintSet (const LiegroupSpacePtr_t &space)
| ExplicitConstraintSet (const ExplicitConstraintSet &other)
Parameters
void | errorThreshold (const value_type &threshold)
| Set error threshold.
value_type | errorThreshold () const
| Get error threshold.
value_type | squaredErrorThreshold () const
| Get error threshold.
Input and outputs
const RowBlockIndices & | inArgs () const
| Set of input configuration variables.
const ColBlockIndices & | inDers () const
| Set of input velocity variables.
const RowBlockIndices & | notOutArgs () const
const ColBlockIndices & | notOutDers () const
ColBlockIndices | activeParameters () const
| Same as inArgs.
const ColBlockIndices & | activeDerivativeParameters () const
| Same as inDers.
const Eigen::MatrixXi & | inOutDependencies () const
Eigen::MatrixXi | inOutDofDependencies () const
| Same as inOutDependencies except that cols correpond to DoFs.
const Eigen::VectorXi & | derFunction () const
const RowBlockIndices & | outArgs () const
const RowBlockIndices & | outDers () const
LiegroupSpacePtr_t | configSpace () const
| The Lie group on which constraints are defined.
std::size_t | nq () const
| The number of variables.
std::size_t | nv () const
| The number of derivative variables.
Right hand side accessors
vector_t | rightHandSideFromInput (vectorIn_t p)
bool | rightHandSideFromInput (const ExplicitPtr_t &constraint, vectorIn_t p)
void | rightHandSideFromInput (const size_type &i, vectorIn_t p)
void | rightHandSide (vectorIn_t rhs)
bool | rightHandSide (const ExplicitPtr_t &constraint, vectorIn_t rhs)
bool | getRightHandSide (const ExplicitPtr_t &constraint, vectorOut_t rhs) const
void | rightHandSide (const size_type &i, vectorIn_t rhs)
vector_t | rightHandSide () const
size_type | rightHandSideSize () const

## Friends
class | solver::BySubstitution

## Detailed Description

Set of explicit constraints

This class combines compatible explicit constraints as defined in the following paper published in Robotics Science and System 2018: https://hal.archives-ouvertes.fr/hal-01804774/file/paper.pdf, Section II-B Definition 4.

An explicit constraint on a robot configuration space is defined by

- a subset of input indices ,
- a subset of output indices ,
- a smooth mapping from to , satisfying the following properties:
- ,
- for any , is defined by

Note
  Right hand side.

For manipulation planning, it is useful to handle a parameterizable right hand side . The expression above thus becomes

The right hand side may be set using the various methods ExplicitConstraintSet::rightHandSide and ExplicitConstraintSet::rightHandSideFromInput.

Note
  For some applications like manipulation planning, an invertible function (of known inverse ) can be specified for each explicit constraint . The above expression then becomes:

To add explicit constraints, use methods ExplicitConstraintSet::add. If the constraint to add is not compatible with the previous one, this method returns -1.

Method ExplicitConstraintSet::solve solves the explicit constraints.

The combination of compatible explicit constraints is an explicit constraint. As such this class can be considered as an explicit constraint.

We will therefore use the following notation

- for the set of indices of input variables,
- for the set of indices of output variables.

## Member Typedef Documentation

## ◆ ColBlockIndices

typedef Eigen::ColBlockIndices hpp::constraints::ExplicitConstraintSet::ColBlockIndices

## ◆ MatrixBlockView

typedef Eigen::MatrixBlockView<matrix_t, Eigen::Dynamic, Eigen::Dynamic, false, false> hpp::constraints::ExplicitConstraintSet::MatrixBlockView

## ◆ RowBlockIndices

typedef Eigen::RowBlockIndices hpp::constraints::ExplicitConstraintSet::RowBlockIndices

## Constructor & Destructor Documentation

## ◆ ExplicitConstraintSet() [1/2]

hpp::constraints::ExplicitConstraintSet::ExplicitConstraintSet | ( | const LiegroupSpacePtr_t & | space | ) |
| inline

Constructor

Parameters

  space | Lie group on which constraints are defined.

## ◆ ExplicitConstraintSet() [2/2]

hpp::constraints::ExplicitConstraintSet::ExplicitConstraintSet | ( | const ExplicitConstraintSet & | other | ) |
| inline

## Member Function Documentation

## ◆ activeDerivativeParameters()

const ColBlockIndices & hpp::constraints::ExplicitConstraintSet::activeDerivativeParameters | ( |  | ) | const

Same as inDers.

## ◆ activeParameters()

ColBlockIndices hpp::constraints::ExplicitConstraintSet::activeParameters | ( |  | ) | const

Same as inArgs.

## ◆ add()

size_type hpp::constraints::ExplicitConstraintSet::add | ( | const ExplicitPtr_t & | constraint | ) |

Attempt to add an explicit constraint

Parameters

  constraint | explicit constraint

Returns
  the index of the function if the function was added, -1 otherwise.

Note
  A function can be added iff it is compatible with the previously added functions.

## ◆ configSpace()

LiegroupSpacePtr_t hpp::constraints::ExplicitConstraintSet::configSpace | ( |  | ) | const
| inline

The Lie group on which constraints are defined.

## ◆ contains()

bool hpp::constraints::ExplicitConstraintSet::contains | ( | const ExplicitPtr_t & | numericalConstraint | ) | const

Check whether an explicit numerical constraint has been added

Parameters

  numericalConstraint | explicit numerical constraint

Returns
  true if the constraint is in the set.

Note
  Comparison between constraints is performed by function names. This means that two constraints with the same function names are considered as equal.

## ◆ derFunction()

const Eigen::VectorXi & hpp::constraints::ExplicitConstraintSet::derFunction | ( |  | ) | const
| inline

## ◆ errorSize()

size_type hpp::constraints::ExplicitConstraintSet::errorSize | ( |  | ) | const
| inline

Size of error vector

Note
  that this size may differ from size of outDers output,

## ◆ errorThreshold() [1/2]

value_type hpp::constraints::ExplicitConstraintSet::errorThreshold | ( |  | ) | const
| inline

Get error threshold.

## ◆ errorThreshold() [2/2]

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

Set error threshold.

## ◆ getRightHandSide()

bool hpp::constraints::ExplicitConstraintSet::getRightHandSide | ( | const ExplicitPtr_t & | constraint,
|  | vectorOut_t | rhs ) const

Get the right hand side for a given explicit constraint

Parameters

  constraint | the explicit constraint,
  rhs | right hand side.

## ◆ inArgs()

const RowBlockIndices & hpp::constraints::ExplicitConstraintSet::inArgs | ( |  | ) | const
| inline

Set of input configuration variables.

## ◆ inDers()

const ColBlockIndices & hpp::constraints::ExplicitConstraintSet::inDers | ( |  | ) | const
| inline

Set of input velocity variables.

## ◆ inOutDependencies()

const Eigen::MatrixXi & hpp::constraints::ExplicitConstraintSet::inOutDependencies | ( |  | ) | const
| inline

Returns a matrix of integer whose:

- rows correspond to functions
- cols correspond to DoF
- values correspond to the dependency degree of a function wrt to a DoF

## ◆ inOutDofDependencies()

Eigen::MatrixXi hpp::constraints::ExplicitConstraintSet::inOutDofDependencies | ( |  | ) | const

Same as inOutDependencies except that cols correpond to DoFs.

## ◆ isConstraintSatisfied()

bool hpp::constraints::ExplicitConstraintSet::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]

bool hpp::constraints::ExplicitConstraintSet::isSatisfied | ( | vectorIn_t | arg,
|  | value_type | errorThreshold = -1 ) const

Whether input vector satisfies the constraints of the solver

Parameters

  arg | input vector
  errorThreshold | threshold to compare against the norms of the constraint errors. Default value is accessible via methods errorThreshold.

## ◆ isSatisfied() [2/2]

bool hpp::constraints::ExplicitConstraintSet::isSatisfied | ( | vectorIn_t | arg,
|  | vectorOut_t | error,
|  | value_type | errorThreshold = -1 ) const

Whether input vector satisfies the constraints of the solver

Parameters

  arg | input vector

Return values

  error | the constraint errors

Parameters

  errorThreshold | threshold to compare against the norms of the constraint errors. Default value is accessible via methods errorThreshold.

## ◆ jacobian()

void hpp::constraints::ExplicitConstraintSet::jacobian | ( | matrixOut_t | jacobian,
|  | vectorIn_t | q ) const

Compute the Jacobian of the explicit constraint resolution

Parameters

  q | input configuration
  jacobian | square Jacobian matrix of same size as velocity i.e. given by nv method.

The result is the Jacobian of the explicit constraint set considered as a projector that maps to any , defined by

Warning
  it is assumed solve(q) has been called before.

## ◆ jacobianNotOutToOut()

MatrixBlockView hpp::constraints::ExplicitConstraintSet::jacobianNotOutToOut | ( | matrix_t & | jacobian | ) | const
| inline

Return Jacobian matrix of output variable wrt not output variables

Return values

  jacobian | Jacobian matrix of the mapping from non output variables to output variables. The columns of this matrix corresponding to variables are filled with the Jacobian of : The columns corresponding to variables are set to 0.

## ◆ notOutArgs()

const RowBlockIndices & hpp::constraints::ExplicitConstraintSet::notOutArgs | ( |  | ) | const
| inline

Set of other configuration variables

Configuration variables that are not output variables

## ◆ notOutDers()

const ColBlockIndices & hpp::constraints::ExplicitConstraintSet::notOutDers | ( |  | ) | const
| inline

Set of other velocity variables

Velocity variables that are not output variables

## ◆ nq()

std::size_t hpp::constraints::ExplicitConstraintSet::nq | ( |  | ) | const
| inline

The number of variables.

## ◆ nv()

std::size_t hpp::constraints::ExplicitConstraintSet::nv | ( |  | ) | const
| inline

The number of derivative variables.

## ◆ outArgs()

const RowBlockIndices & hpp::constraints::ExplicitConstraintSet::outArgs | ( |  | ) | const
| inline

Set of output configuration variables

Returns
  the set of intervals corresponding the the configuration variables that are ouputs of the combination of explicit constraints.

## ◆ outDers()

const RowBlockIndices & hpp::constraints::ExplicitConstraintSet::outDers | ( |  | ) | const
| inline

Set of output velocity variables

Returns
  the set of intervals corresponding the the velocity variables that are ouputs of the combination of explicit constraints.

## ◆ print()

std::ostream & hpp::constraints::ExplicitConstraintSet::print | ( | std::ostream & | os | ) | const

## ◆ rightHandSide() [1/4]

vector_t hpp::constraints::ExplicitConstraintSet::rightHandSide | ( |  | ) | const

Get the right hand sides

Returns
  the right hand sides of the explicit constraints stacked into a vector

## ◆ rightHandSide() [2/4]

bool hpp::constraints::ExplicitConstraintSet::rightHandSide | ( | const ExplicitPtr_t & | constraint,
|  | vectorIn_t | rhs )

Set the right hand side for a given explicit constraint

Parameters

  constraint | the explicit constraint,
  rhs | right hand side.

## ◆ rightHandSide() [3/4]

void hpp::constraints::ExplicitConstraintSet::rightHandSide | ( | const size_type & | i,
|  | vectorIn_t | rhs )

Set the right hand side for a given explicit constraint

Parameters

  i | order of the explicit constraint,
  rhs | right hand side.

## ◆ rightHandSide() [4/4]

void hpp::constraints::ExplicitConstraintSet::rightHandSide | ( | vectorIn_t | rhs | ) |

Set the right hand sides of the explicit constraints.

Parameters

  rhs | the right hand side.

The components of rhs are dispatched to the right hand sides of the explicit constraints in the order they are added.

## ◆ rightHandSideFromInput() [1/3]

bool hpp::constraints::ExplicitConstraintSet::rightHandSideFromInput | ( | const ExplicitPtr_t & | constraint,
|  | vectorIn_t | p )

Compute right hand side of constraint using input configuration.

Parameters

  constraint | explicit constraint,
  p | vector in .

Let be the explicit constraint, compute the right hand side as follows:

where in general

in such a way that all satisfies the explicit constraint.

Note
  For hpp::constraints::explicit_::RelativePose, the implicit formulation does not derive from the explicit one. The right hand side considered is the right hand side of the implicit formulation.

## ◆ rightHandSideFromInput() [2/3]

void hpp::constraints::ExplicitConstraintSet::rightHandSideFromInput | ( | const size_type & | i,
|  | vectorIn_t | p )

Compute right hand side of constraint using input configuration.

Parameters

  i | index of the explicit constraint,
  p | vector in .

Let be the explicit constraint, compute the right hand side as follows:

where in general

in such a way that all satisfies the explicit constraint.

Note
  For hpp::constraints::explicit_::RelativePose, the implicit formulation does not derive from the explicit one. The right hand side considered is the right hand side of the implicit formulation.

## ◆ rightHandSideFromInput() [3/3]

vector_t hpp::constraints::ExplicitConstraintSet::rightHandSideFromInput | ( | vectorIn_t | p | ) |

Compute right hand side of constraints using input configuration.

Parameters

  p | vector in .

For each explicit constraint , compute the right hand side as follows:

where in general

in such a way that all satisfies the explicit constraint.

Note
  For hpp::constraints::explicit_::RelativePose, the implicit formulation does not derive from the explicit one. The right hand side considered is the right hand side of the implicit formulation.

## ◆ rightHandSideSize()

size_type hpp::constraints::ExplicitConstraintSet::rightHandSideSize | ( |  | ) | const

Get size of the right hand side

See documentation of classes Implicit and Explicit for details.

## ◆ solve()

bool hpp::constraints::ExplicitConstraintSet::solve | ( | vectorOut_t | arg | ) | const

Solve the explicit constraints

Parameters

  arg | input configuration,

Return values

  arg | output configuration satisfying the explicit constraints:

## ◆ squaredErrorThreshold()

value_type hpp::constraints::ExplicitConstraintSet::squaredErrorThreshold | ( |  | ) | const
| inline

Get error threshold.

## Friends And Related Symbol Documentation

## ◆ solver::BySubstitution

friend class solver::BySubstitution
| friend

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

- include/hpp/constraints/explicit-constraint-set.hh