Feature that controls the relative (or absolute) pose between two frames A (or world) and B. More...

#include <sot/core/feature-pose.hh>

Inheritance diagram for dynamicgraph::sot::FeaturePose< representation >:

Public Member Functions
virtual const std::string &	getClassName (void) const
	Returns the name class. More...

	FeaturePose (const std::string &name)

virtual	~FeaturePose (void)

virtual unsigned int &	getDimension (unsigned int &dim, int time)
	Verbose method. More...

virtual dg::Vector &	computeError (dg::Vector &res, int time)
	Computes \( {}^oM^{-1}_{fa} {}^oM_{fb} \ominus {}^{fa}M^*_{fb} \). More...

virtual dg::Vector &	computeErrorDot (dg::Vector &res, int time)
	Computes \( \frac{\partial\ominus}{\partial b} X {}^{fa}\nu^*_{fafb} \). More...

virtual dg::Matrix &	computeJacobian (dg::Matrix &res, int time)
	Computes \( \frac{\partial\ominus}{\partial b} Y \left( {{}^{fb}X_{jb}} {}^{jb}J_{jb} - {{}^{fb}X_{ja}} {}^{ja}J_{ja} \right) \). More...

virtual void	display (std::ostream &os) const

void	servoCurrentPosition (const int &time)

Dealing with the reference value to be reach with this feature.
	DECLARE_NO_REFERENCE ()

Public Member Functions inherited from dynamicgraph::sot::FeatureAbstract
void	featureRegistration (void)
	Register the feature in the stack of tasks. More...

void	initCommands (void)

	FeatureAbstract (const std::string &name)
	Default constructor: the name of the class should be given. More...

virtual	~FeatureAbstract (void)
	Default destructor. More...

unsigned int	getDimension (int time)
	Short method. More...

unsigned int	getDimension (void) const
	Shortest method. More...

virtual void	setReference (FeatureAbstract *sdes)=0

virtual void	unsetReference (void)

virtual const FeatureAbstract *	getReferenceAbstract (void) const =0

virtual FeatureAbstract *	getReferenceAbstract (void)=0

virtual bool	isReferenceSet (void) const

virtual void	addDependenciesFromReference (void)=0

virtual void	removeDependenciesFromReference (void)=0

void	setReferenceByName (const std::string &name)

std::string	getReferenceByName (void) const

virtual std::ostream &	writeGraph (std::ostream &os) const
	This method write a graph description on the file named FileName. More...

virtual SignalTimeDependent< dg::Vector, int > &	getErrorDot ()

Static Public Member Functions
static Flags	selectX (void)
	Static Feature selection. More...

static Flags	selectY (void)

static Flags	selectZ (void)

static Flags	selectRX (void)

static Flags	selectRY (void)

static Flags	selectRZ (void)

static Flags	selectTranslation (void)

static Flags	selectRotation (void)

Public Attributes
Input signals
dg::SignalPtr< MatrixHomogeneous, int >	oMja
	Input pose of Joint A wrt to world frame. More...

dg::SignalPtr< MatrixHomogeneous, int >	jaMfa
	Input pose of Frame A wrt to Joint A. More...

dg::SignalPtr< MatrixHomogeneous, int >	oMjb
	Input pose of Joint B wrt to world frame. More...

dg::SignalPtr< MatrixHomogeneous, int >	jbMfb
	Input pose of Frame B wrt to Joint B. More...

dg::SignalPtr< Matrix, int >	jaJja
	Jacobian of the input Joint A, expressed in Joint A More...

dg::SignalPtr< Matrix, int >	jbJjb
	Jacobian of the input Joint B, expressed in Joint B More...

dg::SignalPtr< MatrixHomogeneous, int >	faMfbDes
	The desired pose of Frame B wrt to Frame A. More...

dg::SignalPtr< Vector, int >	faNufafbDes
	The desired velocity of Frame B wrt to Frame A. More...

Output signals
SignalTimeDependent< MatrixHomogeneous, int >	faMfb
	Pose of Frame B wrt to Frame A. More...

SignalTimeDependent< Vector7, int >	q_faMfb
	Pose of Frame B wrt to Frame A. More...

