GCC Code Coverage Report

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