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