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

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

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 segments_t &passiveDofs=segments_t(0), const std::size_t priority=0)

void	add (const DifferentiableFunctionPtr_t &f, const std::size_t &priority, const ComparisonTypes_t &comp) HPP_CONSTRAINTS_DEPRECATED

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

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

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

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

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

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)

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

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

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

void	add (const DifferentiableFunctionPtr_t &f, const std::size_t &priority) HPP_CONSTRAINTS_DEPRECATED

void	add (const DifferentiableFunctionPtr_t &f, const std::size_t &priority, const ComparisonTypes_t &comp) HPP_CONSTRAINTS_DEPRECATED

void	add (const ImplicitPtr_t &constraint, const std::size_t &priority)

virtual void	merge (const HierarchicalIterative &other)

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

Status	solve (vectorOut_t arg) const

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

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

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

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

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

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

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

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

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

◆ 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 ( )

inlinevirtual

Member Function Documentation

◆ add() [1/2]

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

◆ add() [2/2]

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.

◆ computeActiveRowsOfJ()

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.

◆ contains()

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.

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

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]

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

inline

Set error threshold.

◆ errorThreshold() [2/2]

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

inline

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

◆ explicitSolver() [1/2]

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

inline

Deprecated:: Use explicitConstraintSet instead

◆ explicitSolver() [2/2]

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

inline

Deprecated:: Use explicitConstraintSet instead

◆ explicitSolverHasChanged()

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

inline

Deprecated:: call explicitConstraintSetHasChanged instead

◆ 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

inlinevirtual

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/2]

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

inline

Whether input vector satisfies the constraints of the solver

Parameters

arg	input vector

◆ isSatisfied() [2/2]

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

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

◆ projectVectorOnKernel()

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

◆ rightHandSide() [1/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() [2/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.

◆ rightHandSide() [3/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).

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

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

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.

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

template<typename LineSearchType >

Status hpp::constraints::solver::BySubstitution::solve	(	vectorOut_t	arg,
		LineSearchType	ls = `LineSearchType()`
	)		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]

Status hpp::constraints::solver::BySubstitution::solve ( vectorOut_t arg ) 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 file:

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

Public Member Functions

Protected Member Functions

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ BySubstitution() [1/2]

◆ BySubstitution() [2/2]

◆ ~BySubstitution()

Member Function Documentation

◆ add() [1/2]

◆ add() [2/2]

◆ computeActiveRowsOfJ()

◆ contains()

◆ errorSize()

◆ errorThreshold() [1/2]

◆ errorThreshold() [2/2]

◆ explicitConstraintSet() [1/2]

◆ explicitConstraintSet() [2/2]

◆ explicitConstraintSetHasChanged()

◆ explicitSolver() [1/2]

◆ explicitSolver() [2/2]

◆ explicitSolverHasChanged()

◆ getRightHandSide()

◆ impl_solve()

◆ implicitDof()

◆ integrate()

◆ isConstraintSatisfied()

◆ isSatisfied() [1/2]

◆ isSatisfied() [2/2]

◆ numberFreeVariables()

◆ numericalConstraints()

◆ oneStep()

◆ print()

◆ projectOnKernel()

◆ projectVectorOnKernel()

◆ rightHandSide() [1/3]

◆ rightHandSide() [2/3]

◆ rightHandSide() [3/3]

◆ rightHandSideFromConfig() [1/2]

◆ rightHandSideFromConfig() [2/2]

◆ rightHandSideSize()

◆ solve() [1/3]

◆ solve() [2/3]

◆ solve() [3/3]

◆ updateJacobian()