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  */
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::FeatureReferenceHelper
Definition: feature-abstract.hh:225
dynamicgraph
Definition: abstract-sot-external-interface.hh:17
dynamicgraph::sot::FLAG_LINE_5
const SOT_CORE_EXPORT Flags FLAG_LINE_5
dynamicgraph::sot::FeaturePoint6d::selectTranslation
static Flags selectTranslation(void)
Definition: feature-point6d.hh:105
dynamicgraph::sot::FeaturePoint6d::ComputationFrameType
ComputationFrameType
Definition: feature-point6d.hh:60
dynamicgraph::sot::FeaturePoint6d::selectRZ
static Flags selectRZ(void)
Definition: feature-point6d.hh:103
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
DECLARE_REFERENCE_FUNCTIONS
#define DECLARE_REFERENCE_FUNCTIONS(FeatureSpecialized)
Definition: feature-abstract.hh:247
feature-abstract.hh
dynamicgraph::sot::FeaturePoint6d::selectRX
static Flags selectRX(void)
Definition: feature-point6d.hh:101
dynamicgraph::sot::FeatureAbstract
This class gives the abstract definition of a feature.
Definition: feature-abstract.hh:76
dynamicgraph::sot::FeaturePoint6d::positionSIN
dg::SignalPtr< MatrixHomogeneous, int > positionSIN
Definition: feature-point6d.hh:74
dynamicgraph::sot::FeaturePoint6d::selectX
static Flags selectX(void)
Definition: feature-point6d.hh:98
dynamicgraph::sot::FeaturePoint6d::FRAME_DESIRED
@ FRAME_DESIRED
Definition: feature-point6d.hh:60
dynamicgraph::sot::VectorUTheta
Eigen::AngleAxis< double > SOT_CORE_EXPORT VectorUTheta
Definition: matrix-geometry.hh:76
dynamicgraph::sot::FeaturePoint6d::selectZ
static Flags selectZ(void)
Definition: feature-point6d.hh:100
exception-feature.hh
dynamicgraph::sot::FeaturePoint6d::getClassName
virtual const std::string & getClassName(void) const
Returns the name class.
Definition: feature-point6d.hh:56
dynamicgraph::sot::FeaturePoint6d::articularJacobianSIN
dg::SignalPtr< dg::Matrix, int > articularJacobianSIN
Definition: feature-point6d.hh:76
dynamicgraph::sot::FeaturePoint6d::COMPUTATION_FRAME_DEFAULT
static const ComputationFrameType COMPUTATION_FRAME_DEFAULT
Definition: feature-point6d.hh:61
matrix-geometry.hh
exception-task.hh
SOTFEATUREPOINT6D_EXPORT
#define SOTFEATUREPOINT6D_EXPORT
Definition: feature-point6d.hh:34
dynamicgraph::sot::FLAG_LINE_4
const SOT_CORE_EXPORT Flags FLAG_LINE_4
dynamicgraph::sot::FeaturePoint6d::selectY
static Flags selectY(void)
Definition: feature-point6d.hh:99
dynamicgraph::sot::FeaturePoint6d::selectRY
static Flags selectRY(void)
Definition: feature-point6d.hh:102
dynamicgraph::sot::MatrixRotation
Eigen::Matrix< double, 3, 3 > SOT_CORE_EXPORT MatrixRotation
Definition: matrix-geometry.hh:75
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::FeaturePoint6d::selectRotation
static Flags selectRotation(void)
Definition: feature-point6d.hh:106
dynamicgraph::sot::FeaturePoint6d::velocitySIN
dg::SignalPtr< dg::Vector, int > velocitySIN
Definition: feature-point6d.hh:75
dynamicgraph::sot::FeaturePoint6d::CLASS_NAME
static const std::string CLASS_NAME
Definition: feature-point6d.hh:55
dynamicgraph::sot::FeaturePoint6d::~FeaturePoint6d
virtual ~FeaturePoint6d(void)
Definition: feature-point6d.hh:89
dynamicgraph::sot::FLAG_LINE_2
const SOT_CORE_EXPORT Flags FLAG_LINE_2
dynamicgraph::sot::FLAG_LINE_1
const SOT_CORE_EXPORT Flags FLAG_LINE_1
dynamicgraph::sot::FeaturePoint6d
Class that defines point-6d control feature.
Definition: feature-point6d.hh:50