Implicit non-linear constraint. More...

#include <hpp/core/config-projector.hh>

Inheritance diagram for hpp::core::ConfigProjector:

Collaboration diagram for hpp::core::ConfigProjector:

Public Types
enum	LineSearchType { Backtracking, ErrorNormBased, FixedSequence, Constant }

Public Member Functions
virtual ConstraintPtr_t	copy () const
	return shared pointer to copy More...

virtual	~ConfigProjector ()
	Destructor. More...

bool	contains (const constraints::ImplicitPtr_t &numericalConstraint) const
	Check that numerical constraint is in config projector. More...

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

void	lastIsOptional (bool optional)

bool	lastIsOptional () const

bool	optimize (ConfigurationOut_t config, std::size_t maxIter=0)
	Optimize the configuration while respecting the constraints The input configuration must already satisfy the constraints. More...

const DevicePtr_t &	robot () const
	Get robot. More...

void	projectVectorOnKernel (ConfigurationIn_t from, vectorIn_t velocity, vectorOut_t result)
	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...

void	computeValueAndJacobian (ConfigurationIn_t configuration, vectorOut_t value, matrixOut_t reducedJacobian)
	Compute value and reduced jacobian at a given configuration. More...

bool	oneStep (ConfigurationOut_t config, vectorOut_t dq, const value_type &alpha) HPP_CORE_DEPRECATED
	Execute one iteration of the projection algorithm. 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 errorimal number of threshold in config projector. More...

value_type	residualError () const

const value_type &	sigma () const

virtual bool	isSatisfied (ConfigurationIn_t config)
	Check whether a configuration statisfies the constraint. More...

virtual bool	isSatisfied (ConfigurationIn_t config, vector_t &error)
	Check whether a configuration statisfies the constraint. More...

::hpp::statistics::SuccessStatistics &	statistics ()
	Get the statistics. More...

const NumericalConstraints_t &	numericalConstraints () const
	Get the numerical constraints of the config-projector (and so of the Constraint Set) More...

const BySubstitution &	solver () const

Compression of locked degrees of freedom
Degrees of freedom related to locked joint are not taken into account in numerical constraint resolution. The following methods Compress or uncompress vectors or matrices by removing lines and columns corresponding to locked degrees of freedom.
size_type	numberFreeVariables () const
	Return the number of free variables. More...

size_type	numberNonLockedDof () const HPP_CORE_DEPRECATED
	Get number of non-locked degrees of freedom. More...

size_type	dimension () const
	Get constraint dimension. More...

void	compressVector (vectorIn_t normal, vectorOut_t small) const
	Compress Velocity vector by removing output of explicit constraints. More...

void	uncompressVector (vectorIn_t small, vectorOut_t normal) const
	Expand compressed velocity vector. More...

void	compressMatrix (matrixIn_t normal, matrixOut_t small, bool rows=true) const
	Compress matrix. More...

void	uncompressMatrix (matrixIn_t small, matrixOut_t normal, bool rows=true) const
	Uncompress matrix. More...

Right hand side of equalities - inequalities
vector_t	rightHandSideFromConfig (ConfigurationIn_t config)
	Set the right hand side from a configuration. More...

void	rightHandSideFromConfig (const constraints::ImplicitPtr_t &nm, ConfigurationIn_t config)
	Same as rightHandSideFromConfig(ConfigurationIn_t) but only for the specified constraints::Implicit. More...

void	rightHandSide (const vector_t &param)
	Set the level set parameter. More...

void	rightHandSide (const constraints::ImplicitPtr_t &nm, vectorIn_t rhs)
	Same as rightHandSide(vectorIn_t) but only for the specified constraints::Implicit. More...

vector_t	rightHandSide () const
	Get the level set parameter. More...

void	rightHandSideAt (const value_type &s)
	Update the right hand side using Implicit::rightHandSideAt. More...

Public Member Functions inherited from hpp::core::Constraint
bool	apply (ConfigurationOut_t configuration)
	Function that applies the constraint. More...

const std::string &	name () const
	Get name of constraint. More...

virtual	~Constraint ()

Static Public Member Functions
static ConfigProjectorPtr_t	create (const DevicePtr_t &robot, const std::string &name, value_type errorThreshold, size_type maxIterations)
	Return shared pointer to new object. More...

static ConfigProjectorPtr_t	createCopy (const ConfigProjectorPtr_t cp)
	Return shared pointer to copy. More...

static void	defaultLineSearch (LineSearchType ls)

Protected Member Functions
	ConfigProjector (const DevicePtr_t &robot, const std::string &name, value_type errorThreshold, size_type maxIterations)
	Constructor. More...

	ConfigProjector (const ConfigProjector &cp)
	Copy constructor. More...

void	init (const ConfigProjectorPtr_t &self)
	Store weak pointer to itself. More...

virtual bool	impl_compute (ConfigurationOut_t configuration)
	Numerically solve constraint. More...

