sot-core  4.11.6
Hierarchical task solver plug-in for dynamic-graph.
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 
50  : public FeatureAbstract,
51  public FeatureReferenceHelper<FeaturePoint6d> {
52 
53 public:
54  static const std::string CLASS_NAME;
55  virtual const std::string &getClassName(void) const { return CLASS_NAME; }
56 
57  /* --- Frame type --------------------------------------------------------- */
58 protected:
59  enum ComputationFrameType { FRAME_DESIRED, FRAME_CURRENT };
61 
62 public:
64  void computationFrame(const std::string &inFrame);
66  std::string computationFrame() const;
67 
68 private:
69  ComputationFrameType computationFrame_;
70 
71  /* --- SIGNALS ------------------------------------------------------------ */
72 public:
73  dynamicgraph::SignalPtr<MatrixHomogeneous, int> positionSIN;
74  dynamicgraph::SignalPtr<dynamicgraph::Vector, int> velocitySIN;
75  dynamicgraph::SignalPtr<dynamicgraph::Matrix, int> articularJacobianSIN;
76 
80 
86 public:
87  FeaturePoint6d(const std::string &name);
88  virtual ~FeaturePoint6d(void) {}
89 
90  virtual unsigned int &getDimension(unsigned int &dim, int time);
91 
92  virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res,
93  int time);
94  virtual dynamicgraph::Vector &computeErrordot(dynamicgraph::Vector &res,
95  int time);
96  virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res,
97  int time);
98 
100  inline static Flags selectX(void) { return Flags("100000"); }
101  inline static Flags selectY(void) { return Flags("010000"); }
102  inline static Flags selectZ(void) { return Flags("001000"); }
103  inline static Flags selectRX(void) { return Flags("000100"); }
104  inline static Flags selectRY(void) { return Flags("000010"); }
105  inline static Flags selectRZ(void) { return Flags("000001"); }
106 
107  inline static Flags selectTranslation(void) { return Flags("111000"); }
108  inline static Flags selectRotation(void) { return Flags("000111"); }
109 
110  virtual void display(std::ostream &os) const;
111 
112 public:
113  void servoCurrentPosition(void);
114 
115 private:
116  // Intermediate variables for internal computations
117  Eigen::Vector3d v_, omega_, errordot_t_, errordot_th_, Rreftomega_, t_, tref_;
118  VectorUTheta error_th_;
119  MatrixRotation R_, Rref_, Rt_, Rreft_;
120  Eigen::Matrix3d P_, Pinv_;
121  double accuracy_;
122  void inverseJacobianRodrigues();
123 };
124 
125 } /* namespace sot */
126 } /* namespace dynamicgraph */
127 
128 #endif // #ifndef __SOT_FEATURE_POINT6D_HH__
129 
130 /*
131  * Local variables:
132  * c-basic-offset: 2
133  * End:
134  */
Class that defines point-6d control feature.
Definition: feature-point6d.hh:49
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
Definition: feature-abstract.hh:227
Definition: feature-point6d.hh:59
static Flags selectRY(void)
Definition: feature-point6d.hh:104
static Flags selectRX(void)
Definition: feature-point6d.hh:103
static Flags selectZ(void)
Definition: feature-point6d.hh:102
ComputationFrameType
Definition: feature-point6d.hh:59
static Flags selectRZ(void)
Definition: feature-point6d.hh:105
SignalTimeDependent< dynamicgraph::Matrix, int > jacobianSOUT
Jacobian of the error wrt the robot state: .
Definition: feature-abstract.hh:192
virtual ~FeaturePoint6d(void)
Definition: feature-point6d.hh:88
static Flags selectX(void)
Definition: feature-point6d.hh:100
dynamicgraph::SignalPtr< MatrixHomogeneous, int > positionSIN
Definition: feature-point6d.hh:73
static Flags selectRotation(void)
Definition: feature-point6d.hh:108
static Flags selectY(void)
Definition: feature-point6d.hh:101
dynamicgraph::SignalPtr< dynamicgraph::Matrix, int > articularJacobianSIN
Definition: feature-point6d.hh:75
Eigen::AngleAxis< double > SOT_CORE_EXPORT VectorUTheta
Definition: matrix-geometry.hh:76
dynamicgraph::SignalPtr< dynamicgraph::Vector, int > velocitySIN
Definition: feature-point6d.hh:74
static Flags selectTranslation(void)
Definition: feature-point6d.hh:107
Definition: flags.hh:32
This class gives the abstract definition of a feature.
Definition: feature-abstract.hh:75
Eigen::Matrix< double, 3, 3 > SOT_CORE_EXPORT MatrixRotation
Definition: matrix-geometry.hh:75
static const std::string CLASS_NAME
Definition: feature-point6d.hh:54
#define DECLARE_REFERENCE_FUNCTIONS(FeatureSpecialized)
Definition: feature-abstract.hh:249
virtual const std::string & getClassName(void) const
Returns the name class.
Definition: feature-point6d.hh:55
#define SOTFEATUREPOINT6D_EXPORT
Definition: feature-point6d.hh:34
SignalTimeDependent< dynamicgraph::Vector, int > errorSOUT
This signal returns the error between the desired value and the current value : . ...
Definition: feature-abstract.hh:184
Definition: abstract-sot-external-interface.hh:17
static const ComputationFrameType COMPUTATION_FRAME_DEFAULT
Definition: feature-point6d.hh:60