GCC Code Coverage Report


Directory: ./
File: include/sot/core/feature-point6d.hh
Date: 2024-12-13 12:22:33
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