Protected Member Functions inherited from hpp::core::Constraint
	Constraint (const std::string &name)
	Constructor. More...

	Constraint (const Constraint &constraint)

void	init (const ConstraintPtr_t &self)
	Store shared pointer to itself. More...

Detailed Description

Implicit non-linear constraint.

This class defines a numerical constraints on a robot configuration of the form:

\begin{eqnarray*} f_1 (\mathbf{q}) & = \mbox{or} \leq & f_1^0 \\ & \vdots\\ f_m (\mathbf{q}) & = \mbox{or} \leq & f_m^0 \end{eqnarray*}

Functions \(f_i\) are differentiable functions. Vectors \(f_i^0\) are called right hand side.

The constraints are solved numerically by a Newton Raphson like method.

Numerical constraints can be added using method ConfigProjector::add. Default parameter of this method define equality constraints, but inequality constraints can also be defined by passing an object of type ComparisonType to method.

Member Enumeration Documentation

enum hpp::core::ConfigProjector::LineSearchType

Enumerator
Backtracking
ErrorNormBased
FixedSequence
Constant

Constructor & Destructor Documentation

virtual hpp::core::ConfigProjector::~ConfigProjector ( )

virtual

Destructor.

hpp::core::ConfigProjector::ConfigProjector	(	const DevicePtr_t &	robot,
		const std::string &	name,
		value_type	errorThreshold,
		size_type	maxIterations
	)

protected

Constructor.

Parameters

robot	robot the constraint applies to.
errorThreshold	norm of the value of the constraint under which the constraint is considered satified,
maxIterations	maximal number of iteration in the resolution of the constraint.

hpp::core::ConfigProjector::ConfigProjector ( const ConfigProjector & cp )

protected

Copy constructor.

Member Function Documentation

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

Add a numerical constraint.

Parameters

numericalConstraint	The numerical constraint.
passiveDofs	column indices of the jacobian vector that will be set to zero when solving.
priority	priority of the function. The last level might be optional.

Returns: false if numerical constraint had already been inserted.

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

void hpp::core::ConfigProjector::compressMatrix	(	matrixIn_t	normal,
		matrixOut_t	small,
		bool	rows = `true`
	)		const

Compress matrix.

Parameters

normal input matrix

Return values

small compressed matrix

Parameters

rows	whether to compress rows and colums or only columns

void hpp::core::ConfigProjector::compressVector	(	vectorIn_t	normal,
		vectorOut_t	small
	)		const

Compress Velocity vector by removing output of explicit constraints.

Parameters

normal input velocity vector

Return values

small compressed velocity vectors

void hpp::core::ConfigProjector::computeValueAndJacobian	(	ConfigurationIn_t	configuration,
		vectorOut_t	value,
		matrixOut_t	reducedJacobian
	)

Compute value and reduced jacobian at a given configuration.

Parameters

configuration input configuration

Return values

value	values of the differentiable functions stacked in a vector,
reducedJacobian	Reduced Jacobian of the differentiable functions stacked in a matrix. Reduced Jacobian is defined as the Jacobian to which columns corresponding to explicit constraints have been removed and to which columns corresponding to passive dofs are set to 0.

bool hpp::core::ConfigProjector::contains ( const constraints::ImplicitPtr_t & numericalConstraint ) const

Check that numerical constraint is in config projector.

Parameters

numericalConstraint numerical constraint

Returns: true if numerical constraint is already in config projector whatever the passive dofs are.

virtual ConstraintPtr_t hpp::core::ConfigProjector::copy ( ) const

virtual

return shared pointer to copy

Implements hpp::core::Constraint.

static ConfigProjectorPtr_t hpp::core::ConfigProjector::create	(	const DevicePtr_t &	robot,
		const std::string &	name,
		value_type	errorThreshold,
		size_type	maxIterations
	)

static

Return shared pointer to new object.

Parameters

robot	robot the constraint applies to.
errorThreshold	norm of the value of the constraint under which the constraint is considered satified,
maxIterations	maximal number of iteration in the resolution of the constraint.

static ConfigProjectorPtr_t hpp::core::ConfigProjector::createCopy ( const ConfigProjectorPtr_t cp )

static

Return shared pointer to copy.

Parameters

cp	shared pointer to object to copy

static void hpp::core::ConfigProjector::defaultLineSearch ( LineSearchType ls )

static

size_type hpp::core::ConfigProjector::dimension ( ) const

Get constraint dimension.

void hpp::core::ConfigProjector::errorThreshold ( const value_type & threshold )

Set error threshold.

value_type hpp::core::ConfigProjector::errorThreshold ( ) const

Get errorimal number of threshold in config projector.

virtual bool hpp::core::ConfigProjector::impl_compute ( ConfigurationOut_t configuration )

protectedvirtual

Numerically solve constraint.

Implements hpp::core::Constraint.

void hpp::core::ConfigProjector::init ( const ConfigProjectorPtr_t & self )

