Explicit numerical constraint. More...

#include <hpp/constraints/explicit.hh>

Inheritance diagram for hpp::constraints::Explicit:

Collaboration diagram for hpp::constraints::Explicit:

Public Member Functions
virtual ImplicitPtr_t	copy () const
	Copy object and return shared pointer to copy. More...

virtual DifferentiableFunctionPtr_t	explicitFunction () const
	Function that maps input to output. More...

const segments_t &	outputConf () const
	Get output configuration variables. More...

const segments_t &	outputVelocity () const
	Get output degrees of freedom. More...

const segments_t &	inputConf () const
	Get input configuration variables. More...

const segments_t &	inputVelocity () const
	Get input degrees of freedom. More...

virtual void	implicitToExplicitRhs (vectorIn_t implicitRhs, vectorOut_t explicitRhs)
	Convert right hand side. More...

Public Member Functions inherited from hpp::constraints::Implicit
bool	operator== (const Implicit &other) const
	Operator equality. More...

virtual	~Implicit ()

DifferentiableFunction &	function () const
	Return a reference to function \(h\). More...

const DifferentiableFunctionPtr_t &	functionPtr () const
	Return a reference to function \(h\). More...

void	rightHandSideFromConfig (ConfigurationIn_t config)
	Set the right hand side from a configuration. More...

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

vectorIn_t	rightHandSide () const
	Return the right hand side of the equation. More...

size_type	rhsSize () const HPP_CONSTRAINTS_DEPRECATED
	Return the dimension of the left hand side function tangent output space. More...

size_type	parameterSize () const
	Get size of parameter defining the right hand side of the constraint. More...

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

const ComparisonTypes_t &	comparisonType () const
	Return the ComparisonType. More...

void	comparisonType (const ComparisonTypes_t &comp)
	Set the comparison type. More...

bool	constantRightHandSide () const HPP_CONSTRAINTS_DEPRECATED
	Whether right hand side is constant. More...

vectorOut_t	rightHandSide ()
	Return a modifiable reference to right hand side of equation. More...

vectorOut_t	nonConstRightHandSide () HPP_CONSTRAINTS_DEPRECATED
	Return a modifiable reference to right hand side of equation. More...

void	rightHandSideFunction (const DifferentiableFunctionPtr_t &rhsF)
	Set time-varying right hand side. More...

const DifferentiableFunctionPtr_t &	rightHandSideFunction () const
	Get time-varying right hand side. More...

vectorIn_t	rightHandSideAt (const value_type &s)
	Evaluate and set right hand side at given time. More...

Static Public Member Functions
static ExplicitPtr_t	create (const DevicePtr_t &robot, const DifferentiableFunctionPtr_t &function, const segments_t &inputConf, const segments_t &outputConf, const segments_t &inputVelocity, const segments_t &outputVelocity, const ComparisonTypes_t &comp=ComparisonTypes_t()) HPP_CONSTRAINTS_DEPRECATED
	Create instance and return shared pointer. More...

static ExplicitPtr_t	create (const LiegroupSpacePtr_t &configSpace, const DifferentiableFunctionPtr_t &function, const segments_t &inputConf, const segments_t &outputConf, const segments_t &inputVelocity, const segments_t &outputVelocity, const ComparisonTypes_t &comp=ComparisonTypes_t())
	Create instance and return shared pointer. More...

static ExplicitPtr_t	createCopy (const ExplicitPtr_t &other)
	Create a copy and return shared pointer. More...

Static Public Member Functions inherited from hpp::constraints::Implicit
static ImplicitPtr_t	create (const DifferentiableFunctionPtr_t &function)
	Create a shared pointer to a new instance. More...

static ImplicitPtr_t	create (const DifferentiableFunctionPtr_t &function, ComparisonTypes_t comp)
	Create a shared pointer to a new instance. More...

static ImplicitPtr_t	create (const DifferentiableFunctionPtr_t &function, ComparisonTypes_t comp, vectorIn_t rhs)
	Create a shared pointer to a new instance. More...

static ImplicitPtr_t	createCopy (const ImplicitPtr_t &other)
	Create a copy and return shared pointer. More...

