sot-core  4.10.1
Hierarchical task solver plug-in for dynamic-graph.
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_POSE_HH__
10 #define __SOT_FEATURE_POSE_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 
30 
61 template <Representation_t representation = R3xSO3Representation>
62 class SOT_CORE_DLLAPI FeaturePose : public FeatureAbstract {
63 
64 public:
65  static const std::string CLASS_NAME;
66  virtual const std::string &getClassName(void) const { return CLASS_NAME; }
67 
68 public:
72  dynamicgraph::SignalPtr<MatrixHomogeneous, int> oMja;
75  dynamicgraph::SignalPtr<MatrixHomogeneous, int> jaMfa;
77  dynamicgraph::SignalPtr<MatrixHomogeneous, int> oMjb;
79  dynamicgraph::SignalPtr<MatrixHomogeneous, int> jbMfb;
81  dynamicgraph::SignalPtr<Matrix, int> jaJja;
83  dynamicgraph::SignalPtr<Matrix, int> jbJjb;
84 
86  dynamicgraph::SignalPtr<MatrixHomogeneous, int> faMfbDes;
89  dynamicgraph::SignalPtr<Vector, int> faNufafbDes;
95  SignalTimeDependent<MatrixHomogeneous, int> faMfb;
97 
100  SignalTimeDependent<Vector7, int> q_faMfb;
101 
104  SignalTimeDependent<Vector7, int> q_faMfbDes;
110 
116 public:
117  FeaturePose(const std::string &name);
118  virtual ~FeaturePose(void);
119 
120  virtual unsigned int &getDimension(unsigned int &dim, int time);
121 
123  virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res,
124  int time);
131  virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &res,
132  int time);
139  virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res,
140  int time);
141 
143  inline static Flags selectX(void) { return FLAG_LINE_1; }
144  inline static Flags selectY(void) { return FLAG_LINE_2; }
145  inline static Flags selectZ(void) { return FLAG_LINE_3; }
146  inline static Flags selectRX(void) { return FLAG_LINE_4; }
147  inline static Flags selectRY(void) { return FLAG_LINE_5; }
148  inline static Flags selectRZ(void) { return FLAG_LINE_6; }
149 
150  inline static Flags selectTranslation(void) { return Flags(7); }
151  inline static Flags selectRotation(void) { return Flags(56); }
152 
153  virtual void display(std::ostream &os) const;
154 
155 public:
156  void servoCurrentPosition(const int &time);
157 
158 private:
159  MatrixHomogeneous &computefaMfb(MatrixHomogeneous &res, int time);
160  Vector7 &computeQfaMfb(Vector7 &res, int time);
161  Vector7 &computeQfaMfbDes(Vector7 &res, int time);
162 
164 };
165 
166 template <typename T>
167 Vector6d convertVelocity(const MatrixHomogeneous &M,
168  const MatrixHomogeneous &Mdes,
169  const Vector &faNufafbDes);
170 
171 template <> const std::string FeaturePose<SE3Representation>::CLASS_NAME;
172 template <> const std::string FeaturePose<R3xSO3Representation>::CLASS_NAME;
173 
174 #if __cplusplus >= 201103L
175 extern template class SOT_CORE_DLLAPI FeaturePose<SE3Representation>;
176 extern template class SOT_CORE_DLLAPI FeaturePose<R3xSO3Representation>;
177 #endif
178 
179 typedef FeaturePose<R3xSO3Representation> FeaturePose_t;
180 typedef FeaturePose<SE3Representation> FeaturePoseSE3_t;
181 
182 } /* namespace sot */
183 } /* namespace dynamicgraph */
184 
185 #endif // #ifndef __SOT_FEATURE_POSE_HH__
186 
187 /*
188  * Local variables:
189  * c-basic-offset: 2
190  * End:
191  */
dynamicgraph::SignalPtr< MatrixHomogeneous, int > jaMfa
Input pose of Frame A wrt to Joint A.
Definition: feature-pose.hh:75
Eigen::Transform< double, 3, Eigen::Affine > SOT_CORE_EXPORT MatrixHomogeneous
Definition: matrix-geometry.hh:74
SignalPtr< Flags, int > selectionSIN
This vector specifies which dimension are used to perform the computation. For instance let us assume...
Definition: feature-abstract.hh:172
SOT_CORE_EXPORT const Flags FLAG_LINE_2
#define DECLARE_NO_REFERENCE
Definition: feature-abstract.hh:275
virtual const std::string & getClassName(void) const
Returns the name class.
Definition: feature-pose.hh:66
static Flags selectY(void)
Definition: feature-pose.hh:144
SignalTimeDependent< Vector7, int > q_faMfb
Definition: feature-pose.hh:100
static Flags selectRY(void)
Definition: feature-pose.hh:147
SOT_CORE_EXPORT const Flags FLAG_LINE_5
SOT_CORE_EXPORT const Flags FLAG_LINE_4
SignalTimeDependent< dynamicgraph::Matrix, int > jacobianSOUT
Jacobian of the error wrt the robot state: .
Definition: feature-abstract.hh:192
dynamicgraph::SignalPtr< MatrixHomogeneous, int > jbMfb
Input pose of Frame B wrt to Joint B.
Definition: feature-pose.hh:79
static Flags selectTranslation(void)
Definition: feature-pose.hh:150
static const std::string CLASS_NAME
Definition: feature-pose.hh:65
SignalTimeDependent< Vector7, int > q_faMfbDes
Definition: feature-pose.hh:104
FeaturePose< R3xSO3Representation > FeaturePose_t
Definition: feature-pose.hh:179
Vector6d convertVelocity(const MatrixHomogeneous &M, const MatrixHomogeneous &Mdes, const Vector &faNufafbDes)
static Flags selectRotation(void)
Definition: feature-pose.hh:151
FeaturePose< SE3Representation > FeaturePoseSE3_t
Definition: feature-pose.hh:180
Representation_t
Enum used to specify what difference operation is used in FeaturePose.
Definition: feature-pose.hh:29
static Flags selectRZ(void)
Definition: feature-pose.hh:148
SOT_CORE_EXPORT const Flags FLAG_LINE_1
SOT_CORE_EXPORT const Flags FLAG_LINE_3
dynamicgraph::SignalPtr< Matrix, int > jaJja
Jacobian of the input Joint A, expressed in Joint A
Definition: feature-pose.hh:81
Definition: flags.hh:31
Definition: feature-pose.hh:29
This class gives the abstract definition of a feature.
Definition: feature-abstract.hh:75
static Flags selectZ(void)
Definition: feature-pose.hh:145
Eigen::Matrix< double, 7, 1 > SOT_CORE_EXPORT Vector7
Definition: matrix-geometry.hh:83
Feature that controls the relative (or absolute) pose between two frames A (or world) and B...
Definition: feature-pose.hh:62
dynamicgraph::SignalPtr< Matrix, int > jbJjb
Jacobian of the input Joint B, expressed in Joint B
Definition: feature-pose.hh:83
dynamicgraph::SignalPtr< MatrixHomogeneous, int > faMfbDes
The desired pose of Frame B wrt to Frame A.
Definition: feature-pose.hh:86
SOT_CORE_EXPORT const Flags FLAG_LINE_6
dynamicgraph::SignalPtr< MatrixHomogeneous, int > oMjb
Input pose of Joint B wrt to world frame.
Definition: feature-pose.hh:77
dynamicgraph::SignalPtr< Vector, int > faNufafbDes
Definition: feature-pose.hh:89
SignalTimeDependent< dynamicgraph::Vector, int > errorSOUT
This signal returns the error between the desired value and the current value : . ...
Definition: feature-abstract.hh:184
static Flags selectRX(void)
Definition: feature-pose.hh:146
static Flags selectX(void)
Definition: feature-pose.hh:143
Definition: abstract-sot-external-interface.hh:17
Definition: feature-pose.hh:29