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

#include <feature-pose.hh>

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

Public Member Functions
const std::string	CLASS_NAME

const std::string	CLASS_NAME

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

virtual dynamicgraph::Vector &	computeErrorDot (dynamicgraph::Vector &res, sigtime_t time)

virtual dynamicgraph::Matrix &	computeJacobian (dynamicgraph::Matrix &res, sigtime_t time)

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

	FeaturePose (const std::string &name)

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

virtual size_type &	getDimension (size_type &dim, sigtime_t time)
	Verbose method. More...

void	servoCurrentPosition (const sigtime_t &time)

virtual	~FeaturePose (void)

Dealing with the reference value to be reach with this feature.

	DECLARE_NO_REFERENCE ()

Public Member Functions inherited from dynamicgraph::sot::FeatureAbstract
	FeatureAbstract (const std::string &name)
	Default constructor: the name of the class should be given. More...

void	featureRegistration (void)
	Register the feature in the stack of tasks. More...

void	initCommands (void)

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

size_type	getDimension (sigtime_t time)
	Short method. More...

size_type	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< dynamicgraph::Vector, sigtime_t > &	getErrorDot ()

Public Member Functions inherited from dynamicgraph::Entity
std::ostream &	displaySignalList (std::ostream &os) const

	Entity (const std::string &name)

const std::string &	getCommandList () const

virtual std::string	getDocString () const

LoggerVerbosity	getLoggerVerbosityLevel ()

LoggerVerbosity	getLoggerVerbosityLevel ()

const std::string &	getName () const

command::Command *	getNewStyleCommand (const std::string &cmdName)

CommandMap_t	getNewStyleCommandMap ()

SignalBase< sigtime_t > &	getSignal (const std::string &signalName)

const SignalBase< sigtime_t > &	getSignal (const std::string &signalName) const

SignalMap	getSignalMap () const

double	getStreamPrintPeriod ()

double	getStreamPrintPeriod ()

double	getTimeSample ()

double	getTimeSample ()

bool	hasSignal (const std::string &signame) const

Logger &	logger ()

Logger &	logger ()

const Logger &	logger () const

const Logger &	logger () const

void	sendMsg (const std::string &msg, MsgType t=MSG_TYPE_INFO, const std::string &lineId="")

void	sendMsg (const std::string &msg, MsgType t=MSG_TYPE_INFO, const std::string &lineId="")

void	setLoggerVerbosityLevel (LoggerVerbosity lv)

void	setLoggerVerbosityLevel (LoggerVerbosity lv)

bool	setStreamPrintPeriod (double t)

bool	setStreamPrintPeriod (double t)

bool	setTimeSample (double t)

bool	setTimeSample (double t)

virtual SignalBase< sigtime_t > *	test ()

virtual void	test2 (SignalBase< sigtime_t > *)

virtual std::ostream &	writeCompletionList (std::ostream &os) const

virtual	~Entity ()

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

SignalTimeDependent< dynamicgraph::Matrix, sigtime_t >	jacobianSOUT
	Jacobian of the error wrt the robot state: $J = \frac{\partial {\bf s}}{\partial {\bf q}}$ . More...

SignalPtr< Flags, sigtime_t >	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 . More...

Input signals
dynamicgraph::SignalPtr< MatrixHomogeneous, sigtime_t >	oMja
	Input pose of Joint A wrt to world frame. More...

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

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

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

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

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

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

dynamicgraph::SignalPtr< Vector, sigtime_t >	faNufafbDes

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

SignalTimeDependent< Vector7, sigtime_t >	q_faMfb

SignalTimeDependent< Vector7, sigtime_t >	q_faMfbDes

Public Attributes inherited from dynamicgraph::sot::FeatureAbstract
SignalPtr< Flags, sigtime_t >	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 . More...

SignalPtr< dynamicgraph::Vector, sigtime_t >	errordotSIN
	Derivative of the reference value. More...

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

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

SignalTimeDependent< dynamicgraph::Matrix, sigtime_t >	jacobianSOUT
	Jacobian of the error wrt the robot state: $J = \frac{\partial {\bf s}}{\partial {\bf q}}$ . More...

SignalTimeDependent< size_type, sigtime_t >	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 = "FeatureAbstract"
	Store the name of the class. More...

Private Member Functions
MatrixHomogeneous &	computefaMfb (MatrixHomogeneous &res, sigtime_t time)

