Visual Servoing Platform  version 3.0.1
servoAfma6SquareLines2DCamVelocity.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 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 the camera frame
34  *
35  * Authors:
36  * Eric Marchand
37  *
38  *****************************************************************************/
59 #include <visp3/core/vpConfig.h>
60 #include <visp3/core/vpDebug.h> // Debug trace
61 #include <stdlib.h>
62 #include <cmath> // std::fabs
63 #include <limits> // numeric_limits
64 #if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
65 
66 #include <visp3/sensor/vp1394TwoGrabber.h>
67 #include <visp3/core/vpImage.h>
68 #include <visp3/io/vpImageIo.h>
69 #include <visp3/core/vpDisplay.h>
70 #include <visp3/gui/vpDisplayX.h>
71 #include <visp3/gui/vpDisplayOpenCV.h>
72 #include <visp3/gui/vpDisplayGTK.h>
73 
74 #include <visp3/core/vpMath.h>
75 #include <visp3/core/vpHomogeneousMatrix.h>
76 #include <visp3/visual_features/vpFeatureLine.h>
77 #include <visp3/core/vpLine.h>
78 #include <visp3/me/vpMeLine.h>
79 #include <visp3/vs/vpServo.h>
80 #include <visp3/visual_features/vpFeatureBuilder.h>
81 
82 #include <visp3/robot/vpRobotAfma6.h>
83 
84 // Exception
85 #include <visp3/core/vpException.h>
86 #include <visp3/vs/vpServoDisplay.h>
87 
88 #include <visp3/blob/vpDot.h>
89 
90 int
91 main()
92 {
93  try
94  {
96 
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  vpServo task ;
116 
117 
118  std::cout << std::endl ;
119  std::cout << "-------------------------------------------------------" << std::endl ;
120  std::cout << " Test program for vpServo " <<std::endl ;
121  std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
122  std::cout << " Simulation " << std::endl ;
123  std::cout << " task : servo a line " << std::endl ;
124  std::cout << "-------------------------------------------------------" << std::endl ;
125  std::cout << std::endl ;
126 
127  int i ;
128  int nbline =4 ;
129 
130  vpMeLine line[nbline] ;
131 
132  vpMe me ;
133  me.setRange(10) ;
134  me.setPointsToTrack(100) ;
135  me.setThreshold(50000) ;
136  me.setSampleStep(10);
137 
138  //Initialize the tracking. Define the four lines to track.
139  for (i=0 ; i < nbline ; i++)
140  {
142  line[i].setMe(&me) ;
143 
144  line[i].initTracking(I) ;
145  line[i].track(I) ;
146  }
147 
148  vpRobotAfma6 robot ;
149  //robot.move("zero.pos") ;
150 
151  vpCameraParameters cam ;
152  // Update camera parameters
153  robot.getCameraParameters (cam, I);
154 
155  vpTRACE("sets the current position of the visual feature ") ;
156  vpFeatureLine p[nbline] ;
157  for (i=0 ; i < nbline ; i++)
158  vpFeatureBuilder::create(p[i],cam, line[i]) ;
159 
160  vpTRACE("sets the desired position of the visual feature ") ;
161  vpLine lined[nbline];
162  lined[0].setWorldCoordinates(1,0,0,0.05,0,0,1,0);
163  lined[1].setWorldCoordinates(0,1,0,0.05,0,0,1,0);
164  lined[2].setWorldCoordinates(1,0,0,-0.05,0,0,1,0);
165  lined[3].setWorldCoordinates(0,1,0,-0.05,0,0,1,0);
166 
167  vpHomogeneousMatrix cMo(0,0,0.5,0,0,vpMath::rad(0));
168 
169  lined[0].project(cMo);
170  lined[1].project(cMo);
171  lined[2].project(cMo);
172  lined[3].project(cMo);
173 
174  //Those lines are needed to keep the conventions define in vpMeLine (Those in vpLine are less restrictive)
175  //Another way to have the coordinates of the desired features is to learn them before executing the program.
176  lined[0].setRho(-fabs(lined[0].getRho()));
177  lined[0].setTheta(0);
178  lined[1].setRho(-fabs(lined[1].getRho()));
179  lined[1].setTheta(M_PI/2);
180  lined[2].setRho(-fabs(lined[2].getRho()));
181  lined[2].setTheta(M_PI);
182  lined[3].setRho(-fabs(lined[3].getRho()));
183  lined[3].setTheta(-M_PI/2);
184 
185  vpFeatureLine pd[nbline] ;
186 
187  vpFeatureBuilder::create(pd[0],lined[0]);
188  vpFeatureBuilder::create(pd[1],lined[1]);
189  vpFeatureBuilder::create(pd[2],lined[2]);
190  vpFeatureBuilder::create(pd[3],lined[3]);
191 
192  vpTRACE("define the task") ;
193  vpTRACE("\t we want an eye-in-hand control law") ;
194  vpTRACE("\t robot is controlled in the camera frame") ;
197 
198  vpTRACE("\t we want to see a point on a point..") ;
199  std::cout << std::endl ;
200  for (i=0 ; i < nbline ; i++)
201  task.addFeature(p[i],pd[i]) ;
202 
203  vpTRACE("\t set the gain") ;
204  task.setLambda(0.2) ;
205 
206 
207  vpTRACE("Display task information " ) ;
208  task.print() ;
209 
211 
212  unsigned int iter=0 ;
213  vpTRACE("\t loop") ;
214  vpColVector v ;
215  vpImage<vpRGBa> Ic ;
216  double lambda_av =0.05;
217  double alpha = 0.05 ;
218  double beta =3 ;
219 
220  for ( ; ; )
221  {
222  std::cout << "---------------------------------------------" << iter <<std::endl ;
223 
224  try {
225  g.acquire(I) ;
226  vpDisplay::display(I) ;
227 
228  //Track the lines and update the features
229  for (i=0 ; i < nbline ; i++)
230  {
231  line[i].track(I) ;
232  line[i].display(I, vpColor::red) ;
233 
234  vpFeatureBuilder::create(p[i],cam,line[i]);
235 
236  p[i].display(cam, I, vpColor::red) ;
237  pd[i].display(cam, I, vpColor::green) ;
238  }
239 
240  double gain ;
241  {
242  if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
243  gain = lambda_av ;
244  else
245  {
246  gain = alpha * exp (-beta * ( task.getError() ).sumSquare() ) + lambda_av ;
247  }
248  }
249 
250  task.setLambda(gain) ;
251 
252  v = task.computeControlLaw() ;
253 
254  vpDisplay::flush(I) ;
255 
256  if (iter==0) vpDisplay::getClick(I) ;
257  if (v.sumSquare() > 0.5)
258  {
259  v =0 ;
261  robot.stopMotion() ;
263  }
264 
266 
267  }
268  catch(...)
269  {
270  v =0 ;
272  robot.stopMotion() ;
273  exit(1) ;
274  }
275 
276  vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
277  iter++;
278  }
279 
280  vpTRACE("Display task information " ) ;
281  task.print() ;
282  task.kill();
283  }
284  catch (...)
285  {
286  vpERROR_TRACE(" Test failed") ;
287  return 0;
288  }
289 }
290 
291 #else
292 int
293 main()
294 {
295  vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
296 }
297 
298 #endif
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
Definition: vpAfma6.cpp:1235
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void setPointsToTrack(const int &n)
Definition: vpMe.h:249
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setWorldCoordinates(const double &A1, const double &B1, const double &C1, const double &D1, const double &A2, const double &B2, const double &C2, const double &D2)
Definition: vpLine.cpp:94
#define vpERROR_TRACE
Definition: vpDebug.h:391
void setSampleStep(const double &s)
Definition: vpMe.h:263
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:153
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:512
void track(const vpImage< unsigned char > &Im)
Definition: vpMeLine.cpp:812
Definition: vpMe.h:59
static const vpColor green
Definition: vpColor.h:166
void acquire(vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
Control of Irisa&#39;s gantry robot named Afma6.
Definition: vpRobotAfma6.h:210
static const vpColor red
Definition: vpColor.h:163
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
void display(const vpImage< unsigned char > &I, vpColor col)
Definition: vpMeLine.cpp:241
void open(vpImage< unsigned char > &I)
Class that defines a line in the object frame, the camera frame and the image plane. All the parameters must be set in meter.
Definition: vpLine.h:105
void kill()
Definition: vpServo.cpp:191
Initialize the velocity controller.
Definition: vpRobot.h:68
vpColVector computeControlLaw()
Definition: vpServo.cpp:954
#define vpTRACE
Definition: vpDebug.h:414
void setDisplay(vpMeSite::vpMeSiteDisplayType select)
Definition: vpMeTracker.h:101
static void display(const vpImage< unsigned char > &I)
Class that tracks in an image a line moving edges.
Definition: vpMeLine.h:152
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Generic class defining intrinsic camera parameters.
void setLambda(double c)
Definition: vpServo.h:391
Class that defines a 2D line visual feature which is composed by two parameters that are and ...
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:138
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
void initTracking(const vpImage< unsigned char > &I)
Definition: vpMeLine.cpp:255
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:585
static double rad(double deg)
Definition: vpMath.h:104
void setRho(const double rho)
Definition: vpLine.h:125
void setTheta(const double theta)
Definition: vpLine.h:135
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void setFramerate(vp1394TwoFramerateType fps)
double sumSquare() const
void setVideoMode(vp1394TwoVideoModeType videomode)
void setThreshold(const double &t)
Definition: vpMe.h:284
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:314
vpColVector getError() const
Definition: vpServo.h:271
Class for firewire ieee1394 video devices using libdc1394-2.x api.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void setRange(const unsigned int &r)
Definition: vpMe.h:256
void setMe(vpMe *p_me)
Definition: vpMeTracker.h:135
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:222