Protected Member Functions
	Explicit (const DevicePtr_t &robot, const DifferentiableFunctionPtr_t &function, const segments_t &inputConf, const segments_t &outputConf, const segments_t &inputVelocity, const segments_t &outputVelocity, const ComparisonTypes_t &comp) HPP_CONSTRAINTS_DEPRECATED
	Constructor. More...

	Explicit (const LiegroupSpacePtr_t &configSpace, const DifferentiableFunctionPtr_t &function, const segments_t &inputConf, const segments_t &outputConf, const segments_t &inputVelocity, const segments_t &outputVelocity, const ComparisonTypes_t &comp)
	Constructor. More...

	Explicit (const Explicit &other)
	Copy constructor. More...

void	init (const ExplicitWkPtr_t &weak)

Protected Member Functions inherited from hpp::constraints::Implicit
	Implicit (const DifferentiableFunctionPtr_t &function, ComparisonTypes_t comp)
	Constructor. More...

	Implicit (const DifferentiableFunctionPtr_t &function, ComparisonTypes_t comp, vectorIn_t rhs)
	Constructor. More...

	Implicit (const Implicit &other)
	Copy constructor. More...

virtual bool	isEqual (const Implicit &other, bool swapAndTest) const
	Test equality with other instance. More...

void	init (const ImplicitWkPtr_t &weak)

Protected Attributes
DifferentiableFunctionPtr_t	inputToOutput_

segments_t	inputConf_

segments_t	outputConf_

segments_t	inputVelocity_

segments_t	outputVelocity_

Detailed Description

Explicit numerical constraint.

An explicit numerical constraint is a constraint such that some configuration variables called output are function of the others called input.

Let

\((ic_{1}, \cdots, ic_{n_{ic}})\) be the list of indices corresponding to ordered input configuration variables,
\((oc_{1}, \cdots, oc_{n_{oc}})\) be the list of indices corresponding to ordered output configuration variables,
\((iv_{1}, \cdots, iv_{n_{iv}})\) be the list of indices corresponding to ordered input degrees of freedom,
\((ov_{1}, \cdots, ov_{n_{ov}})\) be the list of indices corresponding to ordered output degrees of freedom.

Recall that degrees of freedom refer to velocity vectors.

Let us notice that \(n_{ic} + n_{oc}\) is less than the robot configuration size, and \(n_{iv} + n_{ov}\) is less than the velocity size. Some degrees of freedom may indeed be neither input nor output.

Then the differential function is of the form

\begin{equation*} \left(\begin{array}{c} q_{oc_{1}} \\ \vdots \\ q_{oc_{n_{oc}}} \end{array}\right) - f \left((q_{ic_{1}} \cdots q_{ic_{n_{ic}}})^T\right) \end{equation*}

It is straightforward that an equality constraint with this function can solved explicitely:

\begin{align*} \left(\begin{array}{c} q_{oc_{1}} \\ \vdots \\ q_{oc_{n_{oc}}} \end{array}\right) &- f \left((q_{ic_{1}} \cdots q_{ic_{n_{ic}}})^T\right) = rhs \\ & \mbox {if and only if}\\ \left(\begin{array}{c} q_{oc_{1}} \\ \vdots \\ q_{oc_{n_{oc}}} \end{array}\right) &= f \left((q_{ic_{1}} \cdots q_{ic_{n_{ic}}})^T\right) + rhs \\ \end{align*}

If function \(f\) takes values in a Lie group (SO(2), SO(3)), the above "+" between a Lie group element and a tangent vector has to be undestood as the integration of the constant velocity from the Lie group element:

\begin{equation*} \mathbf{q} + \mathbf{v} = \mathbf{q}.\exp (\mathbf{v}) \end{equation*}

where \(\mathbf{q}\) is a Lie group element and \(\mathbf{v}\) is a tangent vector.

Considered as a Implicit instance, the expression of the Jacobian of the DifferentiableFunction above depends on the output space of function \(f\). The rows corresponding to values in a vector space are expressed as follows.

for any index \(i\) between 0 and the size of velocity vectors, either