SignalTimeDependent< Vector7, int >	q_faMfbDes
	Desired pose of Frame B wrt to Frame A. More...

Public Attributes inherited from dynamicgraph::sot::FeatureAbstract
SignalPtr< Flags, int >	selectionSIN
	This vector specifies which dimension are used to perform the computation. For instance let us assume that the feature is a 3D point. If only the Y-axis should be used for computing error, activation and Jacobian, then the vector to specify is \( [ 0 1 0] \). More...

SignalPtr< dg::Vector, int >	errordotSIN
	Derivative of the reference value. More...

SignalTimeDependent< dg::Vector, int >	errorSOUT
	This signal returns the error between the desired value and the current value : \( E(t) = {\bf s}(t) - {\bf s}^*(t)\). More...

SignalTimeDependent< dg::Vector, int >	errordotSOUT
	Derivative of the error with respect to time: \( \frac{\partial e}{\partial t} = - \frac{d{\bf s}^*}{dt} \). More...

SignalTimeDependent< dg::Matrix, int >	jacobianSOUT
	Jacobian of the error wrt the robot state: \( J = \frac{\partial {\bf s}}{\partial {\bf q}}\). More...

SignalTimeDependent< unsigned int, int >	dimensionSOUT
	Returns the dimension of the feature as an output signal. More...

Static Public Attributes
static const std::string	CLASS_NAME

Static Public Attributes inherited from dynamicgraph::sot::FeatureAbstract
static const std::string	CLASS_NAME
	Store the name of the class. More...

Detailed Description

template<Representation_t representation = R3xSO3Representation>
class dynamicgraph::sot::FeaturePose< representation >

Feature that controls the relative (or absolute) pose between two frames A (or world) and B.

Template Parameters

representation

specify the difference operation to use. This changes

the descent direction,
the meaning of the mask. With R3xSO3Representation, the mask is relative to Frame A. If this feature is alone in a SOT, the relative motion of Frame B wrt Frame A will be a line. This is what most people want.

Notations:

