ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
servoPtu46Point2DArtVelocity.cpp
1 /****************************************************************************
2  *
3  * $Id: servoPtu46Point2DArtVelocity.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 articular
38  *
39  * Authors:
40  * Fabien Spindler
41  *
42  *****************************************************************************/
43 
64 #include <visp/vpConfig.h>
65 #include <visp/vpDebug.h> // Debug trace
66 #ifdef UNIX
67 # include <unistd.h>
68 #endif
69 #include <signal.h>
70 
71 
72 
73 #if (defined(VISP_HAVE_PTU46) & defined (VISP_HAVE_DC1394) )
74 
75 #ifdef VISP_HAVE_PTHREAD
76 # include <pthread.h>
77 #endif
78 
79 #include <visp/vp1394Grabber.h>
80 #include <visp/vpImage.h>
81 #include <visp/vpDisplay.h>
82 #include <visp/vpDisplayX.h>
83 
84 #include <visp/vpMath.h>
85 #include <visp/vpHomogeneousMatrix.h>
86 #include <visp/vpFeaturePoint.h>
87 #include <visp/vpPoint.h>
88 #include <visp/vpServo.h>
89 #include <visp/vpFeatureBuilder.h>
90 
91 #include <visp/vpRobotPtu46.h>
92 
93 // Exception
94 #include <visp/vpException.h>
95 #include <visp/vpMatrixException.h>
96 #include <visp/vpServoDisplay.h>
97 
98 #include <visp/vpDot2.h>
99 
100 
101 #ifdef VISP_HAVE_PTHREAD
102 pthread_mutex_t mutexEndLoop = PTHREAD_MUTEX_INITIALIZER;
103 #endif
104 
105 void signalCtrC( int signumber )
106 {
107 #ifdef VISP_HAVE_PTHREAD
108  pthread_mutex_unlock( &mutexEndLoop );
109 #endif
110  usleep( 1000*10 );
111  vpTRACE("Ctrl-C pressed...");
112 }
113 
114 int
115 main()
116 {
117  std::cout << std::endl ;
118  std::cout << "-------------------------------------------------------" << std::endl ;
119  std::cout << " Test program for vpServo " <<std::endl ;
120  std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
121  std::cout << " Simulation " << std::endl ;
122  std::cout << " task : servo a point " << std::endl ;
123  std::cout << "-------------------------------------------------------" << std::endl ;
124  std::cout << std::endl ;
125 
126  try{
127 
128 #ifdef VISP_HAVE_PTHREAD
129  pthread_mutex_lock( &mutexEndLoop );
130 #endif
131  signal( SIGINT,&signalCtrC );
132 
133  vpRobotPtu46 robot ;
134  {
135  vpColVector q(2); q=0;
138  }
139 
141 
142  vp1394Grabber g;
143 
144  g.open(I) ;
145 
146  try{
147  g.acquire(I) ;
148  }
149  catch(...)
150  {
151  vpERROR_TRACE(" Error caught") ;
152  return(-1) ;
153  }
154 
155 
156  vpDisplayX display(I,100,100,"testDisplayX.cpp ") ;
157  vpTRACE(" ") ;
158 
159  try{
160  vpDisplay::display(I) ;
161  vpDisplay::flush(I) ;
162  }
163  catch(...)
164  {
165  vpERROR_TRACE(" Error caught") ;
166  return(-1) ;
167  }
168 
169 
170  vpServo task ;
171 
172  vpDot2 dot ;
173 
174  try{
175  vpERROR_TRACE("start dot.initTracking(I) ") ;
176  int x,y;
177  vpDisplay::getClick( I,y,x );
178  dot.set_u( x ) ;
179  dot.set_v( y ) ;
180  vpDEBUG_TRACE(25,"Click!");
181  //dot.initTracking(I) ;
182  dot.track(I);
183  vpERROR_TRACE("after dot.initTracking(I) ") ;
184  }
185  catch(...)
186  {
187  vpERROR_TRACE(" Error caught ") ;
188  return(-1) ;
189  }
190 
191  vpCameraParameters cam ;
192 
193  vpTRACE("sets the current position of the visual feature ") ;
194  vpFeaturePoint p ;
195  vpFeatureBuilder::create(p,cam, dot) ; //retrieve x,y and Z of the vpPoint structure
196 
197  p.set_Z(1) ;
198  vpTRACE("sets the desired position of the visual feature ") ;
199  vpFeaturePoint pd ;
200  pd.buildFrom(0,0,1) ;
201 
202  vpTRACE("define the task") ;
203  vpTRACE("\t we want an eye-in-hand control law") ;
204  vpTRACE("\t articular velocity are computed") ;
207 
208 
209  vpTRACE("Set the position of the camera in the end-effector frame ") ;
210  vpHomogeneousMatrix cMe ;
211  // robot.get_cMe(cMe) ;
212 
214  robot.get_cVe(cVe) ;
215  std::cout << cVe <<std::endl ;
216  task.set_cVe(cVe) ;
217 
219  vpTRACE("Set the Jacobian (expressed in the end-effector frame)") ;
220  vpMatrix eJe ;
221  robot.get_eJe(eJe) ;
222  task.set_eJe(eJe) ;
223 
224 
225  vpTRACE("\t we want to see a point on a point..") ;
226  std::cout << std::endl ;
227  task.addFeature(p,pd) ;
228 
229  vpTRACE("\t set the gain") ;
230  task.setLambda(0.1) ;
231 
232 
233  vpTRACE("Display task information " ) ;
234  task.print() ;
235 
236 
238 
239  unsigned int iter=0 ;
240  vpTRACE("\t loop") ;
241 #ifdef VISP_HAVE_PTHREAD
242  while( 0 != pthread_mutex_trylock( &mutexEndLoop ) )
243 #else
244  for ( ; ; )
245 #endif
246  {
247  std::cout << "---------------------------------------------" << iter <<std::endl ;
248 
249  g.acquire(I) ;
250  vpDisplay::display(I) ;
251 
252  dot.track(I) ;
253 
254  // vpDisplay::displayCross(I,(int)dot.I(), (int)dot.J(),
255  // 10,vpColor::green) ;
256 
257 
258  vpFeatureBuilder::create(p,cam, dot);
259 
260  // get the jacobian
261  robot.get_eJe(eJe) ;
262  task.set_eJe(eJe) ;
263 
264  // std::cout << (vpMatrix)cVe*eJe << std::endl ;
265 
266  vpColVector v ;
267  v = task.computeControlLaw() ;
268 
269  vpServoDisplay::display(task,cam,I) ;
270  std::cout << v.t() ;
272  vpDisplay::flush(I) ;
273 
274  vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
275  }
276 
277  vpTRACE("Display task information " ) ;
278  task.print() ;
279  task.kill();
280 
281  } catch (...) { vpERROR_TRACE("Trow uncatched..."); }
282 }
283 
284 
285 #else
286 int
287 main()
288 {
289  vpERROR_TRACE("You don't have a ptu-46 head connected to your computer ",
290  "or 1394 framegrabbing capabilities...");
291 }
292 #endif