Vector7 &	computeQfaMfb (Vector7 &res, sigtime_t time)

Vector7 &	computeQfaMfbDes (Vector7 &res, sigtime_t time)

Additional Inherited Members
Public Types inherited from dynamicgraph::Entity
typedef std::map< const std::string, command::Command * >	CommandMap_t

typedef std::map< std::string, SignalBase< sigtime_t > * >	SignalMap

Protected Member Functions inherited from dynamicgraph::Entity
void	addCommand (const std::string &name, command::Command *command)

void	entityDeregistration ()

void	entityRegistration ()

void	signalDeregistration (const std::string &name)

void	signalRegistration (const SignalArray< sigtime_t > &signals)

Protected Attributes inherited from dynamicgraph::Entity
CommandMap_t	commandMap

Logger	logger_

std::string	name

SignalMap	signalMap

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}$

Definition at line 62 of file feature-pose.hh.

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 )

virtual

Member Function Documentation

◆ CLASS_NAME() [1/2]

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

Definition at line 160 of file feature-pose.hh.

◆ CLASS_NAME() [2/2]

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

Definition at line 162 of file feature-pose.hh.

◆ computeError()

template<Representation_t representation = R3xSO3Representation>

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

virtual

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

Implements dynamicgraph::sot::FeatureAbstract.

◆ computeErrorDot()

template<Representation_t representation = R3xSO3Representation>

