10 #ifndef __SOT_FEATURE_POINT6D_HH__ 11 #define __SOT_FEATURE_POINT6D_HH__ 28 #if defined(feature_point6d_EXPORTS) 29 #define SOTFEATUREPOINT6D_EXPORT __declspec(dllexport) 31 #define SOTFEATUREPOINT6D_EXPORT __declspec(dllimport) 34 #define SOTFEATUREPOINT6D_EXPORT 50 "replaced by FeaturePose")]] SOTFEATUREPOINT6D_EXPORT FeaturePoint6d
51 :
public FeatureAbstract,
52 public FeatureReferenceHelper<FeaturePoint6d> {
54 static const std::string CLASS_NAME;
55 virtual const std::string &getClassName(
void)
const {
return CLASS_NAME; }
59 enum ComputationFrameType { FRAME_DESIRED, FRAME_CURRENT };
60 static const ComputationFrameType COMPUTATION_FRAME_DEFAULT;
64 void computationFrame(
const std::string &inFrame);
66 std::string computationFrame()
const;
69 ComputationFrameType computationFrame_;
73 dynamicgraph::SignalPtr<MatrixHomogeneous, int> positionSIN;
74 dynamicgraph::SignalPtr<dynamicgraph::Vector, int> velocitySIN;
75 dynamicgraph::SignalPtr<dynamicgraph::Matrix, int> articularJacobianSIN;
87 FeaturePoint6d(
const std::string &name);
88 virtual ~FeaturePoint6d(
void) {}
90 virtual unsigned int &getDimension(
unsigned int &dim,
int time);
92 virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector & res,
94 virtual dynamicgraph::Vector &computeErrordot(dynamicgraph::Vector & res,
96 virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix & res,
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"); }
107 inline static Flags selectTranslation(
void) {
return Flags(
"111000"); }
108 inline static Flags selectRotation(
void) {
return Flags(
"000111"); }
110 virtual void display(std::ostream & os)
const;
113 void servoCurrentPosition(
void);
117 Eigen::Vector3d v_, omega_, errordot_t_, errordot_th_, Rreftomega_, t_, tref_;
120 Eigen::Matrix3d P_, Pinv_;
122 void inverseJacobianRodrigues();
128 #endif // #ifndef __SOT_FEATURE_POINT6D_HH__ SignalPtr< Flags, int > selectionSIN
This vector specifies which dimension are used to perform the computation. For instance let us assume...
Definition: feature-abstract.hh:173
Definition: feature-point6d-relative.hh:50
SignalTimeDependent< dynamicgraph::Matrix, int > jacobianSOUT
Jacobian of the error wrt the robot state: .
Definition: feature-abstract.hh:193
Eigen::AngleAxis< double > SOT_CORE_EXPORT VectorUTheta
Definition: matrix-geometry.hh:77
Eigen::Matrix< double, 3, 3 > SOT_CORE_EXPORT MatrixRotation
Definition: matrix-geometry.hh:76
#define DECLARE_REFERENCE_FUNCTIONS(FeatureSpecialized)
Definition: feature-abstract.hh:251
SignalTimeDependent< dynamicgraph::Vector, int > errorSOUT
This signal returns the error between the desired value and the current value : . ...
Definition: feature-abstract.hh:185
Definition: abstract-sot-external-interface.hh:17