feature-point6d.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2010,
3  * François Bleibel,
4  * Olivier Stasse,
5  *
6  * CNRS/AIST
7  *
8  */
9 
10 #ifndef __SOT_FEATURE_POINT6D_HH__
11 #define __SOT_FEATURE_POINT6D_HH__
12 
13 /* --------------------------------------------------------------------- */
14 /* --- INCLUDE --------------------------------------------------------- */
15 /* --------------------------------------------------------------------- */
16 
17 /* SOT */
22 
23 /* --------------------------------------------------------------------- */
24 /* --- API ------------------------------------------------------------- */
25 /* --------------------------------------------------------------------- */
26 
27 #if defined(WIN32)
28 #if defined(feature_point6d_EXPORTS)
29 #define SOTFEATUREPOINT6D_EXPORT __declspec(dllexport)
30 #else
31 #define SOTFEATUREPOINT6D_EXPORT __declspec(dllimport)
32 #endif
33 #else
34 #define SOTFEATUREPOINT6D_EXPORT
35 #endif
36 
37 /* --------------------------------------------------------------------- */
38 /* --- CLASS ----------------------------------------------------------- */
39 /* --------------------------------------------------------------------- */
40 
41 namespace dynamicgraph {
42 namespace sot {
43 namespace dg = dynamicgraph;
44 
51  : public FeatureAbstract,
52  public FeatureReferenceHelper<FeaturePoint6d> {
53 
54 public:
55  static const std::string CLASS_NAME;
56  virtual const std::string &getClassName(void) const { return CLASS_NAME; }
57 
58  /* --- Frame type --------------------------------------------------------- */
59 protected:
60  enum ComputationFrameType { FRAME_DESIRED, FRAME_CURRENT };
62 
63 public:
65  void computationFrame(const std::string &inFrame);
67  std::string computationFrame() const;
68 
69 private:
70  ComputationFrameType computationFrame_;
71 
72  /* --- SIGNALS ------------------------------------------------------------ */
73 public:
74  dg::SignalPtr<MatrixHomogeneous, int> positionSIN;
75  dg::SignalPtr<dg::Vector, int> velocitySIN;
76  dg::SignalPtr<dg::Matrix, int> articularJacobianSIN;
77 
81 
87 public:
88  FeaturePoint6d(const std::string &name);
89  virtual ~FeaturePoint6d(void) {}
90 
91  virtual unsigned int &getDimension(unsigned int &dim, int time);
92 
93  virtual dg::Vector &computeError(dg::Vector &res, int time);
94  virtual dg::Vector &computeErrordot(dg::Vector &res, int time);
95  virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time);
96 
98  inline static Flags selectX(void) { return FLAG_LINE_1; }
99  inline static Flags selectY(void) { return FLAG_LINE_2; }
100  inline static Flags selectZ(void) { return FLAG_LINE_3; }
101  inline static Flags selectRX(void) { return FLAG_LINE_4; }
102  inline static Flags selectRY(void) { return FLAG_LINE_5; }
103  inline static Flags selectRZ(void) { return FLAG_LINE_6; }
104 
105  inline static Flags selectTranslation(void) { return Flags(7); }
106  inline static Flags selectRotation(void) { return Flags(56); }
107 
108  virtual void display(std::ostream &os) const;
109 
110 public:
111  void servoCurrentPosition(void);
112 
113 private:
114  // Intermediate variables for internal computations
115  Eigen::Vector3d v_, omega_, errordot_t_, errordot_th_, Rreftomega_, t_, tref_;
116  VectorUTheta error_th_;
117  MatrixRotation R_, Rref_, Rt_, Rreft_;
118  Eigen::Matrix3d P_, Pinv_;
119  double accuracy_;
120  void inverseJacobianRodrigues();
121 };
122 
123 } /* namespace sot */
124 } /* namespace dynamicgraph */
125 
126 #endif // #ifndef __SOT_FEATURE_POINT6D_HH__
127 
128 /*
129  * Local variables:
130  * c-basic-offset: 2
131  * End:
132  */
Class that defines point-6d control feature.
Definition: feature-point6d.hh:50
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
Definition: feature-abstract.hh:225
SOT_CORE_EXPORT const Flags FLAG_LINE_2
Definition: feature-point6d.hh:60
static Flags selectRY(void)
Definition: feature-point6d.hh:102
static Flags selectRX(void)
Definition: feature-point6d.hh:101
static Flags selectZ(void)
Definition: feature-point6d.hh:100
dg::SignalPtr< dg::Matrix, int > articularJacobianSIN
Definition: feature-point6d.hh:76
ComputationFrameType
Definition: feature-point6d.hh:60
dg::SignalPtr< dg::Vector, int > velocitySIN
Definition: feature-point6d.hh:75
SOT_CORE_EXPORT const Flags FLAG_LINE_5
static Flags selectRZ(void)
Definition: feature-point6d.hh:103
SOT_CORE_EXPORT const Flags FLAG_LINE_4
virtual ~FeaturePoint6d(void)
Definition: feature-point6d.hh:89
SignalTimeDependent< dg::Matrix, int > jacobianSOUT
Jacobian of the error wrt the robot state: .
Definition: feature-abstract.hh:190
static Flags selectX(void)
Definition: feature-point6d.hh:98
static Flags selectRotation(void)
Definition: feature-point6d.hh:106
static Flags selectY(void)
Definition: feature-point6d.hh:99
Eigen::AngleAxis< double > SOT_CORE_EXPORT VectorUTheta
Definition: matrix-geometry.hh:76
SOT_CORE_EXPORT const Flags FLAG_LINE_1
SignalTimeDependent< dg::Vector, int > errorSOUT
This signal returns the error between the desired value and the current value : . ...
Definition: feature-abstract.hh:182
SOT_CORE_EXPORT const Flags FLAG_LINE_3
static Flags selectTranslation(void)
Definition: feature-point6d.hh:105
Definition: flags.hh:31
This class gives the abstract definition of a feature.
Definition: feature-abstract.hh:76
Eigen::Matrix< double, 3, 3 > SOT_CORE_EXPORT MatrixRotation
Definition: matrix-geometry.hh:75
static const std::string CLASS_NAME
Definition: feature-point6d.hh:55
#define DECLARE_REFERENCE_FUNCTIONS(FeatureSpecialized)
Definition: feature-abstract.hh:247
dg::SignalPtr< MatrixHomogeneous, int > positionSIN
Definition: feature-point6d.hh:74
virtual const std::string & getClassName(void) const
Returns the name class.
Definition: feature-point6d.hh:56
SOT_CORE_EXPORT const Flags FLAG_LINE_6
#define SOTFEATUREPOINT6D_EXPORT
Definition: feature-point6d.hh:34
Definition: abstract-sot-external-interface.hh:17
static const ComputationFrameType COMPUTATION_FRAME_DEFAULT
Definition: feature-point6d.hh:61