inlineprotected

Store weak pointer to itself.

References hpp::core::Constraint::init().

virtual bool hpp::core::ConfigProjector::isSatisfied ( ConfigurationIn_t config )

virtual

Check whether a configuration statisfies the constraint.

Implements hpp::core::Constraint.

virtual bool hpp::core::ConfigProjector::isSatisfied	(	ConfigurationIn_t	config,
		vector_t &	error
	)

virtual

Check whether a configuration statisfies the constraint.

Parameters

config the configuration to check

Return values

error concatenation of differences between value of numerical constraints and right hand side.

Implements hpp::core::Constraint.

void hpp::core::ConfigProjector::lastIsOptional ( bool optional )

bool hpp::core::ConfigProjector::lastIsOptional ( ) const

void hpp::core::ConfigProjector::maxIterations ( size_type iterations )

Set maximal number of iterations.

size_type hpp::core::ConfigProjector::maxIterations ( ) const

Get maximal number of iterations in config projector.

size_type hpp::core::ConfigProjector::numberFreeVariables ( ) const

Return the number of free variables.

size_type hpp::core::ConfigProjector::numberNonLockedDof ( ) const

Get number of non-locked degrees of freedom.

Deprecated:: Call numberFreeVariables instead

const NumericalConstraints_t& hpp::core::ConfigProjector::numericalConstraints ( ) const

Get the numerical constraints of the config-projector (and so of the Constraint Set)

bool hpp::core::ConfigProjector::oneStep	(	ConfigurationOut_t	config,
		vectorOut_t	dq,
		const value_type &	alpha
	)

Execute one iteration of the projection algorithm.

Returns: true if the constraints are satisfied

Deprecated:: use solver().oneStep is needed

bool hpp::core::ConfigProjector::optimize	(	ConfigurationOut_t	config,
		std::size_t	maxIter = `0`
	)

Optimize the configuration while respecting the constraints The input configuration must already satisfy the constraints.

Returns: true if the configuration was optimized.

Parameters

maxIter if 0, use maxIterations().

virtual void hpp::core::ConfigProjector::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::core::ConfigProjector::projectVectorOnKernel	(	ConfigurationIn_t	from,
		vectorIn_t	velocity,
		vectorOut_t	result
	)

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

value_type hpp::core::ConfigProjector::residualError ( ) const

void hpp::core::ConfigProjector::rightHandSide ( const vector_t & param )

Set the level set parameter.

Parameters

param the level set parameter.

void hpp::core::ConfigProjector::rightHandSide	(	const constraints::ImplicitPtr_t &	nm,
		vectorIn_t	rhs
	)

Same as rightHandSide(vectorIn_t) but only for the specified constraints::Implicit.

vector_t hpp::core::ConfigProjector::rightHandSide ( ) const

Get the level set parameter.

Returns: the parameter.

void hpp::core::ConfigProjector::rightHandSideAt ( const value_type & s )

Update the right hand side using Implicit::rightHandSideAt.

vector_t hpp::core::ConfigProjector::rightHandSideFromConfig ( ConfigurationIn_t config )

Set the right hand side from a configuration.

in such a way that the configuration satisfies the numerical constraints

Parameters

config the input configuration.

Returns: the right hand side

Warning: Only values of the right hand side corresponding to "equality constraints" are set. As a result, the input configuration may not satisfy the other constraints. The rationale is the following. Equality constraints define a foliation of the configuration space. Leaves of the foliation are defined by the value of the right hand side of the equality constraints. This method is mainly used in manipulation planning to retrieve the leaf a configuration lies on.

void hpp::core::ConfigProjector::rightHandSideFromConfig	(	const constraints::ImplicitPtr_t &	nm,
		ConfigurationIn_t	config
	)

Same as rightHandSideFromConfig(ConfigurationIn_t) but only for the specified constraints::Implicit.

const DevicePtr_t& hpp::core::ConfigProjector::robot ( ) const

inline

Get robot.

const value_type& hpp::core::ConfigProjector::sigma ( ) const

const BySubstitution& hpp::core::ConfigProjector::solver ( ) const

inline

::hpp::statistics::SuccessStatistics& hpp::core::ConfigProjector::statistics ( )

inline

Get the statistics.

void hpp::core::ConfigProjector::uncompressMatrix	(	matrixIn_t	small,
		matrixOut_t	normal,
		bool	rows = `true`
	)		const

Uncompress matrix.

Parameters

small input matrix

Return values

normal uncompressed matrix

Parameters

rows	whether to uncompress rows and colums or only columns

void hpp::core::ConfigProjector::uncompressVector	(	vectorIn_t	small,
		vectorOut_t	normal
	)		const

Expand compressed velocity vector.

Parameters

small compressed velocity vector without output of explicit constraints

Return values

normal uncompressed velocity vector.

Public Types

Public Member Functions

Static Public Member Functions

Protected Member Functions

Detailed Description

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation