ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
servoMomentPoints.cpp
1 /****************************************************************************
2  *
3  * $Id: servoMomentPoints.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Example of visual servoing with moments using discrete points as object
36  * container
37  *
38  * Authors:
39  * Filip Novotny
40  *
41  *****************************************************************************/
42 
48 #include <visp/vpDebug.h>
49 #include <visp/vpConfig.h>
50 #include <iostream>
51 #include <visp/vpHomogeneousMatrix.h>
52 #include <visp/vpMomentObject.h>
53 #include <visp/vpMomentDatabase.h>
54 #include <visp/vpMomentCommon.h>
55 #include <visp/vpFeatureMomentCommon.h>
56 #include <visp/vpDisplayX.h>
57 #include <visp/vpDisplayGTK.h>
58 #include <visp/vpDisplayGDI.h>
59 #include <visp/vpCameraParameters.h>
60 #include <visp/vpIoTools.h>
61 #include <visp/vpMath.h>
62 #include <visp/vpHomogeneousMatrix.h>
63 #include <visp/vpServo.h>
64 #include <visp/vpDebug.h>
65 #include <visp/vpFeatureBuilder.h>
66 #include <visp/vpFeaturePoint.h>
67 #include <visp/vpSimulatorAfma6.h>
68 #include <visp/vpPlane.h>
69 
70 //setup robot parameters
71 void paramRobot();
72 
73 //update moment objects and interface
74 void refreshScene(vpMomentObject &obj);
75 //initialize scene in the interface
76 void initScene();
77 //initialize the moment features
78 void initFeatures();
79 
80 void init(vpHomogeneousMatrix& cMo, vpHomogeneousMatrix& cdMo);
81 void execute(unsigned int nbIter); //launch the simulation
82 void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type);
83 double error();
84 void _planeToABC(vpPlane& pl, double& A,double& B, double& C);
85 void paramRobot();
86 
87 #if !defined(WIN32) && !defined(VISP_HAVE_PTHREAD)
88 // Robot simulator used in this example is not available
89 int main()
90 {
91  std::cout << "Can't run this example since vpSimulatorAfma6 capability is not available." << std::endl;
92  std::cout << "You should install pthread third-party library." << std::endl;
93 }
94 // No display available
95 #elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && !defined(VISP_HAVE_GTK)
96 int main()
97 {
98  std::cout << "Can't run this example since no display capability is available." << std::endl;
99  std::cout << "You should install one of the following third-party library: X11, OpenCV, GDI, GTK." << std::endl;
100 }
101 #else
102 int main(){
103  //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 
114 //init the right display
115 #if defined VISP_HAVE_X11
116 vpDisplayX displayInt;
117 #elif defined VISP_HAVE_OPENCV
118 vpDisplayOpenCV displayInt;
119 #elif defined VISP_HAVE_GDI
120 vpDisplayGDI displayInt;
121 #elif defined VISP_HAVE_D3D9
122 vpDisplayD3D displayInt;
123 #elif defined VISP_HAVE_GTK
124 vpDisplayGTK displayInt;
125 #endif
126 
127 //start and destination positioning matrices
130 
131 vpSimulatorAfma6 robot(false);//robot used in this simulation
132 vpImage<vpRGBa> Iint(480,640, 255);//internal image used for interface display
133 vpServo::vpServoIteractionMatrixType interaction_type; //current or desired
134 vpServo task; //servoing task
135 vpCameraParameters cam;//robot camera parameters
136 double _error; //current error
137 vpImageSimulator imsim;//image simulator used to simulate the perspective-projection camera
138 
139 //moment sets and their corresponding features
140 vpMomentCommon *moments;
141 vpMomentCommon *momentsDes;
142 vpFeatureMomentCommon *featureMoments;
143 vpFeatureMomentCommon *featureMomentsDes;
144 
145 //source and destination objects for moment manipulation
146 vpMomentObject src(6);
147 vpMomentObject dst(6);
148 
149 using namespace std;
150 
151 
152 void initScene(){
153  vector<vpPoint> src_pts;
154  vector<vpPoint> dst_pts;
155 
156  double x[8] = { 1,3, 4,-1 ,-3,-2,-1,1};
157  double y[8] = { 0,1, 4, 4, -2,-2, 1,0};
158  int nbpoints = 8;
159 
160  for (int i = 0 ; i < nbpoints ; i++){
161  vpPoint p;
162  p.setWorldCoordinates(x[i]/20,y[i]/20,0.0);
163  p.track(cMo) ;
164  src_pts.push_back(p);
165  }
166 
168  src.fromVector(src_pts);
169  for (int i = 0 ; i < nbpoints ; i++){
170  vpPoint p;
171  p.setWorldCoordinates(x[i]/20,y[i]/20,0.0);
172  p.track(cdMo) ;
173  dst_pts.push_back(p);
174  }
176  dst.fromVector(dst_pts);
177 
178 }
179 
180 void initFeatures(){
181  //A,B,C parameters of source and destination plane
182  double A; double B; double C;
183  double Ad; double Bd; double Cd;
184  //init main object: using moments up to order 6
185 
186  //Initializing values from regular plane (with ax+by+cz=d convention)
187  vpPlane pl;
188  pl.setABCD(0,0,1.0,0);
189  pl.changeFrame(cMo);
190  _planeToABC(pl,A,B,C);
191 
192  pl.setABCD(0,0,1.0,0);
193  pl.changeFrame(cdMo);
194  _planeToABC(pl,Ad,Bd,Cd);
195 
196  //extracting initial position (actually we only care about Zdst)
198  cdMo.extract(vec);
199 
201  //don't need to be specific, vpMomentCommon automatically loads Xg,Yg,An,Ci,Cj,Alpha moments
204  //same thing with common features
205  featureMoments = new vpFeatureMomentCommon(*moments);
206  featureMomentsDes = new vpFeatureMomentCommon(*momentsDes);
207 
208  moments->updateAll(src);
209  momentsDes->updateAll(dst);
210 
211  featureMoments->updateAll(A,B,C);
212  featureMomentsDes->updateAll(Ad,Bd,Cd);
213 
214  //setup the interaction type
215  task.setInteractionMatrixType(interaction_type) ;
217  task.addFeature(featureMoments->getFeatureGravityNormalized(),featureMomentsDes->getFeatureGravityNormalized());
218  task.addFeature(featureMoments->getFeatureAn(),featureMomentsDes->getFeatureAn());
219  task.addFeature(featureMoments->getFeatureCInvariant(),featureMomentsDes->getFeatureCInvariant(),(1 << 3) | (1 << 5));
220  task.addFeature(featureMoments->getFeatureAlpha(),featureMomentsDes->getFeatureAlpha());
221 
222  task.setLambda(1.) ;
223 }
224 
225 
226 void refreshScene(vpMomentObject &obj){
227  //double x[8] = { 0.05,0.15, 0.2,-0.05 ,-0.15,-0.1,-0.05,0.05};
228  //double y[8] = { 0,0.05, 0.2, 0.2, -0.1,-0.1, 0.05,0};
229  double x[8] = { 1,3, 4,-1 ,-3,-2,-1,1};
230  double y[8] = { 0,1, 4, 4, -2,-2, 1,0};
231  int nbpoints = 8;
232  vector<vpPoint> cur_pts;
233 
234  for (int i = 0 ; i < nbpoints ; i++){
235  vpPoint p;
236  p.setWorldCoordinates(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  vpColVector limMin(6);
321  vpColVector limMax(6);
322  limMin[0] = vpMath::rad(-3600);
323  limMin[1] = vpMath::rad(-3600);
324  limMin[2] = vpMath::rad(-3600);
325  limMin[3] = vpMath::rad(-3600);
326  limMin[4] = vpMath::rad(-3600);
327  limMin[5] = vpMath::rad(-3600);
328 
329  limMax[0] = vpMath::rad(3600);
330  limMax[1] = vpMath::rad(3600);
331  limMax[2] = vpMath::rad(3600);
332  limMax[3] = vpMath::rad(3600);
333  limMax[4] = vpMath::rad(3600);
334  limMax[5] = vpMath::rad(3600);
335 
336  robot.setJointLimit(limMin,limMax);
337  robot.setMaxRotationVelocity(99999);
338  robot.setMaxTranslationVelocity(999999);
339 }
340 
341 void _planeToABC(vpPlane& pl, double& A,double& B, double& C){
342  if(fabs(pl.getD())<std::numeric_limits<double>::epsilon()){
343  std::cout << "Invalid position:" << std::endl;
344  std::cout << cMo << std::endl;
345  std::cout << "Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
346  throw vpException(vpException::divideByZeroError,"invalid position!");
347  }
348  A=-pl.getA()/pl.getD();
349  B=-pl.getB()/pl.getD();
350  C=-pl.getC()/pl.getD();
351 }
352 
353 void paramRobot(){
354  /*Initialise the robot and especially the camera*/
356  robot.setCurrentViewColor(vpColor(150,150,150));
357  robot.setDesiredViewColor(vpColor(200,200,200));
359  removeJointLimits(robot);
361  robot.setConstantSamplingTimeMode(true);
362  /*Initialise the position of the object relative to the pose of the robot's camera*/
364 
365  /*Set the desired position (for the displaypart)*/
366  robot.setDesiredCameraPosition(cdMo);
367  robot.getCameraParameters(cam,Iint);
368 }
369 
370 void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type){interaction_type=type;}
371 double error(){return _error;}
372 
373 #endif