ViSP  3.0.0
testFeatureMoment.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Example of visual servoing with moments using a polygon as object container
32  *
33  * Authors:
34  * Filip Novotny
35  *
36  *****************************************************************************/
37 #include <visp3/core/vpMomentObject.h>
38 #include <visp3/core/vpMomentDatabase.h>
39 #include <visp3/core/vpMomentCommon.h>
40 #include <visp3/visual_features/vpFeatureMomentCommon.h>
41 #include <visp3/core/vpHomogeneousMatrix.h>
42 #include <visp3/vs/vpServo.h>
43 #include <visp3/core/vpDebug.h>
44 #include <visp3/core/vpPlane.h>
45 
46 #include <limits>
47 #include <iostream>
48 
49 //initialize scene in the interface
50 void initScene(const vpHomogeneousMatrix& cMo, const vpHomogeneousMatrix& cdMo,
51  vpMomentObject &src, vpMomentObject &dst);
52 
53 vpMatrix execute(const vpHomogeneousMatrix& cMo, const vpHomogeneousMatrix& cdMo,
54  vpMomentObject &src, vpMomentObject &dst); //launch the test
55 void planeToABC(const vpPlane& pl, double& A,double& B, double& C);
56 int test(double x,double y,double z,double alpha);
57 
58 //Compute a set of parallel positions and check if the matrix is in the right form;
59 int main()
60 {
61  try {
62  int sum=0;
63  for(double i=-0.2;i<0.2;i+=0.1){
64  for(double j=-0.2;j<0.2;j+=0.1){
65  for(double k=-vpMath::rad(30);k<vpMath::rad(30);k+=vpMath::rad(10)){
66  for(double l=0.5;l<1.5;l+=0.1){
67  sum+=test(i,j,l,k);
68  }
69  }
70 
71  }
72  }
73  if(sum<0) return -1;
74  else return 0;
75  }
76  catch(vpException e) {
77  std::cout << "Catch an exception: " << e << std::endl;
78  return 1;
79  }
80 }
81 
82 int test(double x,double y,double z,double alpha){
83  //intial pose
84  vpHomogeneousMatrix cMo(x,y,z,-vpMath::rad(0),vpMath::rad(0),alpha);
85  //Desired pose
87 
88  //source and destination objects for moment manipulation
89  vpMomentObject src(6);
90  vpMomentObject dst(6);
91 
92  //init and run the simulation
93  initScene(cMo, cdMo, src, dst); //initialize graphical scene (for interface)
94 
95  vpMatrix mat = execute(cMo, cdMo, src, dst);
96 
97  if(fabs(mat[0][0]-(-1)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
98  if(fabs(mat[0][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
99  if(fabs(mat[0][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
100 
101  if(fabs(mat[1][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
102  if(fabs(mat[1][1]-(-1)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
103  if(fabs(mat[1][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
104 
105  if(fabs(mat[2][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
106  if(fabs(mat[2][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
107  if(fabs(mat[2][2]-(-1)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
108  if(fabs(mat[2][5]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
109 
110  if(fabs(mat[3][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
111  if(fabs(mat[3][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
112  if(fabs(mat[3][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
113  if(fabs(mat[3][5]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
114 
115  if(fabs(mat[4][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
116  if(fabs(mat[4][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
117  if(fabs(mat[4][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
118  if(fabs(mat[4][5]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
119 
120  if(fabs(mat[5][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
121  if(fabs(mat[5][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
122  if(fabs(mat[5][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
123  if(fabs(mat[5][5]-(-1)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
124 
125  return 0;
126 }
127 
128 void initScene(const vpHomogeneousMatrix& cMo, const vpHomogeneousMatrix& cdMo,
129  vpMomentObject &src, vpMomentObject &dst)
130 {
131  std::vector<vpPoint> src_pts;
132  std::vector<vpPoint> dst_pts;
133 
134  double x[5] = { 0.2, 0.2,-0.2,-0.2, 0.2 };
135  double y[5] = {-0.1, 0.1, 0.1,-0.1,-0.1 };
136  int nbpoints = 4;
137 
138  for (int i = 0 ; i < nbpoints ; i++){
139  vpPoint p(x[i],y[i],0.0);
140  p.track(cMo) ;
141  src_pts.push_back(p);
142  }
143 
145  src.fromVector(src_pts);
146  for (int i = 0 ; i < nbpoints ; i++){
147  vpPoint p(x[i],y[i],0.0);
148  p.track(cdMo) ;
149  dst_pts.push_back(p);
150  }
152  dst.fromVector(dst_pts);
153 }
154 
155 
156 vpMatrix execute(const vpHomogeneousMatrix& cMo, const vpHomogeneousMatrix& cdMo,
157  vpMomentObject &src, vpMomentObject &dst)
158 {
159  vpServo::vpServoIteractionMatrixType interaction_type = vpServo::CURRENT; ; //current or desired
160 
161  vpServo task;
163  //A,B,C parameters of source and destination plane
164  double A; double B; double C;
165  double Ad; double Bd; double Cd;
166  //init main object: using moments up to order 6
167 
168  //Initializing values from regular plane (with ax+by+cz=d convention)
169  vpPlane pl;
170  pl.setABCD(0,0,1.0,0);
171  pl.changeFrame(cMo);
172  planeToABC(pl,A,B,C);
173 
174  pl.setABCD(0,0,1.0,0);
175  pl.changeFrame(cdMo);
176  planeToABC(pl,Ad,Bd,Cd);
177 
178  //extracting initial position (actually we only care about Zdst)
180  cdMo.extract(vec);
181 
183  //don't need to be specific, vpMomentCommon automatically loads Xg,Yg,An,Ci,Cj,Alpha moments
186  //same thing with common features
187  vpFeatureMomentCommon featureMoments(moments);
188  vpFeatureMomentCommon featureMomentsDes(momentsDes);
189 
190  moments.updateAll(src);
191  momentsDes.updateAll(dst);
192 
193  featureMoments.updateAll(A,B,C);
194  featureMomentsDes.updateAll(Ad,Bd,Cd);
195 
196  //setup the interaction type
197  task.setInteractionMatrixType(interaction_type) ;
199  task.addFeature(featureMoments.getFeatureGravityNormalized(),featureMomentsDes.getFeatureGravityNormalized());
200  task.addFeature(featureMoments.getFeatureAn(),featureMomentsDes.getFeatureAn());
201  //the moments are different in case of a symmetric object
202  task.addFeature(featureMoments.getFeatureCInvariant(),featureMomentsDes.getFeatureCInvariant(),(1 << 10) | (1 << 11));
203  task.addFeature(featureMoments.getFeatureAlpha(),featureMomentsDes.getFeatureAlpha());
204 
205  task.setLambda(0.4) ;
206 
207  task.computeControlLaw();
208  vpMatrix mat = task.computeInteractionMatrix();
209  task.kill();
210  return mat;
211 }
212 
213 
214 void planeToABC(const vpPlane& pl, double& A,double& B, double& C)
215 {
216  A=-pl.getA()/pl.getD();
217  B=-pl.getB()/pl.getD();
218  C=-pl.getC()/pl.getD();
219 }
220 
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:92
Implementation of an homogeneous matrix and operations on such kind of matrices.
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:446
This class allows to access common vpFeatureMoments in a pre-filled database.
error that can be emited by ViSP classes.
Definition: vpException.h:73
void setABCD(const double a, const double b, const double c, const double d)
Definition: vpPlane.h:93
Class for generic objects.
Class that defines what is a point.
Definition: vpPoint.h:59
vpMatrix computeInteractionMatrix()
Definition: vpServo.cpp:604
void kill()
Definition: vpServo.cpp:186
vpColVector computeControlLaw()
Definition: vpServo.cpp:899
vpFeatureMomentAlpha & getFeatureAlpha()
void updateAll(double A, double B, double C)
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:372
static std::vector< double > getMu3(vpMomentObject &object)
void setLambda(double c)
Definition: vpServo.h:390
void extract(vpRotationMatrix &R) const
vpServoIteractionMatrixType
Definition: vpServo.h:183
void fromVector(std::vector< vpPoint > &points)
static double getSurface(vpMomentObject &object)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:519
vpFeatureMomentCInvariant & getFeatureCInvariant()
static double rad(double deg)
Definition: vpMath.h:104
void updateAll(vpMomentObject &object)
This class initializes and allows access to commonly used moments.
static double getAlpha(vpMomentObject &object)
vpFeatureMomentGravityCenterNormalized & getFeatureGravityNormalized()
double getB() const
Definition: vpPlane.h:108
void setType(vpObjectType input_type)
double getA() const
Definition: vpPlane.h:106
double getC() const
Definition: vpPlane.h:110
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:58
vpFeatureMomentAreaNormalized & getFeatureAn()
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:217
Class that consider the case of a translation vector.
double getD() const
Definition: vpPlane.h:112