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

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

Inheritance diagram for hpp::constraints::solver::BySubstitution:

Collaboration diagram for hpp::constraints::solver::BySubstitution:

Public Member Functions
	BySubstitution (const LiegroupSpacePtr_t &configSpace)

	BySubstitution (const BySubstitution &other)

virtual	~BySubstitution ()

bool	add (const ImplicitPtr_t &numericalConstraint, const segments_t &passiveDofs=segments_t(0), const std::size_t priority=0)
	Add a numerical constraints. More...

void	add (const DifferentiableFunctionPtr_t &f, const std::size_t &priority, const ComparisonTypes_t &comp) HPP_CONSTRAINTS_DEPRECATED
	Add an implicit constraint. More...

const NumericalConstraints_t &	numericalConstraints () const
	Get the numerical constraints implicit and explicit. More...

ExplicitConstraintSet &	explicitConstraintSet ()
	Get explicit constraint set. More...

const ExplicitConstraintSet &	explicitConstraintSet () const
	Set explicit constraint set. More...

virtual bool	contains (const ImplicitPtr_t &numericalConstraint) const
	Check whether a numerical constraint has been added. More...

size_type	numberFreeVariables () const
	Return the number of free variables. More...

void	explicitConstraintSetHasChanged ()
	Should be called whenever explicit solver is modified. More...

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, ConfigurationOut_t result) const
	Project velocity on constraint tangent space in "from". More...

virtual void	projectOnKernel (ConfigurationIn_t from, ConfigurationIn_t to, ConfigurationOut_t result)
	Project configuration "to" on constraint tangent space in "from". More...

Status	solve (vectorOut_t arg) const

bool	isSatisfied (vectorIn_t arg) const

bool	isSatisfied (vectorIn_t arg, vectorOut_t error) const

template<typename LineSearchType >
bool	oneStep (vectorOut_t arg, LineSearchType &lineSearch) const

void	updateJacobian (vectorIn_t arg) const
	Computes the jacobian of the explicit functions and updates the jacobian of the problem using the chain rule. More...

void	errorThreshold (const value_type &threshold)
	Set error threshold. More...

value_type	errorThreshold () const
	Get error threshold. More...

segments_t	implicitDof () const
	Return the indices in the input vector which are solved implicitely. More...

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

deprecated
ExplicitConstraintSet &	explicitSolver () HPP_CONSTRAINTS_DEPRECATED

const ExplicitConstraintSet &	explicitSolver () const HPP_CONSTRAINTS_DEPRECATED

void	explicitSolverHasChanged () HPP_CONSTRAINTS_DEPRECATED

Right hand side accessors
vector_t	rightHandSideFromConfig (ConfigurationIn_t config)
	Compute right hand side of equality constraints from a configuration. More...

bool	rightHandSideFromConfig (const ImplicitPtr_t &constraint, ConfigurationIn_t config)
	Compute right hand side of a constraint from a configuration. More...

bool	rightHandSide (const ImplicitPtr_t &constraint, vectorIn_t rhs)
	Set right hand side of a constraints. More...

bool	getRightHandSide (const ImplicitPtr_t &constraint, vectorOut_t rhs) const

void	rightHandSide (vectorIn_t rhs)
	Set the right hand side. More...

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

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

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

void	residualError (vectorOut_t error) const
	Returns the error vector. More...

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

void	add (const DifferentiableFunctionPtr_t &f, const std::size_t &priority) HPP_CONSTRAINTS_DEPRECATED
	Add an implicit equality constraint. More...

void	add (const DifferentiableFunctionPtr_t &f, const std::size_t &priority, const ComparisonTypes_t &comp) HPP_CONSTRAINTS_DEPRECATED
	Add an implicit constraint. More...

void	add (const ImplicitPtr_t &constraint, const std::size_t &priority)
	Add an implicit constraint. More...

void	saturation (const Saturation_t &saturate)
	Set the saturation function. More...

const Saturation_t &	saturation () const
	Get the saturation function. More...

template<typename LineSearchType >
Status	solve (vectorOut_t arg, LineSearchType ls=LineSearchType()) const
	Solve the system of non linear equations. More...

Status	solve (vectorOut_t arg) const
	Solve the system of non linear equations. More...

bool	isSatisfied (vectorIn_t arg) const

const value_type &	sigma () const
	Returns the lowest singular value. More...

void	freeVariables (const segments_t intervals)
	Set free velocity variables. More...

void	freeVariables (const Indices_t &indices)
	Set free velocity variables. More...

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

const ImplicitConstraintSet &	constraints (const std::size_t priority)
	Get set of constraints for a give priority level. More...

std::size_t	numberStacks () const

const size_type &	dimension () const

const size_type &	reducedDimension () const
	Dimension of the problem after removing the rows of the jacobian which do not influence the error (only zeros along those lines). More...

ArrayXb	activeParameters () const
	Configuration parameters involved in the constraint resolution. More...

ArrayXb	activeDerivativeParameters () const
	Velocity parameters involved in the constraint resolution. More...

vector_t	rightHandSideFromConfig (ConfigurationIn_t config)
	Compute right hand side of equality constraints from a configuration. More...

