#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	viewJacobian (matrix_t &jacobian) const HPP_CONSTRAINTS_DEPRECATED

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

bool	isSatisfied (vectorIn_t arg) 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

Construction of the problem
size_type	add (const ExplicitPtr_t &constraint)

bool	contains (const ExplicitPtr_t &numericalConstraint) const

	ExplicitConstraintSet (const LiegroupSpacePtr_t &space)

Parameters
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...

Input and outputs
const RowBlockIndices &	inArgs () const
	Set \(in\) of input configuration variables. More...

const ColBlockIndices &	inDers () const
	Set of input velocity variables. More...

const RowBlockIndices &	freeArgs () const HPP_CONSTRAINTS_DEPRECATED

const ColBlockIndices &	freeDers () const HPP_CONSTRAINTS_DEPRECATED

const RowBlockIndices &	notOutArgs () const

const ColBlockIndices &	notOutDers () const

ColBlockIndices	activeParameters () const
	Same as inArgs. More...

const ColBlockIndices &	activeDerivativeParameters () const
	Same as inDers. More...

const Eigen::MatrixXi &	inOutDependencies () const

Eigen::MatrixXi	inOutDofDependencies () const
	Same as inOutDependencies except that cols correpond to DoFs. More...

const Eigen::VectorXi &	derFunction () const

const RowBlockIndices &	outArgs () const

const RowBlockIndices &	outDers () const

std::size_t	argSize () const HPP_CONSTRAINTS_DEPRECATED

std::size_t	derSize () const HPP_CONSTRAINTS_DEPRECATED

LiegroupSpacePtr_t	configSpace () const
	The Lie group on which constraints are defined. More...

std::size_t	nq () const
	The number of variables. More...

std::size_t	nv () const
	The number of derivative variables. More...

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

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 \(E=(in,out,f)\) on a robot configuration space \(\mathcal{C}\) is defined by

a subset of input indices \(in\subset\{1,\cdots, \dim\mathcal{C}\}\),
a subset of output indices \(out\subset\{1,\cdots, \dim\mathcal{C}\}\),
a smooth mapping \(f\) from \(\mathbf{R}^{|in|}\) to \(\mathbf{R}^{|out|}\), satisfying the following properties:
\(in\cap out = \emptyset\),
for any \(\mathbf{p}\in\mathcal{C}\), \(\mathbf{q} = E(\mathbf{p})\) is defined by
\begin{eqnarray} &\mathbf{q}_{\bar{out}} = \mathbf{p}_{\bar{out}}\\ &\mathbf{q}_{out} = f (\mathbf{p}_{in}). \end{eqnarray}

Note: Right hand side.

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

\begin{equation} \mathbf{q}_{out} = f (\mathbf{p}_{in}) + rhs. \end{equation}

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 \( g \) (of known inverse \( g^{-1} \)) can be specified for each explicit constraint \(E\). The above expression then becomes:
\begin{equation} g(\mathbf{q}_{out}) = f(\mathbf{p}_{in}) + rhs \end{equation}

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

\(in\) for the set of indices of input variables,
\(out\) 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()

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

inline

Constructor

Parameters

space Lie group on which constraints are defined.

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.

◆ argSize()

std::size_t hpp::constraints::ExplicitConstraintSet::argSize ( ) const

inline

The number of configuration variables

Deprecated:: use nq instead.

◆ 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

◆ derSize()

std::size_t hpp::constraints::ExplicitConstraintSet::derSize ( ) const

inline

The number of derivative variables

Deprecated:: use nv instead.

◆ errorThreshold() [1/2]

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

inline

Set error threshold.

◆ errorThreshold() [2/2]

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

inline

Get error threshold.

◆ freeArgs()

const RowBlockIndices& hpp::constraints::ExplicitConstraintSet::freeArgs ( ) const

inline

Set \(i\overline{n\cup ou}t\) of other configuration variables

Deprecated:: use notOutArgs

◆ freeDers()

const ColBlockIndices& hpp::constraints::ExplicitConstraintSet::freeDers ( ) const

inline

Set \(i\overline{n\cup ou}t\) of other velocity variables

Deprecated:: use notOutDers

◆ 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 \(in\) 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 ) const

Whether input vector satisfies the constraints of the solver

Parameters

arg	input vector

◆ isSatisfied() [2/2]

bool hpp::constraints::ExplicitConstraintSet::isSatisfied	(	vectorIn_t	arg,
		vectorOut_t	error
	)		const

