#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 std::size_t &priority=0)
BySubstitution	extract (interval_t interval)
const NumericalConstraints_t &	numericalConstraints () const
	Get the numerical constraints implicit and explicit.
ExplicitConstraintSet &	explicitConstraintSet ()
	Get explicit constraint set.
const ExplicitConstraintSet &	explicitConstraintSet () const
	Set explicit constraint set.
size_type	numberFreeVariables () const
	Return the number of free variables.
void	explicitConstraintSetHasChanged ()
	Should be called whenever explicit solver is modified.
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, vectorOut_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, value_type errorThreshold) 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.
value_type	errorThreshold () const
	Get error threshold.
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
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.
void	residualError (vectorOut_t error) const
	Returns the error vector.
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.
virtual bool	contains (const ImplicitPtr_t &numericalConstraint) const
virtual void	merge (const HierarchicalIterative &other)
void	saturation (const Saturation_t &saturate)
	Set the saturation function.
const Saturation_t &	saturation () const
	Get the saturation function.
HierarchicalIterative	extract (interval_t interval) const
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	isSatisfied (vectorIn_t arg, value_type errorThreshold) 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.
void	maxIterations (size_type iterations)
	Set maximal number of iterations.
size_type	maxIterations () const
	Get maximal number of iterations in config projector.
void	errorThreshold (const value_type &threshold)
	Set error threshold.
value_type	errorThreshold () const
	Get error threshold.
value_type	squaredErrorThreshold () const
	Get error threshold.
value_type	inequalityThreshold () const
	Get the inequality threshold.
void	inequalityThreshold (const value_type &it)
	set the inequality threshold
void	lastIsOptional (bool optional)
bool	lastIsOptional () const
void	solveLevelByLevel (bool solveLevelByLevel)
bool	solveLevelByLevel () const
const ImplicitConstraintSet &	constraints (const std::size_t priority)
	Get set of constraints for a give priority level.
const NumericalConstraints_t &	constraints () const
	Get constraints (implicit and explicit)
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.
ArrayXb	activeDerivativeParameters () const
	Velocity parameters involved in the constraint resolution.
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.
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.

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

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 shared_ptr< saturation::Base >	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_
bool	solveLevelByLevel_
Indices_t	freeVariables_
	Unknown of the set of implicit constraints.
Saturation_t	saturate_
NumericalConstraints_t	constraints_
	Members moved from core::ConfigProjector.
std::map< DifferentiableFunctionPtr_t, size_type >	iq_
	Value rank of constraint in its priority level.
std::map< DifferentiableFunctionPtr_t, size_type >	iv_
	Derivative rank of constraint in its priority level.
std::map< DifferentiableFunctionPtr_t, std::size_t >	priority_
	Priority level of constraint.
value_type	sigma_
	The smallest non-zero singular value.
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_

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

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

virtual

Add an implicit constraint

Parameters

constraint	implicit constraint
priority	level of priority of the constraint: priority are in decreasing order: 0 is the highest priority level,

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

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

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

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

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

inline

Get error threshold.

◆ errorThreshold() [2/2]

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

inline

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

◆ extract()

BySubstitution hpp::constraints::solver::BySubstitution::extract ( interval_t interval )

Return another solver defined over a sub-interval of time

If some constraints have a time varying right hand side, this method will build another solver with the same constraints, except that the right hand side is defined over a sub-interval of time

See also: DifferentiableFunction::extract.

Parameters

interval sub-interval of definition

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

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

inline

Whether input vector satisfies the constraints of the solver

Parameters

arg	input vector. Compares to internal error threshold.

◆ isSatisfied() [2/3]

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

inline

Whether input vector satisfies the constraints of the solver

Parameters

arg	input vector
errorThreshold	threshold to use instead of the value stored in the solver.

◆ isSatisfied() [3/3]

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,
		vectorOut_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]

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

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

◆ 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

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]

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

template<typename LineSearchType>

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

include/hpp/constraints/solver/by-substitution.hh
include/hpp/constraints/solver/impl/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()

◆ computeActiveRowsOfJ()

◆ errorSize()

◆ errorThreshold() [1/2]

◆ errorThreshold() [2/2]

◆ explicitConstraintSet() [1/2]

◆ explicitConstraintSet() [2/2]

◆ explicitConstraintSetHasChanged()

◆ extract()

◆ getRightHandSide()

◆ impl_solve()

◆ implicitDof()

◆ integrate()

◆ isConstraintSatisfied()

◆ isSatisfied() [1/3]

◆ isSatisfied() [2/3]

◆ isSatisfied() [3/3]

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