void	rightHandSideAt (const value_type &s)
	Set the right hand side at a given parameter. More...

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

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

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
	If lastIsOptional() is true, then the last level is ignored. More...

const vector_t &	lastStep () const
	Accessor to the last step done. More...

Protected Member Functions
void	computeActiveRowsOfJ (std::size_t iStack)
	Compute which rows of the jacobian of stack_[iStack] are not zero, using the activeDerivativeParameters of the functions. More...

Protected Member Functions inherited from hpp::constraints::solver::HierarchicalIterative
void	update ()
	Allocate datas and update sizes of the problem Should be called whenever the stack is modified. More...

void	computeDescentDirection () const
	Compute a SVD decomposition of each level and find the best descent direction at the first order. More...

void	expandDqSmall () const

void	saturate (vectorOut_t arg) const

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 boost::function< bool(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi &saturation)>	Saturation_t
	This function checks which degrees of freedom are saturated. More...

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_

Indices_t	freeVariables_
	Unknown of the set of implicit constraints. More...

Saturation_t	saturate_

NumericalConstraints_t	constraints_
	Members moved from core::ConfigProjector. More...

std::map< DifferentiableFunctionPtr_t, size_type >	iq_
	Value rank of constraint in its priority level. More...

std::map< DifferentiableFunctionPtr_t, size_type >	iv_
	Derivative rank of constraint in its priority level. More...

std::map< DifferentiableFunctionPtr_t, std::size_t >	priority_
	Priority level of constraint. More...

value_type	sigma_
	The smallest non-zero singular value. More...

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_

mutable::hpp::statistics::SuccessStatistics	statistics_

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 \(\mathbf{q}\)) 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: \(g_1 (\mathbf{q}) = 0, g_2 (\mathbf{q}) = 0, \cdots\). 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

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

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

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

inlinevirtual

Member Function Documentation

bool hpp::constraints::solver::BySubstitution::add	(	const ImplicitPtr_t &	numericalConstraint,
		const segments_t &	passiveDofs = `segments_t(0)`,
		const std::size_t	priority = `0`
	)

Add a numerical constraints.

Parameters

numericalConstraint	The numerical constraint.
passiveDofs	degrees of freedom that are not modified during implicit constraint resolution: column indices of the Jacobian vector that will are set to zero before pseudo-inversion,
priority	priority of the function. The last level might be optional,

Returns: false if numerical constraint had already been inserted.

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

Note: The intervals are interpreted as a list of couple (index_start, length) and NOT as (index_start, index_end).

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

Deprecated:: Use bool BySubstitution::add (const ImplicitPtr_t& numericalConstraint, const segments_t& passiveDofs = segments_t (0), const std::size_t priority = 0) instead.

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

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

virtual bool hpp::constraints::solver::BySubstitution::contains ( const ImplicitPtr_t & numericalConstraint ) const

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.

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

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

inline

Set error threshold.

References hpp::constraints::solver::HierarchicalIterative::errorThreshold().

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

inline

Get error threshold.

References hpp::constraints::solver::HierarchicalIterative::errorThreshold().

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

inline

Get explicit constraint set.

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

inline

Set explicit constraint set.

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

Should be called whenever explicit solver is modified.

ExplicitConstraintSet& hpp::constraints::solver::BySubstitution::explicitSolver ( )

inline

Deprecated:: Use explicitConstraintSet instead

const ExplicitConstraintSet& hpp::constraints::solver::BySubstitution::explicitSolver ( ) const

inline

Deprecated:: Use explicitConstraintSet instead

void hpp::constraints::solver::BySubstitution::explicitSolverHasChanged ( )

inline

Deprecated:: call explicitConstraintSetHasChanged instead

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

virtual

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

template<typename LineSearchType >

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

inline

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.

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

inlinevirtual

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

References hpp::constraints::solver::HierarchicalIterative::integrate().

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

inline

References hpp::constraints::solver::HierarchicalIterative::isSatisfied().

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

inline

References Eigen::assert(), and hpp::constraints::solver::HierarchicalIterative::isSatisfied().

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

inline

Return the number of free variables.

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

inline

Get the numerical constraints implicit and explicit.

template<typename LineSearchType >

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

inline

References hpp::constraints::solver::HierarchicalIterative::isSatisfied().

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

virtual

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

Referenced by hpp::constraints::solver::operator<<().

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

\[ \textbf{q}_{res} = \textbf{q}_{from} + \left(I_n - J^{+}J(\textbf{q}_{from})\right) (\textbf{q}_{to} - \textbf{q}_{from}) \]

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

Project velocity on constraint tangent space in "from".

Parameters

from	configuration,
velocity	velocity to project

\[ \textbf{q}_{res} = \left(I_n - J^{+}J(\textbf{q}_{from})\right) (\textbf{v}) \]

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.

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.

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

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 \(rhs = f(\mathbf{q})\).

Note: Only parameterizable constraints (type Equality) are set

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

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

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

Get size of the right hand side.

Returns: sum of dimensions of parameterizable constraints (type Equality)

template<typename LineSearchType >

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

inline

template<typename LineSearchType >

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

inline

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

inline

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.

Referenced by impl_solve().

Public Member Functions

Protected Member Functions

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation