ViSP  3.0.0
servoPtu46Point2DArtVelocity.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  * tests the control law
32  * eye-in-hand control
33  * velocity computed in articular
34  *
35  * Authors:
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 
60 #include <visp3/core/vpConfig.h>
61 #include <visp3/core/vpDebug.h> // Debug trace
62 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
63 # include <unistd.h>
64 #endif
65 #include <signal.h>
66 
67 
68 
69 #if (defined(VISP_HAVE_PTU46) & defined (VISP_HAVE_DC1394) )
70 
71 #ifdef VISP_HAVE_PTHREAD
72 # include <pthread.h>
73 #endif
74 
75 #include <visp3/sensor/vp1394TwoGrabber.h>
76 #include <visp3/core/vpImage.h>
77 #include <visp3/core/vpDisplay.h>
78 #include <visp3/gui/vpDisplayX.h>
79 
80 #include <visp3/core/vpMath.h>
81 #include <visp3/core/vpHomogeneousMatrix.h>
82 #include <visp3/visual_features/vpFeaturePoint.h>
83 #include <visp3/core/vpPoint.h>
84 #include <visp3/vs/vpServo.h>
85 #include <visp3/visual_features/vpFeatureBuilder.h>
86 
87 #include <visp3/robot/vpRobotPtu46.h>
88 
89 // Exception
90 #include <visp3/core/vpException.h>
91 #include <visp3/vs/vpServoDisplay.h>
92 
93 #include <visp3/blob/vpDot2.h>
94 
95 
96 #ifdef VISP_HAVE_PTHREAD
97 pthread_mutex_t mutexEndLoop = PTHREAD_MUTEX_INITIALIZER;
98 #endif
99 
100 void signalCtrC( int signumber )
101 {
102  (void)(signumber);
103 #ifdef VISP_HAVE_PTHREAD
104  pthread_mutex_unlock( &mutexEndLoop );
105 #endif
106  usleep( 1000*10 );
107  vpTRACE("Ctrl-C pressed...");
108 }
109 
110 int
111 main()
112 {
113  std::cout << std::endl ;
114  std::cout << "-------------------------------------------------------" << std::endl ;
115  std::cout << " Test program for vpServo " <<std::endl ;
116  std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
117  std::cout << " Simulation " << std::endl ;
118  std::cout << " task : servo a point " << std::endl ;
119  std::cout << "-------------------------------------------------------" << std::endl ;
120  std::cout << std::endl ;
121 
122  try{
123 
124 #ifdef VISP_HAVE_PTHREAD
125  pthread_mutex_lock( &mutexEndLoop );
126 #endif
127  signal( SIGINT,&signalCtrC );
128 
129  vpRobotPtu46 robot ;
130  {
131  vpColVector q(2); q=0;
134  }
135 
137 
139 
140  g.open(I) ;
141 
142  try{
143  g.acquire(I) ;
144  }
145  catch(...)
146  {
147  vpERROR_TRACE(" Error caught") ;
148  return(-1) ;
149  }
150 
151 
152  vpDisplayX display(I,100,100,"testDisplayX.cpp ") ;
153  vpTRACE(" ") ;
154 
155  try{
156  vpDisplay::display(I) ;
157  vpDisplay::flush(I) ;
158  }
159  catch(...)
160  {
161  vpERROR_TRACE(" Error caught") ;
162  return(-1) ;
163  }
164 
165  vpServo task ;
166 
167  vpDot2 dot ;
168 
169  try{
170  vpERROR_TRACE("start dot.initTracking(I) ") ;
171  vpImagePoint germ;
172  vpDisplay::getClick( I, germ );
173  dot.setCog(germ);
174  vpDEBUG_TRACE(25,"Click!");
175  //dot.initTracking(I) ;
176  dot.track(I);
177  vpERROR_TRACE("after dot.initTracking(I) ") ;
178  }
179  catch(...)
180  {
181  vpERROR_TRACE(" Error caught ") ;
182  return(-1) ;
183  }
184 
185  vpCameraParameters cam ;
186 
187  vpTRACE("sets the current position of the visual feature ") ;
188  vpFeaturePoint p ;
189  vpFeatureBuilder::create(p,cam, dot) ; //retrieve x,y and Z of the vpPoint structure
190 
191  p.set_Z(1) ;
192  vpTRACE("sets the desired position of the visual feature ") ;
193  vpFeaturePoint pd ;
194  pd.buildFrom(0,0,1) ;
195 
196  vpTRACE("define the task") ;
197  vpTRACE("\t we want an eye-in-hand control law") ;
198  vpTRACE("\t articular velocity are computed") ;
201 
202 
203  vpTRACE("Set the position of the camera in the end-effector frame ") ;
204  vpHomogeneousMatrix cMe ;
205  // robot.get_cMe(cMe) ;
206 
208  robot.get_cVe(cVe) ;
209  std::cout << cVe <<std::endl ;
210  task.set_cVe(cVe) ;
211 
213  vpTRACE("Set the Jacobian (expressed in the end-effector frame)") ;
214  vpMatrix eJe ;
215  robot.get_eJe(eJe) ;
216  task.set_eJe(eJe) ;
217 
218 
219  vpTRACE("\t we want to see a point on a point..") ;
220  std::cout << std::endl ;
221  task.addFeature(p,pd) ;
222 
223  vpTRACE("\t set the gain") ;
224  task.setLambda(0.1) ;
225 
226 
227  vpTRACE("Display task information " ) ;
228  task.print() ;
229 
230 
232 
233  unsigned int iter=0 ;
234  vpTRACE("\t loop") ;
235 #ifdef VISP_HAVE_PTHREAD
236  while( 0 != pthread_mutex_trylock( &mutexEndLoop ) )
237 #else
238  for ( ; ; )
239 #endif
240  {
241  std::cout << "---------------------------------------------" << iter <<std::endl ;
242 
243  g.acquire(I) ;
244  vpDisplay::display(I) ;
245 
246  dot.track(I) ;
247 
248  // vpDisplay::displayCross(I,(int)dot.I(), (int)dot.J(),
249  // 10,vpColor::green) ;
250 
251 
252  vpFeatureBuilder::create(p,cam, dot);
253 
254  // get the jacobian
255  robot.get_eJe(eJe) ;
256  task.set_eJe(eJe) ;
257 
258  // std::cout << (vpMatrix)cVe*eJe << std::endl ;
259 
260  vpColVector v ;
261  v = task.computeControlLaw() ;
262 
263  vpServoDisplay::display(task,cam,I) ;
264  std::cout << v.t() ;
266  vpDisplay::flush(I) ;
267 
268  vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
269  }
270 
271  vpTRACE("Display task information " ) ;
272  task.print() ;
273  task.kill();
274 
275  } catch (...) { vpERROR_TRACE("Trow uncatched..."); }
276 }
277 
278 
279 #else
280 int
281 main()
282 {
283  vpERROR_TRACE("You don't have a ptu-46 head connected to your computer ",
284  "or 1394 framegrabbing capabilities...");
285 }
286 #endif
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:92
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
Implementation of an homogeneous matrix and operations on such kind of matrices.
#define vpERROR_TRACE
Definition: vpDebug.h:391
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &q_dot)
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:459
void setCog(const vpImagePoint &ip)
Definition: vpDot2.h:254
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
Initialize the position controller.
Definition: vpRobot.h:69
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void acquire(vpImage< unsigned char > &I)
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
Definition: vpDot2.h:124
void track(const vpImage< unsigned char > &I)
Definition: vpDot2.cpp:461
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2233
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
void open(vpImage< unsigned char > &I)
void kill()
Definition: vpServo.cpp:186
Initialize the velocity controller.
Definition: vpRobot.h:68
vpColVector getError() const
Definition: vpServo.h:271
Interface for the Directed Perception ptu-46 pan, tilt head .
Definition: vpRobotPtu46.h:84
vpColVector computeControlLaw()
Definition: vpServo.cpp:899
#define vpTRACE
Definition: vpDebug.h:414
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:206
vpRowVector t() const
Generic class defining intrinsic camera parameters.
void setLambda(double c)
Definition: vpServo.h:390
Implementation of a velocity twist matrix and operations on such kind of matrices.
void get_eJe(vpMatrix &_eJe)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:519
void buildFrom(const double x, const double y, const double Z)
void get_cVe(vpVelocityTwistMatrix &_cVe) const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:434
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:248
#define vpDEBUG_TRACE
Definition: vpDebug.h:478
Class for firewire ieee1394 video devices using libdc1394-2.x api.
virtual bool getClick(bool blocking=true)=0
void set_Z(const double Z)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:217
static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)