Visual Servoing Platform  version 3.0.1
servoAfma6Line2DCamVelocity.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  *****************************************************************************/
58 #include <visp3/core/vpConfig.h>
59 #include <visp3/core/vpDebug.h> // Debug trace
60 #include <stdlib.h>
61 #if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
62 
63 #include <visp3/sensor/vp1394TwoGrabber.h>
64 #include <visp3/core/vpImage.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/core/vpLine.h>
74 #include <visp3/me/vpMeLine.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 
95  g.open(I) ;
96 
97  g.acquire(I) ;
98 
99 #ifdef VISP_HAVE_X11
100  vpDisplayX display(I,100,100,"Current image") ;
101 #elif defined(VISP_HAVE_OPENCV)
102  vpDisplayOpenCV display(I,100,100,"Current image") ;
103 #elif defined(VISP_HAVE_GTK)
104  vpDisplayGTK display(I,100,100,"Current image") ;
105 #endif
106 
107  vpDisplay::display(I) ;
108  vpDisplay::flush(I) ;
109 
110 
111  vpServo task ;
112 
113 
114  std::cout << std::endl ;
115  std::cout << "-------------------------------------------------------" << std::endl ;
116  std::cout << " Test program for vpServo " <<std::endl ;
117  std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
118  std::cout << " Simulation " << std::endl ;
119  std::cout << " task : servo a line " << std::endl ;
120  std::cout << "-------------------------------------------------------" << std::endl ;
121  std::cout << std::endl ;
122 
123 
124  vpMeLine line ;
125 
126  vpMe me ;
127  me.setRange(10) ;
128  me.setPointsToTrack(100) ;
129  me.setThreshold(100000) ;
130  me.setSampleStep(10);
132 
133 
134  line.setMe(&me) ;
135 
136  //Initialize the tracking. Define the line to track.
137  line.initTracking(I) ;
138  line.track(I) ;
139 
140  vpRobotAfma6 robot ;
141  // robot.move("pos-init.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 ;
149  vpFeatureBuilder::create(p,cam, line) ;
150 
151  vpTRACE("sets the desired position of the visual feature ") ;
152  vpLine lined;
153  lined.setWorldCoordinates(1,0,0,0,0,0,1,0);
154  vpHomogeneousMatrix cMo(0,0,0.3,0,0,vpMath::rad(0));
155  lined.project(cMo);
156  lined.setRho(-fabs(lined.getRho()));
157  lined.setTheta(0);
158 
159  vpFeatureLine pd ;
160  vpFeatureBuilder::create(pd,lined);
161 
162  vpTRACE("define the task") ;
163  vpTRACE("\t we want an eye-in-hand control law") ;
164  vpTRACE("\t robot is controlled in the camera frame") ;
166 
167  vpTRACE("\t we want to see a point on a point..") ;
168  std::cout << std::endl ;
169  task.addFeature(p,pd) ;
170 
171  vpTRACE("\t set the gain") ;
172  task.setLambda(0.2) ;
173 
174 
175  vpTRACE("Display task information " ) ;
176  task.print() ;
177 
178 
180 
181  unsigned int iter=0 ;
182  vpTRACE("\t loop") ;
183  vpColVector v ;
184  for ( ; ; )
185  {
186  std::cout << "---------------------------------------------" << iter <<std::endl ;
187 
188  try {
189  g.acquire(I) ;
190  vpDisplay::display(I) ;
191 
192  //Track the line
193  line.track(I) ;
194  line.display(I, vpColor::red) ;
195 
196  //Update the current line feature
197  vpFeatureBuilder::create(p,cam,line);
198 
199  //displqy the current and the desired features
200  p.display(cam, I, vpColor::red) ;
201  pd.display(cam, I, vpColor::green) ;
202 
203  v = task.computeControlLaw() ;
204 
205  vpDisplay::flush(I) ;
206  if (iter==0) vpDisplay::getClick(I) ;
208  }
209  catch(...)
210  {
211  v =0 ;
213  robot.stopMotion() ;
214  exit(1) ;
215  }
216 
217  vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
218  iter++;
219  }
220 
221  vpTRACE("Display task information " ) ;
222  task.print() ;
223  task.kill();
224  }
225  catch (...)
226  {
227  vpERROR_TRACE(" Test failed") ;
228  return 0;
229  }
230 }
231 
232 #else
233 int
234 main()
235 {
236  vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
237 }
238 
239 #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
double getRho() const
Definition: vpLine.h:158
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