feature-point6d.hh 4.3 KB
Newer Older
Thomas Moulard's avatar
Thomas Moulard committed
1
2
3
4
/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
5
 *
Thomas Moulard's avatar
Thomas Moulard committed
6
 * CNRS/AIST
7
 *
Thomas Moulard's avatar
Thomas Moulard committed
8
 */
9
10
11
12
13
14
15
16
17

#ifndef __SOT_FEATURE_POINT6D_HH__
#define __SOT_FEATURE_POINT6D_HH__

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/* SOT */
18
#include <sot/core/exception-feature.hh>
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
19
20
#include <sot/core/exception-task.hh>
#include <sot/core/feature-abstract.hh>
21
#include <sot/core/matrix-geometry.hh>
22
23
24
25
26

/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
27
28
29
#if defined(WIN32)
#if defined(feature_point6d_EXPORTS)
#define SOTFEATUREPOINT6D_EXPORT __declspec(dllexport)
30
#else
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
31
32
33
34
#define SOTFEATUREPOINT6D_EXPORT __declspec(dllimport)
#endif
#else
#define SOTFEATUREPOINT6D_EXPORT
35
36
37
38
39
40
#endif

/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
41
42
namespace dynamicgraph {
namespace sot {
43
44

/*!
45
  \class FeaturePoint6d
46
47
  \brief Class that defines point-6d control feature.
  \deprecated This class was replaced by FeaturePose.
48
*/
Guilhem Saurel's avatar
Guilhem Saurel committed
49
50
51
class[[deprecated("replaced by FeaturePose")]] SOTFEATUREPOINT6D_EXPORT
    FeaturePoint6d : public FeatureAbstract,
                     public FeatureReferenceHelper<FeaturePoint6d> {
52

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
53
public:
54
  static const std::string CLASS_NAME;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
55
  virtual const std::string &getClassName(void) const { return CLASS_NAME; }
56

57
  /* --- Frame type --------------------------------------------------------- */
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
58
59
protected:
  enum ComputationFrameType { FRAME_DESIRED, FRAME_CURRENT };
60
  static const ComputationFrameType COMPUTATION_FRAME_DEFAULT;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
61
62

public:
63
  /// \brief Set computation frame
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
64
  void computationFrame(const std::string &inFrame);
65
  /// \brief Get computation frame
66
  std::string computationFrame() const;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
67
68

private:
69
  ComputationFrameType computationFrame_;
70
71

  /* --- SIGNALS ------------------------------------------------------------ */
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
72
public:
Olivier Stasse's avatar
Olivier Stasse committed
73
74
75
  dynamicgraph::SignalPtr<MatrixHomogeneous, int> positionSIN;
  dynamicgraph::SignalPtr<dynamicgraph::Vector, int> velocitySIN;
  dynamicgraph::SignalPtr<dynamicgraph::Matrix, int> articularJacobianSIN;
76

77
  using FeatureAbstract::errorSOUT;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
78
79
  using FeatureAbstract::jacobianSOUT;
  using FeatureAbstract::selectionSIN;
80
81
82
83
84

  /*! \name Dealing with the reference value to be reach with this feature.
    @{  */
  DECLARE_REFERENCE_FUNCTIONS(FeaturePoint6d);
  /*! @} */
85

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
86
87
88
public:
  FeaturePoint6d(const std::string &name);
  virtual ~FeaturePoint6d(void) {}
89

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
90
  virtual unsigned int &getDimension(unsigned int &dim, int time);
Thomas Moulard's avatar
Thomas Moulard committed
91

Maximilien Naveau's avatar
Maximilien Naveau committed
92
  virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector & res,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
93
                                             int time);
Maximilien Naveau's avatar
Maximilien Naveau committed
94
  virtual dynamicgraph::Vector &computeErrordot(dynamicgraph::Vector & res,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
95
                                                int time);
Maximilien Naveau's avatar
Maximilien Naveau committed
96
  virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix & res,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
97
                                                int time);
98
99

  /** Static Feature selection. */
Joseph Mirabel's avatar
Joseph Mirabel committed
100
101
102
103
104
105
106
107
108
  inline static Flags selectX(void) { return Flags("100000"); }
  inline static Flags selectY(void) { return Flags("010000"); }
  inline static Flags selectZ(void) { return Flags("001000"); }
  inline static Flags selectRX(void) { return Flags("000100"); }
  inline static Flags selectRY(void) { return Flags("000010"); }
  inline static Flags selectRZ(void) { return Flags("000001"); }

  inline static Flags selectTranslation(void) { return Flags("111000"); }
  inline static Flags selectRotation(void) { return Flags("000111"); }
109

Maximilien Naveau's avatar
Maximilien Naveau committed
110
  virtual void display(std::ostream & os) const;
111

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
112
113
public:
  void servoCurrentPosition(void);
114

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
115
private:
116
  // Intermediate variables for internal computations
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
117
118
  Eigen::Vector3d v_, omega_, errordot_t_, errordot_th_, Rreftomega_, t_, tref_;
  VectorUTheta error_th_;
119
  MatrixRotation R_, Rref_, Rt_, Rreft_;
120
  Eigen::Matrix3d P_, Pinv_;
121
  double accuracy_;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
122
123
  void inverseJacobianRodrigues();
};
124

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
125
126
} /* namespace sot */
} /* namespace dynamicgraph */
127
128
129
130
131
132
133
134

#endif // #ifndef __SOT_FEATURE_POINT6D_HH__

/*
 * Local variables:
 * c-basic-offset: 2
 * End:
 */