#include <hpp/constraints/explicit.hh>

Inheritance diagram for hpp::constraints::Explicit:

[legend]

Collaboration diagram for hpp::constraints::Explicit:

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

DifferentiableFunctionPtr_t	explicitFunction () const

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

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

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

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

virtual void	outputValue (LiegroupElementRef result, vectorIn_t qin, LiegroupElementConstRef rhs) const

virtual void	jacobianOutputValue (vectorIn_t qin, LiegroupElementConstRef g_value, LiegroupElementConstRef rhs, matrixOut_t jacobian) const

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

Implicit &	operator= (const Implicit &other)
	Operator assignation.

virtual	~Implicit ()

const ComparisonTypes_t &	comparisonType () const
	Return the ComparisonType.

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

const segments_t &	activeRows () const

bool	checkAllRowsActive () const
	Check if all rows are active (no inactive rows)

const Eigen::RowBlockIndices &	equalityIndices () const
	Get indices of constraint coordinates that are equality.

void	setInactiveRowsToZero (vectorOut_t error) const

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

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

virtual std::pair< JointConstPtr_t, JointConstPtr_t >	doesConstrainRelPoseBetween (DeviceConstPtr_t robot) const

void	rightHandSideFromConfig (ConfigurationIn_t config, LiegroupElementRef rhs)

bool	checkRightHandSide (LiegroupElementConstRef rhs) const

size_type	parameterSize () const

size_type	rightHandSideSize () const

void	rightHandSideFunction (const DifferentiableFunctionPtr_t &rhsF)

const DifferentiableFunctionPtr_t &	rightHandSideFunction () const

vectorIn_t	rightHandSideAt (const value_type &s)

Static Public Member Functions
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(), std::vector< bool > mask=std::vector< bool >())

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

Static Public Member Functions inherited from hpp::constraints::Implicit
static ImplicitPtr_t	create (const DifferentiableFunctionPtr_t &func, ComparisonTypes_t comp, std::vector< bool > mask=std::vector< bool >())

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

Protected Member Functions

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, std::vector< bool > mask)

Explicit (const DifferentiableFunctionPtr_t &implicitFunction, const DifferentiableFunctionPtr_t &function, const segments_t &inputConf, const segments_t &outputConf, const segments_t &inputVelocity, const segments_t &outputVelocity, const ComparisonTypes_t &comp, std::vector< bool > mask)

(const LiegroupSpacePtr_t&, constDifferentiableFunctionPtr_t&, const segments_t& inputConf, constsegments_t& outputConf, const segments_t& inputVelocity, constsegments_t&, const ComparisonTypes_t&);

Parameters

implicitFunction

Explicit (const Explicit &other)

Copy constructor.

bool isEqual (const Implicit &other, bool swapAndTest) const

void init (const ExplicitWkPtr_t &weak)

Explicit ()

Protected Member Functions inherited from hpp::constraints::Implicit

Implicit (const DifferentiableFunctionPtr_t &function, ComparisonTypes_t comp, std::vector< bool > mask)

Implicit (const Implicit &other)

Copy constructor.

void init (const ImplicitWkPtr_t &weak)

Implicit ()

Protected Attributes
DifferentiableFunctionPtr_t	inputToOutput_

segments_t	inputConf_

segments_t	outputConf_

segments_t	inputVelocity_

segments_t	outputVelocity_

Detailed Description

Explicit numerical constraint

Definition

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{eqnarray*} \mathbf{q}_{out} - g \left(\mathbf{q}_{in}\right) \ \ &\mbox{with}& \mathbf{q}_{out} = \left(q_{oc_{1}} \cdots q_{oc_{n_{oc}}}\right)^T, \ \ \ \mathbf{q}_{in} = (q_{ic_{1}} \cdots q_{ic_{n_{ic}}})^T \end{eqnarray*}

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

\begin{align*} \mathbf{q}_{out} &- g \left(\mathbf{q}_{in}\right) = rhs \\ & \mbox {if and only if}\\ \mathbf{q}_{out} &= g \left(\mathbf{q}_{in}\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 an 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 g}{q_1} & & & & -\frac{\partial g}{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_{g}^T R_{out}) & -J_{log}(R_{g}^T R_{out})R_{out}^T R_{g} \frac{\partial g}{\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_{g}\) 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*}

"Domain of

definition"

Some explicit constraints might be defined over only a subspace of the input

space. If the input value is not in the definition subspace, the explicit constraint will throw an exception of type FunctionNotDefinedForThisValue.

Constructor & Destructor Documentation

◆ Explicit() [1/4]

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,
		std::vector< bool >	mask
	)

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}})\).
mask	mask defining which components of the error are taken into account to determine whether the constraint is satisfied (See parent class for details).

◆ Explicit() [2/4]

