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