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