The frames are refered to with fa and fb.
Each frame is attached to a joint, which are refered to with ja and jb.
the difference operator is defined differently depending on the representation:
- R3xSO3Representation:
  \[ \begin{array}{ccccc} \ominus & : & (R^3\times SO(3))^2 & \to & R^3 \times \mathfrak{so}(3) \\ & & (a = (t_a,R_a),b = (t_b,R_b)) & \mapsto & b \ominus a = (t_b - t_a, \log(R_a^{-1} R_b) \\ \end{array} \]
- SE3Representation:
  \[ \begin{array}{ccccc} \ominus & : & SE(3)^2 & \to & \mathfrak{se}(3) \\ & & a, b & \mapsto & b \ominus a = \log(a^{-1} b) \\ \end{array} \]

Constructor & Destructor Documentation

◆ FeaturePose()

template<Representation_t representation = R3xSO3Representation>

dynamicgraph::sot::FeaturePose< representation >::FeaturePose ( const std::string & name )

◆ ~FeaturePose()

template<Representation_t representation = R3xSO3Representation>

virtual dynamicgraph::sot::FeaturePose< representation >::~FeaturePose ( void )

inlinevirtual

Member Function Documentation

◆ computeError()

template<Representation_t representation = R3xSO3Representation>

virtual dg::Vector& dynamicgraph::sot::FeaturePose< representation >::computeError	(	dg::Vector &	res,
		int	time
	)

virtual

Computes \( {}^oM^{-1}_{fa} {}^oM_{fb} \ominus {}^{fa}M^*_{fb} \).

Implements dynamicgraph::sot::FeatureAbstract.

◆ computeErrorDot()

template<Representation_t representation = R3xSO3Representation>

virtual dg::Vector& dynamicgraph::sot::FeaturePose< representation >::computeErrorDot	(	dg::Vector &	res,
		int	time
	)

virtual

Computes \( \frac{\partial\ominus}{\partial b} X {}^{fa}\nu^*_{fafb} \).

There are two different cases, depending on the representation:

R3xSO3Representation: \( X = \left( \begin{array}{cc} I_3 & [ {}^{fa}t_{fb} ] \\ 0_3 & {{}^{fa}R^*_{fb}}^T \end{array} \right) \)
SE3Representation: \( X = {{}^{fa}X^*_{fb}}^{-1} \) (see pinocchio::SE3Base<Scalar,Options>::toActionMatrix)

Reimplemented from dynamicgraph::sot::FeatureAbstract.

◆ computeJacobian()

template<Representation_t representation = R3xSO3Representation>

virtual dg::Matrix& dynamicgraph::sot::FeaturePose< representation >::computeJacobian	(	dg::Matrix &	res,
		int	time
	)

virtual

Computes \( \frac{\partial\ominus}{\partial b} Y \left( {{}^{fb}X_{jb}} {}^{jb}J_{jb} - {{}^{fb}X_{ja}} {}^{ja}J_{ja} \right) \).

There are two different cases, depending on the representation:

R3xSO3Representation: \( Y = \left( \begin{array}{cc} {{}^{fa}R_{fb}} & 0_3 \\ 0_3 & I_3 \end{array} \right) \)
SE3Representation: \( Y = I_6 \)

Implements dynamicgraph::sot::FeatureAbstract.

◆ DECLARE_NO_REFERENCE()

template<Representation_t representation = R3xSO3Representation>

dynamicgraph::sot::FeaturePose< representation >::DECLARE_NO_REFERENCE ( )

◆ display()

template<Representation_t representation = R3xSO3Representation>

virtual void dynamicgraph::sot::FeaturePose< representation >::display ( std::ostream & os ) const

virtual

◆ getClassName()

template<Representation_t representation = R3xSO3Representation>

virtual const std::string& dynamicgraph::sot::FeaturePose< representation >::getClassName ( void ) const

inlinevirtual

Returns the name class.

Reimplemented from dynamicgraph::sot::FeatureAbstract.

◆ getDimension()

template<Representation_t representation = R3xSO3Representation>

virtual unsigned int& dynamicgraph::sot::FeaturePose< representation >::getDimension	(	unsigned int &	res,
		int	time
	)

virtual

Verbose method.

res: The integer in which the dimension will be return.

time: The time at which the feature should be considered.

Returns: Dimension of the feature.

Note: Be careful with features changing their dimension according to time.

Implements dynamicgraph::sot::FeatureAbstract.

◆ selectRotation()

template<Representation_t representation = R3xSO3Representation>

static Flags dynamicgraph::sot::FeaturePose< representation >::selectRotation ( void )

inlinestatic

◆ selectRX()

template<Representation_t representation = R3xSO3Representation>

static Flags dynamicgraph::sot::FeaturePose< representation >::selectRX ( void )

inlinestatic

References dynamicgraph::sot::FLAG_LINE_4.

◆ selectRY()

template<Representation_t representation = R3xSO3Representation>

static Flags dynamicgraph::sot::FeaturePose< representation >::selectRY ( void )

inlinestatic

References dynamicgraph::sot::FLAG_LINE_5.

◆ selectRZ()

template<Representation_t representation = R3xSO3Representation>

static Flags dynamicgraph::sot::FeaturePose< representation >::selectRZ ( void )

inlinestatic

References dynamicgraph::sot::FLAG_LINE_6.

◆ selectTranslation()

template<Representation_t representation = R3xSO3Representation>

static Flags dynamicgraph::sot::FeaturePose< representation >::selectTranslation ( void )

inlinestatic

◆ selectX()

template<Representation_t representation = R3xSO3Representation>

static Flags dynamicgraph::sot::FeaturePose< representation >::selectX ( void )

inlinestatic

Static Feature selection.

References dynamicgraph::sot::FLAG_LINE_1.

◆ selectY()

template<Representation_t representation = R3xSO3Representation>

static Flags dynamicgraph::sot::FeaturePose< representation >::selectY ( void )

inlinestatic

References dynamicgraph::sot::FLAG_LINE_2.

◆ selectZ()

template<Representation_t representation = R3xSO3Representation>

static Flags dynamicgraph::sot::FeaturePose< representation >::selectZ ( void )

inlinestatic

References dynamicgraph::sot::FLAG_LINE_3.

◆ servoCurrentPosition()

template<Representation_t representation = R3xSO3Representation>

void dynamicgraph::sot::FeaturePose< representation >::servoCurrentPosition ( const int & time )

Member Data Documentation

◆ CLASS_NAME

template<Representation_t representation = R3xSO3Representation>

const std::string dynamicgraph::sot::FeaturePose< representation >::CLASS_NAME

static

◆ faMfb

template<Representation_t representation = R3xSO3Representation>

SignalTimeDependent<MatrixHomogeneous, int> dynamicgraph::sot::FeaturePose< representation >::faMfb

Pose of Frame B wrt to Frame A.

◆ faMfbDes

template<Representation_t representation = R3xSO3Representation>

dg::SignalPtr<MatrixHomogeneous, int> dynamicgraph::sot::FeaturePose< representation >::faMfbDes

The desired pose of Frame B wrt to Frame A.

◆ faNufafbDes

template<Representation_t representation = R3xSO3Representation>

dg::SignalPtr<Vector, int> dynamicgraph::sot::FeaturePose< representation >::faNufafbDes

The desired velocity of Frame B wrt to Frame A.

The value is expressed in Frame A.

◆ jaJja

template<Representation_t representation = R3xSO3Representation>

dg::SignalPtr<Matrix, int> dynamicgraph::sot::FeaturePose< representation >::jaJja

Jacobian of the input Joint A, expressed in Joint A

◆ jaMfa

template<Representation_t representation = R3xSO3Representation>

dg::SignalPtr<MatrixHomogeneous, int> dynamicgraph::sot::FeaturePose< representation >::jaMfa

Input pose of Frame A wrt to Joint A.

◆ jbJjb

template<Representation_t representation = R3xSO3Representation>

dg::SignalPtr<Matrix, int> dynamicgraph::sot::FeaturePose< representation >::jbJjb

Jacobian of the input Joint B, expressed in Joint B

◆ jbMfb

template<Representation_t representation = R3xSO3Representation>

dg::SignalPtr<MatrixHomogeneous, int> dynamicgraph::sot::FeaturePose< representation >::jbMfb

Input pose of Frame B wrt to Joint B.

◆ oMja

template<Representation_t representation = R3xSO3Representation>

dg::SignalPtr<MatrixHomogeneous, int> dynamicgraph::sot::FeaturePose< representation >::oMja

Input pose of Joint A wrt to world frame.

◆ oMjb

template<Representation_t representation = R3xSO3Representation>

dg::SignalPtr<MatrixHomogeneous, int> dynamicgraph::sot::FeaturePose< representation >::oMjb

Input pose of Joint B wrt to world frame.

◆ q_faMfb

template<Representation_t representation = R3xSO3Representation>

SignalTimeDependent<Vector7, int> dynamicgraph::sot::FeaturePose< representation >::q_faMfb

Pose of Frame B wrt to Frame A.

It is expressed as a translation followed by a quaternion.

◆ q_faMfbDes

template<Representation_t representation = R3xSO3Representation>

SignalTimeDependent<Vector7, int> dynamicgraph::sot::FeaturePose< representation >::q_faMfbDes

Desired pose of Frame B wrt to Frame A.

It is expressed as a translation followed by a quaternion.

Public Member Functions

Static Public Member Functions

Public Attributes

Static Public Attributes

Detailed Description

template<Representation_t representation = R3xSO3Representation> class dynamicgraph::sot::FeaturePose< representation >

Constructor & Destructor Documentation

◆ FeaturePose()

◆ ~FeaturePose()

Member Function Documentation

◆ computeError()

◆ computeErrorDot()

◆ computeJacobian()

◆ DECLARE_NO_REFERENCE()

◆ display()

◆ getClassName()

◆ getDimension()

◆ selectRotation()

◆ selectRX()

◆ selectRY()

◆ selectRZ()

◆ selectTranslation()

◆ selectX()

◆ selectY()

◆ selectZ()

◆ servoCurrentPosition()

Member Data Documentation

◆ CLASS_NAME

◆ faMfb

◆ faMfbDes

◆ faNufafbDes

◆ jaJja

◆ jaMfa

◆ jbJjb

◆ jbMfb

◆ oMja

◆ oMjb

◆ q_faMfb

◆ q_faMfbDes

template<Representation_t representation = R3xSO3Representation>
class dynamicgraph::sot::FeaturePose< representation >