Set of explicit constraints. More...

#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
	Return Jacobian matrix of output variable wrt not output variables. More...

MatrixBlockView	jacobianNotOutToOut (matrix_t &jacobian) const
	Return Jacobian matrix of output variable wrt not output variables. More...

void	jacobian (matrixOut_t jacobian, vectorIn_t q) const
	Compute the Jacobian of the explicit constraint resolution. More...

std::ostream &	print (std::ostream &os) const

Resolution
bool	solve (vectorOut_t arg) const
	Solve the explicit constraints. More...

bool	isSatisfied (vectorIn_t arg) const

bool	isSatisfied (vectorIn_t arg, vectorOut_t error) const

Construction of the problem
size_type	add (const ExplicitPtr_t &constraint)
	Attempt to add an explicit constraint. More...

bool	contains (const ExplicitPtr_t &numericalConstraint) const
	Check whether an explicit numerical constraint has been added. More...

	ExplicitConstraintSet (const LiegroupSpacePtr_t &space)
	Constructor. More...

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
	Set \(i\overline{n\cup ou}t\) of other configuration variables. More...

const ColBlockIndices &	freeDers () const HPP_CONSTRAINTS_DEPRECATED
	Set \(i\overline{n\cup ou}t\) of other velocity variables. More...

const RowBlockIndices &	notOutArgs () const
	Set \(i\overline{n\cup ou}t\) of other configuration variables. More...

const ColBlockIndices &	notOutDers () const
	Set \(i\overline{n\cup ou}t\) of other velocity variables. More...

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

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

const Eigen::MatrixXi &	inOutDependencies () const
	Returns a matrix of integer whose: More...

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

const Eigen::VectorXi &	derFunction () const

const RowBlockIndices &	outArgs () const
	Set \(out\) of output configuration variables. More...

const RowBlockIndices &	outDers () const
	Set of output velocity variables. More...

std::size_t	argSize () const HPP_CONSTRAINTS_DEPRECATED
	The number of configuration variables. More...

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

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)
	Compute right hand side of constraints using input configuration. More...

bool	rightHandSideFromInput (const ExplicitPtr_t &constraint, vectorIn_t p)
	Compute right hand side of constraint using input configuration. More...

void	rightHandSideFromInput (const size_type &i, vectorIn_t p)
	Compute right hand side of constraint using input configuration. More...

void	rightHandSide (vectorIn_t rhs)
	Set the right hand sides of the explicit constraints. More...

bool	rightHandSide (const ExplicitPtr_t &constraint, vectorIn_t rhs)
	Set the right hand side for a given explicit constraint. More...

bool	getRightHandSide (const ExplicitPtr_t &constraint, vectorOut_t rhs) const
	Get the right hand side for a given explicit constraint. More...

void	rightHandSide (const size_type &i, vectorIn_t rhs)
	Set the right hand side for a given explicit constraint. More...

vector_t	rightHandSide () const
	Get the right hand sides. More...

size_type	rightHandSideSize () const
	Get size of the right hand side. More...

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

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

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

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

Constructor & Destructor Documentation

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

inline

Constructor.

Parameters

space Lie group on which constraints are defined.

Member Function Documentation

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

Same as inDers.

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

Same as inArgs.

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.

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

inline

The number of configuration variables.

Deprecated:: use nq instead.

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

inline

The Lie group on which constraints are defined.

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.

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

inline

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

inline

The number of derivative variables.

Deprecated:: use nv instead.

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

inline

Set error threshold.

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

inline

Get error threshold.

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

inline

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

Deprecated:: use notOutArgs

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

inline

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

Deprecated:: use notOutDers

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.

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

inline

Set \(in\) of input configuration variables.

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

inline

Set of input velocity variables.

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

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

Same as inOutDependencies except that cols correpond to DoFs.

bool hpp::constraints::ExplicitConstraintSet::isSatisfied ( vectorIn_t arg ) const

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

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.

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.

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

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

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

inline

The number of variables.

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

inline

The number of derivative variables.

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.

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.

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

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.

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.

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.

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

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.

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.

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.

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

Get size of the right hand side.

See documentation of classes Implicit and Explicit for details.

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)\)

Referenced by hpp::constraints::solver::BySubstitution::impl_solve().

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

inline

Get error threshold.

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

inline

Return Jacobian matrix of output variable wrt not output variables.

Deprecated:: use jacobianNotOutToOut instead.

Public Types

Public Member Functions

Detailed Description

Member Typedef Documentation

Constructor & Destructor Documentation

Member Function Documentation