\(\dot{q}_i\) is an input degree of freedom: \(\exists j\) integer, \(1 \leq j \leq n_{iv}\) such that \(i=iv_{j}\),
\(\dot{q}_i\) is an output degree of freedom: \(\exists j\) integer, \(1\leq j \leq n_{ov}\) such that \(i=ov_{j}\), or
\(\dot{q}_i\) neither input nor output. In this case, the corresponding column is equal to 0.
\begin{equation*} J = \left(\begin{array}{cccccccccccc} \cdots & ov_1 & \cdots & iv_{1} & \cdots & ov_2 & \cdots & iv_2 & \cdots & ov_{n_{ov}} & \cdots \\ & 1 & & & & 0 & & & & & \\ & 0 & & & & 1 & & & & & \\ & & & -\frac{\partial f}{q_1} & & & & -\frac{\partial f}{q_2} \\ &&&&&\\ & 0 & & & & 0 & & & & 1 \end{array}\right) \end{equation*}
The rows corresponding to values in SO(3) have the following expression.
\begin{equation*} J = \left(\begin{array}{cccccccccccc} ov_1 \ ov_2 \ ov_3 & iv_1 \cdots iv_{n_{iv}} \\ J_{log}(R_{f}^T R_{out}) & -J_{log}(R_{f}^T R_{out})R_{out}^T R_{f} \frac{\partial f}{\partial q_{in}} \end{array}\right) \end{equation*}
where
\(R_{out}\) is the rotation matrix corresponding to unit quaternion \((q_{oc1},q_{oc2},q_{oc3},q_{oc4})\),
\(R_{f}\) is the rotation matrix corresponding to the part of the output value of \(f\) corresponding to SO(3),
\(J_{log}\) is the Jacobian matrix of function that associates to a rotation matrix \(R\) the vector \(\omega\) such that
\begin{equation*} R = \exp (\left[\omega\right]_{\times}) \end{equation*}

Constructor & Destructor Documentation

hpp::constraints::Explicit::Explicit	(	const DevicePtr_t &	robot,
		const DifferentiableFunctionPtr_t &	function,
		const segments_t &	inputConf,
		const segments_t &	outputConf,
		const segments_t &	inputVelocity,
		const segments_t &	outputVelocity,
		const ComparisonTypes_t &	comp
	)

protected

Constructor.

Parameters

robot	Robot for which the constraint is defined.
function	relation between input configuration variables and output configuration variables,
inputConf	set of integer intervals defining indices \((ic_{1}, \cdots, ic_{n_{ic}})\),
outputConf	set of integer intervals defining indices \((oc_{1}, \cdots, oc_{n_{oc}})\),
inputVelocity	set of integer defining indices \((iv_{1}, \cdots, iv_{n_{iv}})\).
outputVelocity	set of integer defining indices \((ov_{1}, \cdots, ov_{n_{ov}})\).

Note: comparison type for this constraint is always equality

Deprecated:: Use constructor that takes LiegroupSpacePtr_t instead of DevicePtr_t as input and used robot->configSpace () as argument.

hpp::constraints::Explicit::Explicit	(	const LiegroupSpacePtr_t &	configSpace,
		const DifferentiableFunctionPtr_t &	function,
		const segments_t &	inputConf,
		const segments_t &	outputConf,
		const segments_t &	inputVelocity,
		const segments_t &	outputVelocity,
		const ComparisonTypes_t &	comp
	)

protected

Constructor.

Parameters

configSpace	Configuration space on which the constraint is defined,
function	relation between input configuration variables and output configuration variables,
inputConf	set of integer intervals defining indices \((ic_{1}, \cdots, ic_{n_{ic}})\),
outputConf	set of integer intervals defining indices \((oc_{1}, \cdots, oc_{n_{oc}})\),
inputVelocity	set of integer defining indices \((iv_{1}, \cdots, iv_{n_{iv}})\).
outputVelocity	set of integer defining indices \((ov_{1}, \cdots, ov_{n_{ov}})\).

Note: comparison type for this constraint is always equality

hpp::constraints::Explicit::Explicit ( const Explicit & other )

protected

Copy constructor.

Member Function Documentation

virtual ImplicitPtr_t hpp::constraints::Explicit::copy ( ) const

virtual

Copy object and return shared pointer to copy.

Reimplemented from hpp::constraints::Implicit.

Reimplemented in hpp::constraints::LockedJoint, and hpp::constraints::explicit_::RelativePose.