hpp::constraints::Explicit::Explicit	(	const DifferentiableFunctionPtr_t &	implicitFunction,
		const DifferentiableFunctionPtr_t &	function,
		const segments_t &	inputConf,
		const segments_t &	outputConf,
		const segments_t &	inputVelocity,
		const segments_t &	outputVelocity,
		const ComparisonTypes_t &	comp,
		std::vector< bool >	mask
	)

protected

(const LiegroupSpacePtr_t&, constDifferentiableFunctionPtr_t&, const segments_t& inputConf, constsegments_t& outputConf, const segments_t& inputVelocity, constsegments_t&, const ComparisonTypes_t&);

Parameters

implicitFunction

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}})\).
mask	mask defining which components of the error are taken into account to determine whether the constraint is satisfied (See parent class for details). (const LiegroupSpacePtr_t&, constDifferentiableFunctionPtr_t&, const segments_t& inputConf, constsegments_t& outputConf, const segments_t& inputVelocity, constsegments_t&, const ComparisonTypes_t&);
implicitFunction	differentiable function of the implicit formulation if different from default expression \begin{equation} \mathbf{q}_{out} - g \left(\mathbf{q}_{in}\right), \end{equation}

◆ Explicit() [3/4]

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

protected

Copy constructor.

◆ Explicit() [4/4]

hpp::constraints::Explicit::Explicit ( )

inlineprotected

Member Function Documentation

◆ copy()

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::explicit_::ConvexShapeContact, hpp::constraints::explicit_::RelativePose, and hpp::constraints::LockedJoint.

◆ create()

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()`,
		std::vector< bool >	mask = `std::vector< bool >()`
	)

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}})\).
mask	mask defining which components of the error are taken into account to determine whether the constraint is satisfied (See parent class for details).

◆ createCopy()

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

static

Create a copy and return shared pointer.

◆ explicitFunction()

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

inline

Function that maps input to output

Returns: function \(f\).

◆ init()

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

protected

◆ inputConf()

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

inline

Get input configuration variables.

◆ inputVelocity()

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

inline

Get input degrees of freedom.

◆ isEqual()

bool hpp::constraints::Explicit::isEqual	(	const Implicit &	other,
		bool	swapAndTest
	)		const

protectedvirtual

Test equality with other instance

Parameters

other	object to copy
swapAndTest	whether we should also check other == this

Reimplemented from hpp::constraints::Implicit.

Reimplemented in hpp::constraints::LockedJoint.

◆ jacobianOutputValue()

virtual void hpp::constraints::Explicit::jacobianOutputValue	(	vectorIn_t	qin,
		LiegroupElementConstRef	g_value,
		LiegroupElementConstRef	rhs,
		matrixOut_t	jacobian
	)		const

virtual

Compute Jacobian of output value

\begin{eqnarray*} J &=& \frac{\partial}{\partial\mathbf{q}_{in}}\left(g(\mathbf{q}_{in}) + rhs\right). \end{eqnarray*}

Parameters

qin	vector of input variables,
g_value	\(f(\mathbf{q}_{in})\) provided to avoid recomputation,
rhs	right hand side (of implicit formulation).

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

◆ outputConf()

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

inline

Get output configuration variables.

◆ outputValue()

virtual void hpp::constraints::Explicit::outputValue	(	LiegroupElementRef	result,
		vectorIn_t	qin,
		LiegroupElementConstRef	rhs
	)		const

virtual

Compute the value of the output configuration variables

Parameters

qin	input configuration variables,
rhs	right hand side of constraint

The default implementation computes

\begin{equation} g \left((q_{ic_{1}} \cdots q_{ic_{n_{ic}}})^T\right) + rhs \end{equation}

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

◆ outputVelocity()

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

inline

Get output degrees of freedom.

Member Data Documentation

◆ inputConf_

segments_t hpp::constraints::Explicit::inputConf_

protected

◆ inputToOutput_

DifferentiableFunctionPtr_t hpp::constraints::Explicit::inputToOutput_

protected

◆ inputVelocity_

segments_t hpp::constraints::Explicit::inputVelocity_

protected

◆ outputConf_

segments_t hpp::constraints::Explicit::outputConf_

protected

◆ outputVelocity_

segments_t hpp::constraints::Explicit::outputVelocity_

protected

The documentation for this class was generated from the following file:

include/hpp/constraints/explicit.hh

Public Member Functions

Static Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Definition

"Domain of

Constructor & Destructor Documentation

◆ Explicit() [1/4]

◆ Explicit() [2/4]

◆ Explicit() [3/4]

◆ Explicit() [4/4]

Member Function Documentation

◆ copy()

◆ create()

◆ createCopy()

◆ explicitFunction()

◆ init()

◆ inputConf()

◆ inputVelocity()

◆ isEqual()

◆ jacobianOutputValue()

◆ outputConf()

◆ outputValue()

◆ outputVelocity()

Member Data Documentation

◆ inputConf_

◆ inputToOutput_

◆ inputVelocity_

◆ outputConf_

◆ outputVelocity_