virtual dynamicgraph::Vector& dynamicgraph::sot::FeaturePose< representation >::computeErrorDot	(	dynamicgraph::Vector &	res,
		sigtime_t	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.

◆ computefaMfb()

template<Representation_t representation = R3xSO3Representation>

MatrixHomogeneous& dynamicgraph::sot::FeaturePose< representation >::computefaMfb	(	MatrixHomogeneous &	res,
		sigtime_t	time
	)

private

◆ computeJacobian()

template<Representation_t representation = R3xSO3Representation>

virtual dynamicgraph::Matrix& dynamicgraph::sot::FeaturePose< representation >::computeJacobian	(	dynamicgraph::Matrix &	res,
		sigtime_t	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:

Implements dynamicgraph::sot::FeatureAbstract.

◆ computeQfaMfb()

template<Representation_t representation = R3xSO3Representation>

Vector7& dynamicgraph::sot::FeaturePose< representation >::computeQfaMfb	(	Vector7 &	res,
		sigtime_t	time
	)

private

◆ computeQfaMfbDes()

template<Representation_t representation = R3xSO3Representation>

Vector7& dynamicgraph::sot::FeaturePose< representation >::computeQfaMfbDes	(	Vector7 &	res,
		sigtime_t	time
	)

private

◆ 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

Reimplemented from dynamicgraph::Entity.

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

Definition at line 65 of file feature-pose.hh.

◆ getDimension()

template<Representation_t representation = R3xSO3Representation>

virtual size_type& dynamicgraph::sot::FeaturePose< representation >::getDimension	(	size_type &	res,
		sigtime_t	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.

◆ servoCurrentPosition()

template<Representation_t representation = R3xSO3Representation>

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

Member Data Documentation

◆ CLASS_NAME

template<Representation_t representation = R3xSO3Representation>

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

static

Definition at line 64 of file feature-pose.hh.

◆ errorSOUT

template<Representation_t representation = R3xSO3Representation>

SignalTimeDependent<dynamicgraph::Vector, sigtime_t> dynamicgraph::sot::FeatureAbstract::errorSOUT

This signal returns the error between the desired value and the current value : $E(t) = {\bf s}(t) - {\bf s}^*(t)$ .

Definition at line 185 of file feature-abstract.hh.

◆ faMfb

template<Representation_t representation = R3xSO3Representation>

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

Pose of Frame B wrt to Frame A.

Definition at line 95 of file feature-pose.hh.

◆ faMfbDes

template<Representation_t representation = R3xSO3Representation>

dynamicgraph::SignalPtr<MatrixHomogeneous, sigtime_t> dynamicgraph::sot::FeaturePose< representation >::faMfbDes

The desired pose of Frame B wrt to Frame A.

Definition at line 85 of file feature-pose.hh.

◆ faNufafbDes

template<Representation_t representation = R3xSO3Representation>

dynamicgraph::SignalPtr<Vector, sigtime_t> dynamicgraph::sot::FeaturePose< representation >::faNufafbDes

The desired velocity of Frame B wrt to Frame A. The value is expressed in Frame A.

Definition at line 88 of file feature-pose.hh.

◆ jacobianSOUT

template<Representation_t representation = R3xSO3Representation>

SignalTimeDependent<dynamicgraph::Matrix, sigtime_t> dynamicgraph::sot::FeatureAbstract::jacobianSOUT

Jacobian of the error wrt the robot state: $J = \frac{\partial {\bf s}}{\partial {\bf q}}$ .

Definition at line 193 of file feature-abstract.hh.

◆ jaJja

template<Representation_t representation = R3xSO3Representation>

dynamicgraph::SignalPtr<Matrix, sigtime_t> dynamicgraph::sot::FeaturePose< representation >::jaJja

Jacobian of the input Joint A, expressed in Joint A

Definition at line 80 of file feature-pose.hh.

◆ jaMfa

template<Representation_t representation = R3xSO3Representation>

dynamicgraph::SignalPtr<MatrixHomogeneous, sigtime_t> dynamicgraph::sot::FeaturePose< representation >::jaMfa

Input pose of Frame A wrt to Joint A.

Definition at line 74 of file feature-pose.hh.

◆ jbJjb

template<Representation_t representation = R3xSO3Representation>

dynamicgraph::SignalPtr<Matrix, sigtime_t> dynamicgraph::sot::FeaturePose< representation >::jbJjb

Jacobian of the input Joint B, expressed in Joint B

Definition at line 82 of file feature-pose.hh.

◆ jbMfb

template<Representation_t representation = R3xSO3Representation>

dynamicgraph::SignalPtr<MatrixHomogeneous, sigtime_t> dynamicgraph::sot::FeaturePose< representation >::jbMfb

Input pose of Frame B wrt to Joint B.

Definition at line 78 of file feature-pose.hh.

◆ oMja

template<Representation_t representation = R3xSO3Representation>

dynamicgraph::SignalPtr<MatrixHomogeneous, sigtime_t> dynamicgraph::sot::FeaturePose< representation >::oMja

Input pose of Joint A wrt to world frame.

Definition at line 72 of file feature-pose.hh.

◆ oMjb

template<Representation_t representation = R3xSO3Representation>

dynamicgraph::SignalPtr<MatrixHomogeneous, sigtime_t> dynamicgraph::sot::FeaturePose< representation >::oMjb

Input pose of Joint B wrt to world frame.

Definition at line 76 of file feature-pose.hh.

◆ q_faMfb

template<Representation_t representation = R3xSO3Representation>

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

Pose of Frame B wrt to Frame A. It is expressed as a translation followed by a quaternion.

Definition at line 99 of file feature-pose.hh.

◆ q_faMfbDes

template<Representation_t representation = R3xSO3Representation>

SignalTimeDependent<Vector7, sigtime_t> 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.

Definition at line 103 of file feature-pose.hh.

◆ selectionSIN

template<Representation_t representation = R3xSO3Representation>

SignalPtr<Flags, sigtime_t> dynamicgraph::sot::FeatureAbstract::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] $ .

Definition at line 173 of file feature-abstract.hh.

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

feature-pose.hh

Public Member Functions

Public Attributes

Static Public Attributes

Private Member Functions

Additional Inherited Members

Detailed Description

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

Constructor & Destructor Documentation

◆ FeaturePose()

◆ ~FeaturePose()

Member Function Documentation

◆ CLASS_NAME() [1/2]

◆ CLASS_NAME() [2/2]

◆ computeError()

◆ computeErrorDot()

◆ computefaMfb()

◆ computeJacobian()

◆ computeQfaMfb()

◆ computeQfaMfbDes()

◆ DECLARE_NO_REFERENCE()

◆ display()

◆ getClassName()

◆ getDimension()

◆ servoCurrentPosition()

Member Data Documentation

◆ CLASS_NAME

◆ errorSOUT

◆ faMfb

◆ faMfbDes

◆ faNufafbDes

◆ jacobianSOUT

◆ jaJja

◆ jaMfa

◆ jbJjb

◆ jbMfb

◆ oMja

◆ oMjb

◆ q_faMfb

◆ q_faMfbDes

◆ selectionSIN

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