Visual Servoing Platform  version 3.4.0
servoAfma6TwoLines2DCamVelocity.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * tests the control law
33  * eye-in-hand control
34  * velocity computed in the camera frame
35  *
36  * Authors:
37  * Eric Marchand
38  *
39  *****************************************************************************/
40 
61 #include <cmath> // std::fabs
62 #include <limits> // numeric_limits
63 #include <stdlib.h>
64 #include <visp3/core/vpConfig.h>
65 #include <visp3/core/vpDebug.h> // Debug trace
66 #if (defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394))
67 
68 #include <visp3/core/vpDisplay.h>
69 #include <visp3/core/vpImage.h>
70 #include <visp3/gui/vpDisplayGTK.h>
71 #include <visp3/gui/vpDisplayOpenCV.h>
72 #include <visp3/gui/vpDisplayX.h>
73 #include <visp3/io/vpImageIo.h>
74 #include <visp3/sensor/vp1394TwoGrabber.h>
75 
76 #include <visp3/core/vpHomogeneousMatrix.h>
77 #include <visp3/core/vpLine.h>
78 #include <visp3/core/vpMath.h>
79 #include <visp3/me/vpMeLine.h>
80 #include <visp3/visual_features/vpFeatureBuilder.h>
81 #include <visp3/visual_features/vpFeatureLine.h>
82 #include <visp3/vs/vpServo.h>
83 
84 #include <visp3/robot/vpRobotAfma6.h>
85 
86 // Exception
87 #include <visp3/core/vpException.h>
88 #include <visp3/vs/vpServoDisplay.h>
89 
90 int main()
91 {
92  try {
94 
98  g.open(I);
99 
100  g.acquire(I);
101 
102 #ifdef VISP_HAVE_X11
103  vpDisplayX display(I, 100, 100, "Current image");
104 #elif defined(VISP_HAVE_OPENCV)
105  vpDisplayOpenCV display(I, 100, 100, "Current image");
106 #elif defined(VISP_HAVE_GTK)
107  vpDisplayGTK display(I, 100, 100, "Current image");
108 #endif
109 
111  vpDisplay::flush(I);
112 
113  vpServo task;
114 
115  std::cout << std::endl;
116  std::cout << "-------------------------------------------------------" << std::endl;
117  std::cout << " Test program for vpServo " << std::endl;
118  std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl;
119  std::cout << " Simulation " << std::endl;
120  std::cout << " task : servo a point " << std::endl;
121  std::cout << "-------------------------------------------------------" << std::endl;
122  std::cout << std::endl;
123 
124  int i;
125  int nbline = 2;
126 
127  vpMeLine line[nbline];
128 
129  vpMe me;
130  me.setRange(10);
131  me.setPointsToTrack(100);
132  me.setThreshold(50000);
133  me.setSampleStep(10);
134 
135  // Initialize the tracking. Define the two lines to track
136  vpTRACE("The two lines to track must be parallels ");
137  // vpTRACE("The two lines to track must be perpendicular ") ;
138  for (i = 0; i < nbline; i++) {
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  vpLine lined[2];
160  lined[0].setWorldCoordinates(1, 0, 0, -0.05, 0, 0, 1, 0);
161  lined[1].setWorldCoordinates(1, 0, 0, 0.05, 0, 0, 1, 0);
162 
163  vpHomogeneousMatrix cMo(0, 0, 0.5, 0, 0, vpMath::rad(0));
164 
165  lined[0].project(cMo);
166  lined[1].project(cMo);
167 
168  // Those lines are needed to keep the conventions define in vpMeLine
169  // (Those in vpLine are less restrictive) Another way to have the
170  // coordinates of the desired features is to learn them before executing
171  // the program.
172  lined[0].setRho(-fabs(lined[0].getRho()));
173  lined[0].setTheta(0);
174  lined[1].setRho(-fabs(lined[1].getRho()));
175  lined[1].setTheta(M_PI);
176 
177  vpFeatureLine pd[nbline];
178  vpFeatureBuilder::create(pd[0], lined[0]);
179  vpFeatureBuilder::create(pd[1], lined[1]);
180 
181  vpTRACE("define the task");
182  vpTRACE("\t we want an eye-in-hand control law");
183  vpTRACE("\t robot is controlled in the camera frame");
185 
186  vpTRACE("\t we want to see a point on a point..");
187  std::cout << std::endl;
188  for (i = 0; i < nbline; i++)
189  task.addFeature(p[i], pd[i]);
190 
191  vpTRACE("\t set the gain");
192  task.setLambda(0.2);
193 
194  vpTRACE("Display task information ");
195  task.print();
196 
198 
199  unsigned int iter = 0;
200  vpTRACE("\t loop");
201  vpColVector v;
202 
203  vpImage<vpRGBa> Ic;
204  double lambda_av = 0.05;
205  double alpha = 0.2;
206  double beta = 3;
207 
208  for (;;) {
209  std::cout << "---------------------------------------------" << iter << std::endl;
210 
211  try {
212  g.acquire(I);
214 
215  // Track the lines and update the features
216  for (i = 0; i < nbline; i++) {
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  gain = alpha * exp(-beta * (task.getError()).sumSquare()) + lambda_av;
235  }
236  }
237  task.setLambda(gain);
238 
239  v = task.computeControlLaw();
240 
241  if (iter == 0)
244  } catch (...) {
245  v = 0;
247  robot.stopMotion();
248  exit(1);
249  }
250 
251  vpTRACE("\t\t || s - s* || = %f ", (task.getError()).sumSquare());
252  iter++;
253  }
254 
255  vpTRACE("Display task information ");
256  task.print();
257  return EXIT_SUCCESS;
258  }
259  catch (const vpException &e) {
260  std::cout << "Test failed with exception: " << e << std::endl;
261  return EXIT_FAILURE;
262  }
263 }
264 
265 #else
266 int main()
267 {
268  std::cout << "You do not have an afma6 robot connected to your computer..." << std::endl;
269  return EXIT_SUCCESS;
270 }
271 
272 #endif
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
Definition: vpAfma6.cpp:1256
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void setPointsToTrack(const int &n)
Definition: vpMe.h:264
Implementation of an homogeneous matrix and operations on such kind of matrices.
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:490
void setRho(double rho)
Definition: vpLine.h:153
void setSampleStep(const double &s)
Definition: vpMe.h:278
void setTheta(double theta)
Definition: vpLine.h:163
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:150
error that can be emited by ViSP classes.
Definition: vpException.h:71
void track(const vpImage< unsigned char > &Im)
Definition: vpMeLine.cpp:746
Definition: vpMe.h:60
static const vpColor green
Definition: vpColor.h:220
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:211
static const vpColor red
Definition: vpColor.h:217
void display(const vpImage< unsigned char > &I, vpColor col)
Definition: vpMeLine.cpp:224
void open(vpImage< unsigned char > &I)
Class that defines a 3D line in the object frame and allows forward projection of the line in the cam...
Definition: vpLine.h:104
Initialize the velocity controller.
Definition: vpRobot.h:66
vpColVector getError() const
Definition: vpServo.h:278
void setWorldCoordinates(const double &oA1, const double &oB1, const double &oC1, const double &oD1, const double &oA2, const double &oB2, const double &oC2, const double &oD2)
Definition: vpLine.cpp:85
vpColVector computeControlLaw()
Definition: vpServo.cpp:929
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
#define vpTRACE
Definition: vpDebug.h:416
void setDisplay(vpMeSite::vpMeSiteDisplayType select)
Definition: vpMeTracker.h:152
static void display(const vpImage< unsigned char > &I)
Class that tracks in an image a line moving edges.
Definition: vpMeLine.h:151
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:404
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:134
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
void initTracking(const vpImage< unsigned char > &I)
Definition: vpMeLine.cpp:236
static double rad(double deg)
Definition: vpMath.h:110
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void setFramerate(vp1394TwoFramerateType fps)
void setVideoMode(vp1394TwoVideoModeType videomode)
void setThreshold(const double &t)
Definition: vpMe.h:300
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:306
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:271
void setMe(vpMe *p_me)
Definition: vpMeTracker.h:173
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:218