static ExplicitPtr_t hpp::constraints::Explicit::create	(	const DevicePtr_t &	robot,
		const DifferentiableFunctionPtr_t &	function,
		const segments_t &	inputConf,
		const segments_t &	outputConf,
		const segments_t &	inputVelocity,
		const segments_t &	outputVelocity,
		const ComparisonTypes_t &	comp = `ComparisonTypes_t()`
	)

static

Create instance and return shared pointer.

Parameters

robot	Robot for which the constraint is defined.
function	relation between input configuration variables and output configuration variables,
inputConf	set of integer intervals defining indices \((ic_{1}, \cdots, ic_{n_{ic}})\),
outputConf	set of integer intervals defining indices \((oc_{1}, \cdots, oc_{n_{oc}})\),
inputVelocity	set of integer defining indices \((iv_{1}, \cdots, iv_{n_{iv}})\).
outputVelocity	set of integer defining indices \((ov_{1}, \cdots, ov_{n_{ov}})\).

Note: comparison type for this constraint is always equality

Deprecated:: Call method that takes LiegroupSpacePtr_t instead of DevicePtr_t as input and used robot->configSpace () as argument.

static ExplicitPtr_t hpp::constraints::Explicit::create	(	const LiegroupSpacePtr_t &	configSpace,
		const DifferentiableFunctionPtr_t &	function,
		const segments_t &	inputConf,
		const segments_t &	outputConf,
		const segments_t &	inputVelocity,
		const segments_t &	outputVelocity,
		const ComparisonTypes_t &	comp = `ComparisonTypes_t()`
	)

static

Create instance and return shared pointer.

Parameters

configSpace	Configuration space on which the constraint is defined,
function	relation between input configuration variables and output configuration variables,
inputConf	set of integer intervals defining indices \((ic_{1}, \cdots, ic_{n_{ic}})\),
outputConf	set of integer intervals defining indices \((oc_{1}, \cdots, oc_{n_{oc}})\),
inputVelocity	set of integer defining indices \((iv_{1}, \cdots, iv_{n_{iv}})\).
outputVelocity	set of integer defining indices \((ov_{1}, \cdots, ov_{n_{ov}})\).

Note: comparison type for this constraint is always equality

static ExplicitPtr_t hpp::constraints::Explicit::createCopy ( const ExplicitPtr_t & other )

static

Create a copy and return shared pointer.

virtual DifferentiableFunctionPtr_t hpp::constraints::Explicit::explicitFunction ( ) const

inlinevirtual

Function that maps input to output.

Returns: function \(f\).

virtual void hpp::constraints::Explicit::implicitToExplicitRhs	(	vectorIn_t	implicitRhs,
		vectorOut_t	explicitRhs
	)

inlinevirtual

Convert right hand side.

Parameters

implicitRhs right hand side of implicit formulation,

Return values

explicitRhs right hand side of explicit formulation.

When implicit formulation is different from explicit formulation,

See also: hpp::constraints::explicit_::RelativePose, right hand side are also different. This method converts right hand side from implicit to explicit formulation.

When implicit formulation derives from explicit one, this method copies the first argument to the second one.

Reimplemented in hpp::constraints::explicit_::RelativePose.

void hpp::constraints::Explicit::init ( const ExplicitWkPtr_t & weak )

protected

const segments_t& hpp::constraints::Explicit::inputConf ( ) const

inline

Get input configuration variables.

const segments_t& hpp::constraints::Explicit::inputVelocity ( ) const

inline

Get input degrees of freedom.

const segments_t& hpp::constraints::Explicit::outputConf ( ) const

inline

Get output configuration variables.

const segments_t& hpp::constraints::Explicit::outputVelocity ( ) const

inline

Get output degrees of freedom.

Member Data Documentation

segments_t hpp::constraints::Explicit::inputConf_

protected

DifferentiableFunctionPtr_t hpp::constraints::Explicit::inputToOutput_

protected

segments_t hpp::constraints::Explicit::inputVelocity_

protected

segments_t hpp::constraints::Explicit::outputConf_

protected

segments_t hpp::constraints::Explicit::outputVelocity_

protected

Public Member Functions

Static Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation