ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
servoAfma6Line2DCamVelocity.cpp
1 /****************************************************************************
2  *
3  * $Id: servoAfma6Line2DCamVelocity.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  * tests the control law
36  * eye-in-hand control
37  * velocity computed in the camera frame
38  *
39  * Authors:
40  * Eric Marchand
41  *
42  *****************************************************************************/
62 #include <visp/vpConfig.h>
63 #include <visp/vpDebug.h> // Debug trace
64 #include <stdlib.h>
65 #if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394_2))
66 
67 #include <visp/vp1394TwoGrabber.h>
68 #include <visp/vpImage.h>
69 #include <visp/vpDisplay.h>
70 #include <visp/vpDisplayX.h>
71 #include <visp/vpDisplayOpenCV.h>
72 #include <visp/vpDisplayGTK.h>
73 
74 #include <visp/vpMath.h>
75 #include <visp/vpHomogeneousMatrix.h>
76 #include <visp/vpFeatureLine.h>
77 #include <visp/vpLine.h>
78 #include <visp/vpMeLine.h>
79 #include <visp/vpServo.h>
80 #include <visp/vpFeatureBuilder.h>
81 
82 #include <visp/vpRobotAfma6.h>
83 
84 // Exception
85 #include <visp/vpException.h>
86 #include <visp/vpMatrixException.h>
87 #include <visp/vpServoDisplay.h>
88 
89 int
90 main()
91 {
92  try
93  {
95 
99 
100  g.open(I) ;
101 
102  g.acquire(I) ;
103 
104 #ifdef VISP_HAVE_X11
105  vpDisplayX display(I,100,100,"Current image") ;
106 #elif defined(VISP_HAVE_OPENCV)
107  vpDisplayOpenCV display(I,100,100,"Current image") ;
108 #elif defined(VISP_HAVE_GTK)
109  vpDisplayGTK display(I,100,100,"Current image") ;
110 #endif
111 
112  vpDisplay::display(I) ;
113  vpDisplay::flush(I) ;
114 
115 
116  vpServo task ;
117 
118 
119  std::cout << std::endl ;
120  std::cout << "-------------------------------------------------------" << std::endl ;
121  std::cout << " Test program for vpServo " <<std::endl ;
122  std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
123  std::cout << " Simulation " << std::endl ;
124  std::cout << " task : servo a line " << std::endl ;
125  std::cout << "-------------------------------------------------------" << std::endl ;
126  std::cout << std::endl ;
127 
128 
129  vpMeLine line ;
130 
131  vpMe me ;
132  me.setRange(10) ;
133  me.setPointsToTrack(100) ;
134  me.setThreshold(100000) ;
135  me.setSampleStep(10);
137 
138 
139  line.setMe(&me) ;
140 
141  //Initialize the tracking. Define the line to track.
142  line.initTracking(I) ;
143  line.track(I) ;
144 
145  vpRobotAfma6 robot ;
146  // robot.move("pos-init.pos") ;
147 
148  vpCameraParameters cam ;
149  // Update camera parameters
150  robot.getCameraParameters (cam, I);
151 
152  vpTRACE("sets the current position of the visual feature ") ;
153  vpFeatureLine p ;
154  vpFeatureBuilder::create(p,cam, line) ;
155 
156  vpTRACE("sets the desired position of the visual feature ") ;
157  vpLine lined;
158  lined.setWorldCoordinates(1,0,0,0,0,0,1,0);
159  vpHomogeneousMatrix cMo(0,0,0.3,0,0,vpMath::rad(0));
160  lined.project(cMo);
161  lined.setRho(-fabs(lined.getRho()));
162  lined.setTheta(0);
163 
164  vpFeatureLine pd ;
165  vpFeatureBuilder::create(pd,lined);
166 
167  vpTRACE("define the task") ;
168  vpTRACE("\t we want an eye-in-hand control law") ;
169  vpTRACE("\t robot is controlled in the camera frame") ;
171 
172  vpTRACE("\t we want to see a point on a point..") ;
173  std::cout << std::endl ;
174  task.addFeature(p,pd) ;
175 
176  vpTRACE("\t set the gain") ;
177  task.setLambda(0.2) ;
178 
179 
180  vpTRACE("Display task information " ) ;
181  task.print() ;
182 
183 
185 
186  unsigned int iter=0 ;
187  vpTRACE("\t loop") ;
188  vpColVector v ;
189  for ( ; ; )
190  {
191  std::cout << "---------------------------------------------" << iter <<std::endl ;
192 
193  try {
194  g.acquire(I) ;
195  vpDisplay::display(I) ;
196 
197  //Track the line
198  line.track(I) ;
199  line.display(I, vpColor::red) ;
200 
201  //Update the current line feature
202  vpFeatureBuilder::create(p,cam,line);
203 
204  //displqy the current and the desired features
205  p.display(cam, I, vpColor::red) ;
206  pd.display(cam, I, vpColor::green) ;
207 
208  v = task.computeControlLaw() ;
209 
210  vpDisplay::flush(I) ;
211  if (iter==0) vpDisplay::getClick(I) ;
213  }
214  catch(...)
215  {
216  v =0 ;
218  robot.stopMotion() ;
219  exit(1) ;
220  }
221 
222  vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
223  iter++;
224  }
225 
226  vpTRACE("Display task information " ) ;
227  task.print() ;
228  task.kill();
229  }
230  catch (...)
231  {
232  vpERROR_TRACE(" Test failed") ;
233  return 0;
234  }
235 }
236 
237 #else
238 int
239 main()
240 {
241  vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
242 }
243 
244 #endif