Visual Servoing Platform  version 3.0.1
servoAfma6Cylinder2DCamVelocitySecondaryTask.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 
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/me/vpMeLine.h>
78 #include <visp3/core/vpCylinder.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 
89 int
90 main()
91 {
92  try
93  {
95 
99  g.open(I) ;
100 
101  g.acquire(I) ;
102 
103 #ifdef VISP_HAVE_X11
104  vpDisplayX display(I,100,100,"Current image") ;
105 #elif defined(VISP_HAVE_OPENCV)
106  vpDisplayOpenCV display(I,100,100,"Current image") ;
107 #elif defined(VISP_HAVE_GTK)
108  vpDisplayGTK display(I,100,100,"Current image") ;
109 #endif
110 
111  vpDisplay::display(I) ;
112  vpDisplay::flush(I) ;
113 
114  vpServo task ;
115 
116  std::cout << std::endl ;
117  std::cout << "-------------------------------------------------------" << std::endl ;
118  std::cout << " Test program for vpServo " <<std::endl ;
119  std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
120  std::cout << " Simulation " << std::endl ;
121  std::cout << " task : servo a point " << std::endl ;
122  std::cout << "-------------------------------------------------------" << std::endl ;
123  std::cout << std::endl ;
124 
125 
126  int i ;
127  int nbline =2 ;
128  vpMeLine line[nbline] ;
129 
130  vpMe me ;
131  me.setRange(20) ;
132  me.setPointsToTrack(100) ;
133  me.setThreshold(2000) ;
134  me.setSampleStep(10);
135 
136  //Initialize the tracking of the two edges of the cylinder
137  for (i=0 ; i < nbline ; i++)
138  {
140  line[i].setMe(&me) ;
141 
142  line[i].initTracking(I) ;
143  line[i].track(I) ;
144  }
145 
146  vpRobotAfma6 robot ;
147  //robot.move("zero.pos") ;
148 
149  vpCameraParameters cam ;
150  // Update camera parameters
151  robot.getCameraParameters (cam, I);
152 
153  vpTRACE("sets the current position of the visual feature ") ;
154  vpFeatureLine p[nbline] ;
155  for (i=0 ; i < nbline ; i++)
156  vpFeatureBuilder::create(p[i],cam, line[i]) ;
157 
158  vpTRACE("sets the desired position of the visual feature ") ;
159  vpCylinder cyld(0,1,0,0,0,0,0.04);
160 
161  vpHomogeneousMatrix cMo(0,0,0.5,0,0,vpMath::rad(0));
162 
163  cyld.project(cMo);
164 
165  vpFeatureLine pd[nbline] ;
168 
169  //Those lines are needed to keep the conventions define in vpMeLine (Those in vpLine are less restrictive)
170  //Another way to have the coordinates of the desired features is to learn them before executing the program.
171  pd[0].setRhoTheta(-fabs(pd[0].getRho()),0);
172  pd[1].setRhoTheta(-fabs(pd[1].getRho()),M_PI);
173 
174  vpTRACE("define the task") ;
175  vpTRACE("\t we want an eye-in-hand control law") ;
176  vpTRACE("\t robot is controlled in the camera frame") ;
179 
180  vpTRACE("\t we want to see a point on a point..") ;
181  std::cout << std::endl ;
182  for (i=0 ; i < nbline ; i++)
183  task.addFeature(p[i],pd[i]) ;
184 
185  vpTRACE("\t set the gain") ;
186  task.setLambda(0.3) ;
187 
188 
189  vpTRACE("Display task information " ) ;
190  task.print() ;
191 
192 
194 
195  unsigned int iter=0 ;
196  vpTRACE("\t loop") ;
197  vpColVector v ;
198  vpImage<vpRGBa> Ic ;
199  double lambda_av =0.05;
200  double alpha = 0.02;
201  double beta =3;
202  double erreur = 1;
203 
204 
205  //First loop to reach the convergence position
206  while(erreur > 0.00001)
207  {
208  std::cout << "---------------------------------------------" << iter <<std::endl ;
209 
210  try {
211  g.acquire(I) ;
212  vpDisplay::display(I) ;
213 
214  //Track the two edges and update the features
215  for (i=0 ; i < nbline ; i++)
216  {
217  line[i].track(I) ;
218  line[i].display(I, vpColor::red) ;
219 
220  vpFeatureBuilder::create(p[i],cam,line[i]);
221 
222  p[i].display(cam, I, vpColor::red) ;
223  pd[i].display(cam, I, vpColor::green) ;
224  }
225 
226  vpDisplay::flush(I) ;
227 
228  //Adaptative gain
229  double gain ;
230  {
231  if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
232  gain = lambda_av ;
233  else
234  {
235  gain = alpha * exp (-beta * ( task.getError() ).sumSquare() ) + lambda_av ;
236  }
237  }
238  task.setLambda(gain) ;
239 
240  v = task.computeControlLaw() ;
241 
242  if (iter==0) vpDisplay::getClick(I) ;
243  }
244  catch(...)
245  {
246  v =0 ;
248  robot.stopMotion() ;
249  exit(1) ;
250  }
251 
253  erreur = ( task.getError() ).sumSquare();
254  vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
255  iter++;
256  }
257 
258  /**********************************************************************************************/
259 
260  //Second loop is to compute the control while taking into account the secondary task.
261  vpColVector e1(6) ; e1 = 0 ;
262  vpColVector e2(6) ; e2 = 0 ;
263  vpColVector proj_e1 ;
264  vpColVector proj_e2 ;
265  iter = 0;
266  double rapport = 0;
267  double vitesse = 0.02;
268  unsigned int tempo = 1200;
269 
270  for ( ; ; )
271  {
272  std::cout << "---------------------------------------------" << iter <<std::endl ;
273 
274  try {
275  g.acquire(I) ;
276  vpDisplay::display(I) ;
277 
278  //Track the two edges and update the features
279  for (i=0 ; i < nbline ; i++)
280  {
281  line[i].track(I) ;
282  line[i].display(I, vpColor::red) ;
283 
284  vpFeatureBuilder::create(p[i],cam,line[i]);
285 
286  p[i].display(cam, I, vpColor::red) ;
287  pd[i].display(cam, I, vpColor::green) ;
288  }
289 
290  vpDisplay::flush(I) ;
291 
292  v = task.computeControlLaw() ;
293 
294  //Compute the new control law corresponding to the secondary task
295  if ( iter%tempo < 400 /*&& iter%tempo >= 0*/)
296  {
297  e2 = 0;
298  e1[0] = fabs(vitesse) ;
299  proj_e1 = task.secondaryTask(e1);
300  rapport = vitesse/proj_e1[0];
301  proj_e1 *= rapport ;
302  v += proj_e1 ;
303  if ( iter == 199 ) iter+=200; //This line is needed to make on ly an half turn during the first cycle
304  }
305 
306  if ( iter%tempo < 600 && iter%tempo >= 400)
307  {
308  e1 = 0;
309  e2[1] = fabs(vitesse) ;
310  proj_e2 = task.secondaryTask(e2);
311  rapport = vitesse/proj_e2[1];
312  proj_e2 *= rapport ;
313  v += proj_e2 ;
314  }
315 
316  if ( iter%tempo < 1000 && iter%tempo >= 600)
317  {
318  e2 = 0;
319  e1[0] = -fabs(vitesse) ;
320  proj_e1 = task.secondaryTask(e1);
321  rapport = -vitesse/proj_e1[0];
322  proj_e1 *= rapport ;
323  v += proj_e1 ;
324  }
325 
326  if ( iter%tempo < 1200 && iter%tempo >= 1000)
327  {
328  e1 = 0;
329  e2[1] = -fabs(vitesse) ;
330  proj_e2 = task.secondaryTask(e2);
331  rapport = -vitesse/proj_e2[1];
332  proj_e2 *= rapport ;
333  v += proj_e2 ;
334  }
335 
337  }
338  catch(...)
339  {
340  v =0 ;
342  robot.stopMotion() ;
343  exit(1) ;
344  }
345 
346  vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
347  iter++;
348  }
349 
350 
351  vpTRACE("Display task information " ) ;
352  task.print() ;
353  task.kill();
354  }
355  catch (...)
356  {
357  vpERROR_TRACE(" Test failed") ;
358  return 0;
359  }
360 }
361 
362 #else
363 int
364 main()
365 {
366  vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
367 }
368 
369 #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
vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator=false)
Definition: vpServo.cpp:1513
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)