Visual Servoing Platform  version 3.0.1
servoAfma6TwoLines2DCamVelocity.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  *****************************************************************************/
39 
63 #include <visp3/core/vpConfig.h>
64 #include <visp3/core/vpDebug.h> // Debug trace
65 #include <stdlib.h>
66 #include <cmath> // std::fabs
67 #include <limits> // numeric_limits
68 #if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
69 
70 #include <visp3/sensor/vp1394TwoGrabber.h>
71 #include <visp3/core/vpImage.h>
72 #include <visp3/io/vpImageIo.h>
73 #include <visp3/core/vpDisplay.h>
74 #include <visp3/gui/vpDisplayX.h>
75 #include <visp3/gui/vpDisplayOpenCV.h>
76 #include <visp3/gui/vpDisplayGTK.h>
77 
78 #include <visp3/core/vpMath.h>
79 #include <visp3/core/vpHomogeneousMatrix.h>
80 #include <visp3/visual_features/vpFeatureLine.h>
81 #include <visp3/core/vpLine.h>
82 #include <visp3/me/vpMeLine.h>
83 #include <visp3/vs/vpServo.h>
84 #include <visp3/visual_features/vpFeatureBuilder.h>
85 
86 #include <visp3/robot/vpRobotAfma6.h>
87 
88 // Exception
89 #include <visp3/core/vpException.h>
90 #include <visp3/vs/vpServoDisplay.h>
91 
92 int
93 main()
94 {
95  try
96  {
98 
102  g.open(I) ;
103 
104  g.acquire(I) ;
105 
106 #ifdef VISP_HAVE_X11
107  vpDisplayX display(I,100,100,"Current image") ;
108 #elif defined(VISP_HAVE_OPENCV)
109  vpDisplayOpenCV display(I,100,100,"Current image") ;
110 #elif defined(VISP_HAVE_GTK)
111  vpDisplayGTK display(I,100,100,"Current image") ;
112 #endif
113 
114  vpDisplay::display(I) ;
115  vpDisplay::flush(I) ;
116 
117  vpServo task ;
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 point " << std::endl ;
125  std::cout << "-------------------------------------------------------" << std::endl ;
126  std::cout << std::endl ;
127 
128  int i ;
129  int nbline =2 ;
130 
131  vpMeLine line[nbline] ;
132 
133  vpMe me ;
134  me.setRange(10) ;
135  me.setPointsToTrack(100) ;
136  me.setThreshold(50000) ;
137  me.setSampleStep(10);
138 
139  //Initialize the tracking. Define the two lines to track
140  vpTRACE("The two lines to track must be parallels ") ;
141  //vpTRACE("The two lines to track must be perpendicular ") ;
142  for (i=0 ; i < nbline ; i++)
143  {
145  line[i].setMe(&me) ;
146 
147  line[i].initTracking(I) ;
148  line[i].track(I) ;
149  }
150 
151  vpRobotAfma6 robot ;
152  //robot.move("zero.pos") ;
153 
154  vpCameraParameters cam ;
155  // Update camera parameters
156  robot.getCameraParameters (cam, I);
157 
158  vpTRACE("sets the current position of the visual feature ") ;
159  vpFeatureLine p[nbline] ;
160  for (i=0 ; i < nbline ; i++)
161  vpFeatureBuilder::create(p[i],cam, line[i]) ;
162 
163  vpTRACE("sets the desired position of the visual feature ") ;
164  vpLine lined[2];
165  lined[0].setWorldCoordinates(1,0,0,-0.05,0,0,1,0);
166  lined[1].setWorldCoordinates(1,0,0,0.05,0,0,1,0);
167 
168  vpHomogeneousMatrix cMo(0,0,0.5,0,0,vpMath::rad(0));
169 
170  lined[0].project(cMo);
171  lined[1].project(cMo);
172 
173  //Those lines are needed to keep the conventions define in vpMeLine (Those in vpLine are less restrictive)
174  //Another way to have the coordinates of the desired features is to learn them before executing the program.
175  lined[0].setRho(-fabs(lined[0].getRho()));
176  lined[0].setTheta(0);
177  lined[1].setRho(-fabs(lined[1].getRho()));
178  lined[1].setTheta(M_PI);
179 
180  vpFeatureLine pd[nbline] ;
181  vpFeatureBuilder::create(pd[0],lined[0]);
182  vpFeatureBuilder::create(pd[1],lined[1]);
183 
184  vpTRACE("define the task") ;
185  vpTRACE("\t we want an eye-in-hand control law") ;
186  vpTRACE("\t robot is controlled in the camera frame") ;
188 
189  vpTRACE("\t we want to see a point on a point..") ;
190  std::cout << std::endl ;
191  for (i=0 ; i < nbline ; i++)
192  task.addFeature(p[i],pd[i]) ;
193 
194  vpTRACE("\t set the gain") ;
195  task.setLambda(0.2) ;
196 
197 
198  vpTRACE("Display task information " ) ;
199  task.print() ;
200 
201 
203 
204  unsigned int iter=0 ;
205  vpTRACE("\t loop") ;
206  vpColVector v ;
207 
208  vpImage<vpRGBa> Ic ;
209  double lambda_av =0.05;
210  double alpha = 0.2;
211  double beta =3;
212 
213  for ( ; ; )
214  {
215  std::cout << "---------------------------------------------" << iter <<std::endl ;
216 
217  try {
218  g.acquire(I) ;
219  vpDisplay::display(I) ;
220 
221  //Track the lines and update the features
222  for (i=0 ; i < nbline ; i++)
223  {
224  line[i].track(I) ;
225  line[i].display(I, vpColor::red) ;
226 
227  vpFeatureBuilder::create(p[i],cam,line[i]);
228 
229  p[i].display(cam, I, vpColor::red) ;
230  pd[i].display(cam, I, vpColor::green) ;
231  }
232 
233  vpDisplay::flush(I) ;
234 
235  //Adaptative gain
236  double gain ;
237  {
238  if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
239  gain = lambda_av ;
240  else
241  {
242  gain = alpha * exp (-beta * ( task.getError() ).sumSquare() ) + lambda_av ;
243  }
244  }
245  task.setLambda(gain) ;
246 
247  v = task.computeControlLaw() ;
248 
249  if (iter==0) vpDisplay::getClick(I) ;
251  }
252  catch(...)
253  {
254  v =0 ;
256  robot.stopMotion() ;
257  exit(1) ;
258  }
259 
260  vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
261  iter++;
262  }
263 
264  vpTRACE("Display task information " ) ;
265  task.print() ;
266  task.kill();
267  }
268  catch (...)
269  {
270  vpERROR_TRACE(" Test failed") ;
271  return 0;
272  }
273 }
274 
275 #else
276 int
277 main()
278 {
279  vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
280 }
281 
282 #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
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)
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