#include <hpp/constraints/explicit/relative-pose.hh>

Inheritance diagram for hpp::constraints::explicit_::RelativePose:

Collaboration diagram for hpp::constraints::explicit_::RelativePose:

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

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

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

Public Member Functions inherited from hpp::constraints::Explicit
DifferentiableFunctionPtr_t	explicitFunction () const

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

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

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

virtual	~Implicit ()

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

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

const segments_t &	activeRows () const

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

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

void	setInactiveRowsToZero (vectorOut_t error) const

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

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

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 RelativePosePtr_t	create (const std::string &name, const DevicePtr_t &robot, const JointConstPtr_t &joint1, const JointConstPtr_t &joint2, const Transform3s &frame1, const Transform3s &frame2, ComparisonTypes_t comp, std::vector< bool > mask=std::vector< bool >())

static RelativePosePtr_t	createCopy (const RelativePosePtr_t &other)

Static Public Member Functions inherited from hpp::constraints::Explicit
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. More...

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

Protected Member Functions
	RelativePose (const std::string &name, const DevicePtr_t &robot, const JointConstPtr_t &joint1, const JointConstPtr_t &joint2, const Transform3s &frame1, const Transform3s &frame2, ComparisonTypes_t comp=ComparisonTypes_t(), std::vector< bool > mask=std::vector< bool >(6, true))

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

void	init (RelativePoseWkPtr_t weak)
	Store weak pointer to itself. More...

Protected Member Functions inherited from 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)

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

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

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

void	init (const ImplicitWkPtr_t &weak)

	Implicit ()

Additional Inherited Members
Protected Attributes inherited from hpp::constraints::Explicit
DifferentiableFunctionPtr_t	inputToOutput_

segments_t	inputConf_

segments_t	outputConf_

segments_t	inputVelocity_

segments_t	outputVelocity_

Detailed Description

Constraint of relative pose between two frames on a kinematic chain

Given an input configuration \(\mathbf{q}\), solving this constraint consists in computing output variables with respect to input variables:

\[ \mathbf{q}_{out} = g(\mathbf{q}_{in}) \]

where \(\mathbf{q}_{out}\) are the configuration variables of input joint2, \({q}_{in}\) are the configuration variables of input joint1 and parent joints, and \(g\) is a mapping with values is SE(3). Note that joint2 should be a freeflyer joint.

Note

According to the documentation of class Explicit, the implicit formulation should be

\[ f(\mathbf{q}) = \mathbf{q}_{out} - g(\mathbf{q}_{in}). \]

As function \(g\) takes values in SE(3), the above expression is equivalent to

\[ f(\mathbf{q}) = \log_{SE(3)}\left(g(\mathbf{q}_{in})^{-1} \mathbf{q}_{out}\right) \]

that represents the log of the error of input joint2 pose ( \(\mathbf{q}_{out}\)) with respect to its desired value ( \(g(\mathbf{q}_{in}\)). The problem with this expression is that it is different from the corresponding implicit formulation hpp::constraints::RelativeTransformationR3xSO3 that compares the poses of input joint1 and joint2. For manipulation planning applications where pairs of constraints and complements are replaced by an explicit constraint, this difference of formulation results in inconsistencies such as a configuration satisfying one formulation (the error being below the threshold) but not the other one. To cope with this issue, the default implicit formulation is replaced by the one defined by class hpp::constraints::RelativeTransformationR3xSO3.

Constructor & Destructor Documentation

◆ RelativePose() [1/2]

hpp::constraints::explicit_::RelativePose::RelativePose	(	const std::string &	name,
		const DevicePtr_t &	robot,
		const JointConstPtr_t &	joint1,
		const JointConstPtr_t &	joint2,
		const Transform3s &	frame1,
		const Transform3s &	frame2,
		ComparisonTypes_t	comp = `ComparisonTypes_t()`,
		std::vector< bool >	mask = `std::vector< bool >(6, true)`
	)

protected

Constructor

Parameters

name	the name of the constraints,
robot	the robot the constraints is applied to,
joint1	the first joint the transformation of which is constrained,
joint2	the second joint the transformation of which is constrained,
frame1	position of a fixed frame in joint 1,
frame2	position of a fixed frame in joint 2,
comp	vector of comparison types
mask	mask defining which components of the error are taken into account to determine whether the constraint is satisfied.

Note: if joint1 is 0x0, joint 1 frame is considered to be the global frame.

◆ RelativePose() [2/2]

hpp::constraints::explicit_::RelativePose::RelativePose ( const RelativePose & other )

protected

Copy constructor.

Member Function Documentation

◆ copy()

virtual ImplicitPtr_t hpp::constraints::explicit_::RelativePose::copy ( ) const

virtual

Copy object and return shared pointer to copy.

Reimplemented from hpp::constraints::Explicit.

◆ create()

static RelativePosePtr_t hpp::constraints::explicit_::RelativePose::create	(	const std::string &	name,
		const DevicePtr_t &	robot,
		const JointConstPtr_t &	joint1,
		const JointConstPtr_t &	joint2,
		const Transform3s &	frame1,
		const Transform3s &	frame2,
		ComparisonTypes_t	comp,
		std::vector< bool >	mask = `std::vector< bool >()`
	)

static

Create instance and return shared pointer

Parameters

name	the name of the constraints,
robot	the robot the constraints is applied to,
joint1	the first joint the transformation of which is constrained,
joint2	the second joint the transformation of which is constrained,
frame1	position of a fixed frame in joint 1,
frame2	position of a fixed frame in joint 2,
comp	vector of comparison types
mask	mask defining which components of the error are taken into account to determine whether the constraint is satisfied.

Note: if joint1 is 0x0, joint 1 frame is considered to be the global frame.

◆ createCopy()

static RelativePosePtr_t hpp::constraints::explicit_::RelativePose::createCopy ( const RelativePosePtr_t & other )

static

◆ init()

void hpp::constraints::explicit_::RelativePose::init ( RelativePoseWkPtr_t weak )

protected

Store weak pointer to itself.

◆ jacobianOutputValue()

virtual void hpp::constraints::explicit_::RelativePose::jacobianOutputValue	(	vectorIn_t	qin,
		LiegroupElementConstRef	f_value,
		LiegroupElementConstRef	rhs,
		matrixOut_t	jacobian
	)		const

virtual

Compute Jacobian of output value

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

Parameters

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

Reimplemented from hpp::constraints::Explicit.

◆ outputValue()

virtual void hpp::constraints::explicit_::RelativePose::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

\begin{equation} f \left(\mathbf{q}_{in}\right) + rhs_{expl} \end{equation}

where \(rhs_{expl}\) is the explicit right hand side converted using the following expression:

\begin{equation} rhs_{expl} = \log_{SE(3)}\left( F_{2/J_2} rhs_{impl} F_{2/J_2}^{-1} \right) \end{equation}

Reimplemented from hpp::constraints::Explicit.

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

include/hpp/constraints/explicit/relative-pose.hh

Public Member Functions

Static Public Member Functions

Protected Member Functions

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ RelativePose() [1/2]

◆ RelativePose() [2/2]

Member Function Documentation

◆ copy()

◆ create()

◆ createCopy()

◆ init()

◆ jacobianOutputValue()

◆ outputValue()