ViSP  3.0.0
servoMomentPolygon.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 
43 #include <visp3/core/vpDebug.h>
44 #include <visp3/core/vpConfig.h>
45 #include <iostream>
46 #include <visp3/core/vpHomogeneousMatrix.h>
47 #include <visp3/core/vpMomentObject.h>
48 #include <visp3/core/vpMomentDatabase.h>
49 #include <visp3/core/vpMomentCommon.h>
50 #include <visp3/visual_features/vpFeatureMomentCommon.h>
51 #include <visp3/gui/vpDisplayX.h>
52 #include <visp3/gui/vpDisplayGTK.h>
53 #include <visp3/gui/vpDisplayGDI.h>
54 #include <visp3/core/vpCameraParameters.h>
55 #include <visp3/core/vpIoTools.h>
56 #include <visp3/core/vpMath.h>
57 #include <visp3/core/vpHomogeneousMatrix.h>
58 #include <visp3/vs/vpServo.h>
59 #include <visp3/core/vpDebug.h>
60 #include <visp3/visual_features/vpFeatureBuilder.h>
61 #include <visp3/visual_features/vpFeaturePoint.h>
62 #include <visp3/robot/vpSimulatorAfma6.h>
63 #include <visp3/core/vpPlane.h>
64 
65 #if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD)
66 // Robot simulator used in this example is not available
67 int main()
68 {
69  std::cout << "Can't run this example since vpSimulatorAfma6 capability is not available." << std::endl;
70  std::cout << "You should install pthread third-party library." << std::endl;
71 }
72 // No display available
73 #elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && !defined(VISP_HAVE_GTK)
74 int main()
75 {
76  std::cout << "Can't run this example since no display capability is available." << std::endl;
77  std::cout << "You should install one of the following third-party library: X11, OpenCV, GDI, GTK." << std::endl;
78 }
79 #else
80 
81 //setup robot parameters
82 void paramRobot();
83 
84 //update moment objects and interface
85 void refreshScene(vpMomentObject &obj);
86 //initialize scene in the interface
87 void initScene();
88 //initialize the moment features
89 void initFeatures();
90 
91 void init(vpHomogeneousMatrix& cMo, vpHomogeneousMatrix& cdMo);
92 void execute(unsigned int nbIter); //launch the simulation
93 void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type);
94 double error();
95 void planeToABC(vpPlane& pl, double& A,double& B, double& C);
96 void paramRobot();
97 void removeJointLimits(vpSimulatorAfma6& robot);
98 
99 int main()
100 {
101  try { //intial pose
102  vpHomogeneousMatrix cMo(-0.1,-0.1,1.5,-vpMath::rad(20),-vpMath::rad(20),-vpMath::rad(30));
103  //Desired pose
105 
106  //init and run the simulation
107  init(cMo,cdMo);
108  execute(1500);
109  return 0;
110  }
111  catch(vpException e) {
112  std::cout << "Catch an exception: " << e << std::endl;
113  return 1;
114  }
115 }
116 
117 //init the right display
118 #if defined VISP_HAVE_X11
119 vpDisplayX displayInt;
120 #elif defined VISP_HAVE_OPENCV
121 vpDisplayOpenCV displayInt;
122 #elif defined VISP_HAVE_GDI
123 vpDisplayGDI displayInt;
124 #elif defined VISP_HAVE_D3D9
125 vpDisplayD3D displayInt;
126 #elif defined VISP_HAVE_GTK
127 vpDisplayGTK displayInt;
128 #endif
129 
130 //start and destination positioning matrices
133 
134 vpSimulatorAfma6 robot(false);//robot used in this simulation
135 vpImage<vpRGBa> Iint(480,640, 255);//internal image used for interface display
136 vpServo task; //servoing task
137 vpCameraParameters cam;//robot camera parameters
138 double _error; //current error
139 vpServo::vpServoIteractionMatrixType interaction_type; //current or desired
140 vpImageSimulator imsim;//image simulator used to simulate the perspective-projection camera
141 
142 //source and destination objects for moment manipulation
143 vpMomentObject src(6);
144 vpMomentObject dst(6);
145 //moment sets and their corresponding features
146 vpMomentCommon *moments;
147 vpMomentCommon *momentsDes;
148 vpFeatureMomentCommon *featureMoments;
149 vpFeatureMomentCommon *featureMomentsDes;
150 
151 using namespace std;
152 
153 
154 void initScene(){
155  vector<vpPoint> src_pts;
156  vector<vpPoint> dst_pts;
157 
158  double x[5] = { 0.2, 0.2,-0.2,-0.2, 0.2 };
159  double y[5] = {-0.1, 0.1, 0.1,-0.1,-0.1 };
160  int nbpoints = 4;
161 
162  for (int i = 0 ; i < nbpoints ; i++){
163  vpPoint p(x[i],y[i],0.0);
164  p.track(cMo) ;
165  src_pts.push_back(p);
166  }
167 
169  src.fromVector(src_pts);
170  for (int i = 0 ; i < nbpoints ; i++){
171  vpPoint p(x[i],y[i],0.0);
172  p.track(cdMo) ;
173  dst_pts.push_back(p);
174  }
176  dst.fromVector(dst_pts);
177 
178 }
179 
180 void refreshScene(vpMomentObject &obj){
181  double x[5] = { 0.2, 0.2,-0.2,-0.2, 0.2 };
182  double y[5] = {-0.1, 0.1, 0.1,-0.1,-0.1 };
183  int nbpoints = 5;
184  vector<vpPoint> cur_pts;
185 
186  for (int i = 0 ; i < nbpoints ; i++){
187  vpPoint p(x[i],y[i],0.0);
188  p.track(cMo) ;
189  cur_pts.push_back(p);
190  }
191  obj.fromVector(cur_pts);
192 }
193 
194 void init(vpHomogeneousMatrix& _cMo, vpHomogeneousMatrix& _cdMo)
195 {
196  cMo = _cMo;
197  cdMo = _cdMo;
198  interaction_type = vpServo::CURRENT;
199  displayInt.init(Iint,700,0, "Visual servoing with moments") ;
200 
201  paramRobot(); //set up robot parameters
202 
204  initScene(); //initialize graphical scene (for interface)
205  initFeatures();//initialize moment features
206 }
207 
208 void initFeatures(){
209  //A,B,C parameters of source and destination plane
210  double A; double B; double C;
211  double Ad; double Bd; double Cd;
212  //init main object: using moments up to order 6
213 
214  //Initializing values from regular plane (with ax+by+cz=d convention)
215  vpPlane pl;
216  pl.setABCD(0,0,1.0,0);
217  pl.changeFrame(cMo);
218  planeToABC(pl,A,B,C);
219 
220  pl.setABCD(0,0,1.0,0);
221  pl.changeFrame(cdMo);
222  planeToABC(pl,Ad,Bd,Cd);
223 
224  //extracting initial position (actually we only care about Zdst)
226  cdMo.extract(vec);
227 
229  //don't need to be specific, vpMomentCommon automatically loads Xg,Yg,An,Ci,Cj,Alpha moments
232  //same thing with common features
233  featureMoments = new vpFeatureMomentCommon(*moments);
234  featureMomentsDes = new vpFeatureMomentCommon(*momentsDes);
235 
236  moments->updateAll(src);
237  momentsDes->updateAll(dst);
238 
239  featureMoments->updateAll(A,B,C);
240  featureMomentsDes->updateAll(Ad,Bd,Cd);
241 
242  //setup the interaction type
243  task.setInteractionMatrixType(interaction_type) ;
245  task.addFeature(featureMoments->getFeatureGravityNormalized(),featureMomentsDes->getFeatureGravityNormalized());
246  task.addFeature(featureMoments->getFeatureAn(),featureMomentsDes->getFeatureAn());
247  //the moments are different in case of a symmetric object
248  task.addFeature(featureMoments->getFeatureCInvariant(),featureMomentsDes->getFeatureCInvariant(),(1 << 10) | (1 << 11));
249  task.addFeature(featureMoments->getFeatureAlpha(),featureMomentsDes->getFeatureAlpha());
250 
251  task.setLambda(0.4) ;
252 }
253 
254 void execute(unsigned int nbIter){
255  //init main object: using moments up to order 5
256  vpMomentObject obj(6);
257  //setting object type (disrete, continuous[form polygon])
259 
260  vpTRACE("Display task information " ) ;
261  task.print() ;
262 
263  vpDisplay::display(Iint);
264  robot.getInternalView(Iint);
265  vpDisplay::flush(Iint);
266  unsigned int iter=0;
267  double t=0;
269  while(iter++<nbIter ){
270  vpColVector v ;
271  t = vpTime::measureTimeMs();
272  //get the cMo
273  cMo = robot.get_cMo();
274  //setup the plane in A,B,C style
275  vpPlane pl;
276  double A,B,C;
277  pl.setABCD(0,0,1.0,0);
278  pl.changeFrame(cMo);
279  planeToABC(pl,A,B,C);
280 
281  //track points, draw points and add refresh our object
282  refreshScene(obj);
283  //this is the most important thing to do: update our moments
284  moments->updateAll(obj);
285  //and update our features. Do it in that order. Features need to use the information computed by moments
286  featureMoments->updateAll(A,B,C);
287 
288  vpDisplay::display(Iint) ;
289  robot.getInternalView(Iint);
290  vpDisplay::flush(Iint);
291 
292  if (iter == 1)
293  vpDisplay::getClick(Iint) ;
294  v = task.computeControlLaw() ;
295 
296  //pilot robot using position control. The displacement is t*v with t=10ms step
297  robot.setPosition(vpRobot::CAMERA_FRAME,0.01*v);
298 
299  vpTime::wait(t,10);
300  _error = ( task.getError() ).sumSquare();
301  }
302 
303  task.kill();
304 
305  vpTRACE("\n\nClick in the internal view window to end...");
306  vpDisplay::getClick(Iint) ;
307 
308  delete moments;
309  delete momentsDes;
310  delete featureMoments;
311  delete featureMomentsDes;
312 }
313 
314 void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type){interaction_type=type;}
315 double error(){return _error;}
316 
317 void removeJointLimits(vpSimulatorAfma6& robot_)
318 {
319  vpColVector limMin(6);
320  vpColVector limMax(6);
321  limMin[0] = vpMath::rad(-3600);
322  limMin[1] = vpMath::rad(-3600);
323  limMin[2] = vpMath::rad(-3600);
324  limMin[3] = vpMath::rad(-3600);
325  limMin[4] = vpMath::rad(-3600);
326  limMin[5] = vpMath::rad(-3600);
327 
328  limMax[0] = vpMath::rad(3600);
329  limMax[1] = vpMath::rad(3600);
330  limMax[2] = vpMath::rad(3600);
331  limMax[3] = vpMath::rad(3600);
332  limMax[4] = vpMath::rad(3600);
333  limMax[5] = vpMath::rad(3600);
334 
335  robot_.setJointLimit(limMin,limMax);
336 }
337 
338 void planeToABC(vpPlane& pl, double& A,double& B, double& C){
339  if(fabs(pl.getD())<std::numeric_limits<double>::epsilon()){
340  std::cout << "Invalid position:" << std::endl;
341  std::cout << cMo << std::endl;
342  std::cout << "Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
343  throw vpException(vpException::divideByZeroError,"invalid position!");
344  }
345  A=-pl.getA()/pl.getD();
346  B=-pl.getB()/pl.getD();
347  C=-pl.getC()/pl.getD();
348 }
349 
350 void paramRobot(){
351  /*Initialise the robot and especially the camera*/
353  robot.setCurrentViewColor(vpColor(150,150,150));
354  robot.setDesiredViewColor(vpColor(200,200,200));
356  removeJointLimits(robot);
358  /*Initialise the position of the object relative to the pose of the robot's camera*/
359  robot.initialiseObjectRelativeToCamera(cMo);
360 
361  /*Set the desired position (for the displaypart)*/
362  robot.setDesiredCameraPosition(cdMo);
363  robot.getCameraParameters(cam,Iint);
364 }
365 
366 #endif
void setPosition(const vpHomogeneousMatrix &wMc)
The object displayed at the desired position is the same than the scene object defined in vpSceneObje...
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:150
Perspective projection without distortion model.
Implementation of an homogeneous matrix and operations on such kind of matrices.
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
Define the X11 console to display images.
Definition: vpDisplayX.h:148
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.
Initialize the position controller.
Definition: vpRobot.h:69
error that can be emited by ViSP classes.
Definition: vpException.h:73
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
void setABCD(const double a, const double b, const double c, const double d)
Definition: vpPlane.h:93
Class for generic objects.
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const char *title=NULL)
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2233
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
Class that defines what is a point.
Definition: vpPoint.h:59
Display for windows using Direct3D.
Definition: vpDisplayD3D.h:105
void kill()
Definition: vpServo.cpp:186
vpColVector getError() const
Definition: vpServo.h:271
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
#define vpTRACE
Definition: vpDebug.h:414
static std::vector< double > getMu3(vpMomentObject &object)
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:206
The vpDisplayOpenCV allows to display image using the opencv library.
Generic class defining intrinsic camera parameters.
void setLambda(double c)
Definition: vpServo.h:390
Class which enables to project an image in the 3D space and get the view of a virtual camera...
Simulator of Irisa&#39;s gantry robot named Afma6.
The vpDisplayGTK allows to display image using the GTK+ library version 1.2.
Definition: vpDisplayGTK.h:141
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
A 40cm by 40cm plate with 4 points at coordinates (-0.07,-0.05,0), (0.07,0.05,0), (0...
void updateAll(vpMomentObject &object)
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:93
This class initializes and allows access to commonly used moments.
static double getAlpha(vpMomentObject &object)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
vpFeatureMomentGravityCenterNormalized & getFeatureGravityNormalized()
double getB() const
Definition: vpPlane.h:108
void setType(vpObjectType input_type)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:248
double getA() const
Definition: vpPlane.h:106
double getC() const
Definition: vpPlane.h:110
virtual bool getClick(bool blocking=true)=0
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