Whether input vector satisfies the constraints of the solver

Parameters

arg	input vector

Return values

error the constraint errors

◆ 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 \(\mathbf{p}\in\mathcal{C}\), \(\mathbf{q} = E(\mathbf{p})\) defined by

\begin{eqnarray} \mathbf{q}_{\bar{out}} &=& \mathbf{p}_{out} \\ \mathbf{q}_{out} &=& g^{-1} (f (\mathbf{p}_{in}) + rhs) \end{eqnarray}

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 \(in\) are filled with the Jacobian of \(f\):

\begin{equation} \frac{\partial f}{\partial \mathbf{q}_{in}} (\mathbf{q}_{in}). \end{equation}

The columns corresponding to variables \(i\overline{n\cup ou}t\) are set to 0.

◆ notOutArgs()

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

inline

Set \(i\overline{n\cup ou}t\) of other configuration variables

Configuration variables that are not output variables

◆ notOutDers()

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

inline

Set \(i\overline{n\cup ou}t\) 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 \(out\) 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]

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.

◆ 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]

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

◆ rightHandSideFromInput() [1/3]

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

Compute right hand side of constraints using input configuration.

Parameters

p	vector in \(\mathcal{C}\).

For each explicit constraint \(E=(in,out,f)\), compute the right hand side as follows:

\begin{equation} rhs = f (\mathbf{q}), \end{equation}

where in general

\begin{equation} f(\mathbf{q}) = \mathbf{p}_{out} - f(\mathbf{q}_{in), \end{equation}

in such a way that all \(\mathbf{q}\) 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]

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 \(\mathcal{C}\).

Let \(E=(in,out,f)\) be the explicit constraint, compute the right hand side as follows:

\begin{equation} rhs = f (\mathbf{q}), \end{equation}

where in general

\begin{equation} f(\mathbf{q}) = \mathbf{p}_{out} - f(\mathbf{q}_{in), \end{equation}

in such a way that all \(\mathbf{q}\) 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]

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 \(\mathcal{C}\).

Let \(E=(in,out,f)\) be the explicit constraint, compute the right hand side as follows:

\begin{equation} rhs = f (\mathbf{q}), \end{equation}

where in general

\begin{equation} f(\mathbf{q}) = \mathbf{p}_{out} - f(\mathbf{q}_{in), \end{equation}

in such a way that all \(\mathbf{q}\) 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: \(\mathbf{q}_{out} = g^{-1} \left(f(\mathbf{q}_{in}) + rhs\right)\)

◆ squaredErrorThreshold()

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

inline

Get error threshold.

◆ viewJacobian()

MatrixBlockView hpp::constraints::ExplicitConstraintSet::viewJacobian ( matrix_t & jacobian ) const

inline

Return Jacobian matrix of output variable wrt not output variables

Deprecated:: use jacobianNotOutToOut instead.

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

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

Public Types

Public Member Functions

Detailed Description

Member Typedef Documentation

◆ ColBlockIndices

◆ MatrixBlockView

◆ RowBlockIndices

Constructor & Destructor Documentation

◆ ExplicitConstraintSet()

Member Function Documentation

◆ activeDerivativeParameters()

◆ activeParameters()

◆ add()

◆ argSize()

◆ configSpace()

◆ contains()

◆ derFunction()

◆ derSize()

◆ errorThreshold() [1/2]

◆ errorThreshold() [2/2]

◆ freeArgs()

◆ freeDers()

◆ getRightHandSide()

◆ inArgs()

◆ inDers()

◆ inOutDependencies()

◆ inOutDofDependencies()

◆ isConstraintSatisfied()

◆ isSatisfied() [1/2]

◆ isSatisfied() [2/2]

◆ jacobian()

◆ jacobianNotOutToOut()

◆ notOutArgs()

◆ notOutDers()

◆ nq()

◆ nv()

◆ outArgs()

◆ outDers()

◆ print()

◆ rightHandSide() [1/4]

◆ rightHandSide() [2/4]

◆ rightHandSide() [3/4]

◆ rightHandSide() [4/4]

◆ rightHandSideFromInput() [1/3]

◆ rightHandSideFromInput() [2/3]

◆ rightHandSideFromInput() [3/3]

◆ rightHandSideSize()

◆ solve()

◆ squaredErrorThreshold()

◆ viewJacobian()