Directory: | ./ |
---|---|
File: | include/sot/core/feature-point6d.hh |
Date: | 2024-08-13 12:13:25 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 2 | 3 | 66.7% |
Branches: | 2 | 6 | 33.3% |
Line | Branch | Exec | Source |
---|---|---|---|
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 */ | ||
18 | #include <sot/core/exception-feature.hh> | ||
19 | #include <sot/core/exception-task.hh> | ||
20 | #include <sot/core/feature-abstract.hh> | ||
21 | #include <sot/core/matrix-geometry.hh> | ||
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 | |||
44 | /*! | ||
45 | \class FeaturePoint6d | ||
46 | \brief Class that defines point-6d control feature. | ||
47 | \deprecated This class was replaced by FeaturePose. | ||
48 | */ | ||
49 | class [[deprecated( | ||
50 | "replaced by FeaturePose")]] SOTFEATUREPOINT6D_EXPORT FeaturePoint6d | ||
51 | : public FeatureAbstract, | ||
52 | public FeatureReferenceHelper<FeaturePoint6d> { | ||
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 }; | ||
60 | static const ComputationFrameType COMPUTATION_FRAME_DEFAULT; | ||
61 | |||
62 | public: | ||
63 | /// \brief Set computation frame | ||
64 | void computationFrame(const std::string &inFrame); | ||
65 | /// \brief Get computation frame | ||
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 | |||
77 | using FeatureAbstract::errorSOUT; | ||
78 | using FeatureAbstract::jacobianSOUT; | ||
79 | using FeatureAbstract::selectionSIN; | ||
80 | |||
81 | /*! \name Dealing with the reference value to be reach with this feature. | ||
82 | @{ */ | ||
83 |
2/6✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
|
19 | DECLARE_REFERENCE_FUNCTIONS(FeaturePoint6d); |
84 | /*! @} */ | ||
85 | |||
86 | public: | ||
87 | FeaturePoint6d(const std::string &name); | ||
88 | 4 | 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 | |||
99 | /** Static Feature selection. */ | ||
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 | */ | ||
135 |