feature-pose.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2019,
3  * Joseph Mirabel
4  *
5  * LAAS-CNRS
6  *
7  */
8 
9 #ifndef __SOT_FEATURE_TRANSFORMATION_HH__
10 #define __SOT_FEATURE_TRANSFORMATION_HH__
11 
12 /* --------------------------------------------------------------------- */
13 /* --- INCLUDE --------------------------------------------------------- */
14 /* --------------------------------------------------------------------- */
15 
16 /* SOT */
17 #include <sot/core/config.hh>
20 
21 /* --------------------------------------------------------------------- */
22 /* --- CLASS ----------------------------------------------------------- */
23 /* --------------------------------------------------------------------- */
24 
25 namespace dynamicgraph {
26 namespace sot {
27 namespace dg = dynamicgraph;
28 
31 
62 template <Representation_t representation = R3xSO3Representation>
64 
65 public:
66  static const std::string CLASS_NAME;
67  virtual const std::string &getClassName(void) const { return CLASS_NAME; }
68 
69 public:
73  dg::SignalPtr<MatrixHomogeneous, int> oMja;
76  dg::SignalPtr<MatrixHomogeneous, int> jaMfa;
78  dg::SignalPtr<MatrixHomogeneous, int> oMjb;
80  dg::SignalPtr<MatrixHomogeneous, int> jbMfb;
82  dg::SignalPtr<Matrix, int> jaJja;
84  dg::SignalPtr<Matrix, int> jbJjb;
85 
87  dg::SignalPtr<MatrixHomogeneous, int> faMfbDes;
90  dg::SignalPtr<Vector, int> faNufafbDes;
96  SignalTimeDependent<MatrixHomogeneous, int> faMfb;
98 
101  SignalTimeDependent<Vector7, int> q_faMfb;
102 
105  SignalTimeDependent<Vector7, int> q_faMfbDes;
111 
117 public:
118  FeaturePose(const std::string &name);
119  virtual ~FeaturePose(void) {}
120 
121  virtual unsigned int &getDimension(unsigned int &dim, int time);
122 
124  virtual dg::Vector &computeError(dg::Vector &res, int time);
131  virtual dg::Vector &computeErrorDot(dg::Vector &res, int time);
138  virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time);
139 
141  inline static Flags selectX(void) { return FLAG_LINE_1; }
142  inline static Flags selectY(void) { return FLAG_LINE_2; }
143  inline static Flags selectZ(void) { return FLAG_LINE_3; }
144  inline static Flags selectRX(void) { return FLAG_LINE_4; }
145  inline static Flags selectRY(void) { return FLAG_LINE_5; }
146  inline static Flags selectRZ(void) { return FLAG_LINE_6; }
147 
148  inline static Flags selectTranslation(void) { return Flags(7); }
149  inline static Flags selectRotation(void) { return Flags(56); }
150 
151  virtual void display(std::ostream &os) const;
152 
153 public:
154  void servoCurrentPosition(const int &time);
155 
156 private:
157  MatrixHomogeneous &computefaMfb(MatrixHomogeneous &res, int time);
158  Vector7 &computeQfaMfb(Vector7 &res, int time);
159  Vector7 &computeQfaMfbDes(Vector7 &res, int time);
160 
162 };
163 
164 } /* namespace sot */
165 } /* namespace dynamicgraph */
166 
167 #endif // #ifndef __SOT_FEATURE_TRANSFORMATION_HH__
168 
169 /*
170  * Local variables:
171  * c-basic-offset: 2
172  * End:
173  */
DECLARE_NO_REFERENCE
#define DECLARE_NO_REFERENCE
Definition: feature-abstract.hh:273
dynamicgraph::sot::FeatureAbstract::jacobianSOUT
SignalTimeDependent< dg::Matrix, int > jacobianSOUT
Jacobian of the error wrt the robot state: .
Definition: feature-abstract.hh:190
dynamicgraph::sot::FLAG_LINE_3
const SOT_CORE_EXPORT Flags FLAG_LINE_3
dynamicgraph::sot::SE3Representation
@ SE3Representation
Definition: feature-pose.hh:30
dynamicgraph::sot::FeaturePose::selectRY
static Flags selectRY(void)
Definition: feature-pose.hh:145
dynamicgraph::sot::MatrixHomogeneous
Eigen::Transform< double, 3, Eigen::Affine > SOT_CORE_EXPORT MatrixHomogeneous
Definition: matrix-geometry.hh:74
dynamicgraph
Definition: abstract-sot-external-interface.hh:17
dynamicgraph::sot::FLAG_LINE_5
const SOT_CORE_EXPORT Flags FLAG_LINE_5
dynamicgraph::sot::FeaturePose::selectTranslation
static Flags selectTranslation(void)
Definition: feature-pose.hh:148
dynamicgraph::sot::FeaturePose::~FeaturePose
virtual ~FeaturePose(void)
Definition: feature-pose.hh:119
dynamicgraph::sot::FeaturePose::selectY
static Flags selectY(void)
Definition: feature-pose.hh:142
dynamicgraph::sot::FeaturePose::q_faMfbDes
SignalTimeDependent< Vector7, int > q_faMfbDes
Definition: feature-pose.hh:105
dynamicgraph::sot::FeatureAbstract::errorSOUT
SignalTimeDependent< dg::Vector, int > errorSOUT
This signal returns the error between the desired value and the current value : .
Definition: feature-abstract.hh:182
dynamicgraph::sot::FeaturePose::selectZ
static Flags selectZ(void)
Definition: feature-pose.hh:143
dynamicgraph::sot::FeaturePose::oMjb
dg::SignalPtr< MatrixHomogeneous, int > oMjb
Input pose of Joint B wrt to world frame.
Definition: feature-pose.hh:78
feature-abstract.hh
dynamicgraph::sot::FeaturePose::selectRZ
static Flags selectRZ(void)
Definition: feature-pose.hh:146
dynamicgraph::sot::FeaturePose::q_faMfb
SignalTimeDependent< Vector7, int > q_faMfb
Definition: feature-pose.hh:101
dynamicgraph::sot::FeaturePose::jbMfb
dg::SignalPtr< MatrixHomogeneous, int > jbMfb
Input pose of Frame B wrt to Joint B.
Definition: feature-pose.hh:80
dynamicgraph::sot::FeaturePose::faNufafbDes
dg::SignalPtr< Vector, int > faNufafbDes
Definition: feature-pose.hh:90
dynamicgraph::sot::FeatureAbstract
This class gives the abstract definition of a feature.
Definition: feature-abstract.hh:76
dynamicgraph::sot::R3xSO3Representation
@ R3xSO3Representation
Definition: feature-pose.hh:30
dynamicgraph::sot::FeaturePose::selectX
static Flags selectX(void)
Definition: feature-pose.hh:141
dynamicgraph::sot::FeaturePose
Feature that controls the relative (or absolute) pose between two frames A (or world) and B.
Definition: feature-pose.hh:63
dynamicgraph::sot::FeaturePose::selectRotation
static Flags selectRotation(void)
Definition: feature-pose.hh:149
dynamicgraph::sot::FeaturePose::faMfbDes
dg::SignalPtr< MatrixHomogeneous, int > faMfbDes
The desired pose of Frame B wrt to Frame A.
Definition: feature-pose.hh:87
dynamicgraph::sot::FeaturePose::CLASS_NAME
static const std::string CLASS_NAME
Definition: feature-pose.hh:66
dynamicgraph::sot::FeaturePose::getClassName
virtual const std::string & getClassName(void) const
Returns the name class.
Definition: feature-pose.hh:67
dynamicgraph::sot::Representation_t
Representation_t
Enum used to specify what difference operation is used in FeaturePose.
Definition: feature-pose.hh:30
dynamicgraph::sot::FeaturePose::jbJjb
dg::SignalPtr< Matrix, int > jbJjb
Jacobian of the input Joint B, expressed in Joint B
Definition: feature-pose.hh:84
dynamicgraph::sot::FeaturePose::selectRX
static Flags selectRX(void)
Definition: feature-pose.hh:144
matrix-geometry.hh
dynamicgraph::sot::FLAG_LINE_4
const SOT_CORE_EXPORT Flags FLAG_LINE_4
SOT_CORE_DLLAPI
#define SOT_CORE_DLLAPI
Definition: config.hh:64
dynamicgraph::sot::Vector7
Eigen::Matrix< double, 7, 1 > SOT_CORE_EXPORT Vector7
Definition: matrix-geometry.hh:83
dynamicgraph::sot::FLAG_LINE_6
const SOT_CORE_EXPORT Flags FLAG_LINE_6
dynamicgraph::sot::FeatureAbstract::selectionSIN
SignalPtr< Flags, int > selectionSIN
This vector specifies which dimension are used to perform the computation. For instance let us assume...
Definition: feature-abstract.hh:170
dynamicgraph::sot::Flags
Definition: flags.hh:31
dynamicgraph::sot::FeaturePose::jaMfa
dg::SignalPtr< MatrixHomogeneous, int > jaMfa
Input pose of Frame A wrt to Joint A.
Definition: feature-pose.hh:76
dynamicgraph::sot::FeaturePose::jaJja
dg::SignalPtr< Matrix, int > jaJja
Jacobian of the input Joint A, expressed in Joint A
Definition: feature-pose.hh:82
dynamicgraph::sot::FLAG_LINE_2
const SOT_CORE_EXPORT Flags FLAG_LINE_2
config.hh
dynamicgraph::sot::FLAG_LINE_1
const SOT_CORE_EXPORT Flags FLAG_LINE_1