Visual Servoing Platform  version 3.4.0
servoAfma4Point2DCamVelocity.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  * Fabien Spindler
39  *
40  *****************************************************************************/
41 
62 #include <stdlib.h>
63 #include <visp3/core/vpConfig.h>
64 #include <visp3/core/vpDebug.h> // Debug trace
65 #if (defined(VISP_HAVE_AFMA4) && defined(VISP_HAVE_DC1394))
66 
67 #include <visp3/core/vpDisplay.h>
68 #include <visp3/core/vpImage.h>
69 #include <visp3/core/vpImagePoint.h>
70 #include <visp3/gui/vpDisplayGTK.h>
71 #include <visp3/gui/vpDisplayOpenCV.h>
72 #include <visp3/gui/vpDisplayX.h>
73 #include <visp3/sensor/vp1394TwoGrabber.h>
74 
75 #include <visp3/core/vpHomogeneousMatrix.h>
76 #include <visp3/core/vpIoTools.h>
77 #include <visp3/core/vpMath.h>
78 #include <visp3/core/vpPoint.h>
79 #include <visp3/robot/vpRobotAfma4.h>
80 #include <visp3/visual_features/vpFeatureBuilder.h>
81 #include <visp3/visual_features/vpFeaturePoint.h>
82 #include <visp3/vs/vpServo.h>
83 
84 // Exception
85 #include <visp3/core/vpException.h>
86 #include <visp3/vs/vpServoDisplay.h>
87 
88 #include <visp3/blob/vpDot.h>
89 
90 int main()
91 {
92  try {
93  // Log file creation in /tmp/$USERNAME/log.dat
94  // This file contains by line:
95  // - the 6 computed cam velocities (m/s, rad/s) to achieve the task
96  // - the 6 mesured joint velocities (m/s, rad/s)
97  // - the 6 mesured joint positions (m, rad)
98  // - the 2 values of s - s*
99  std::string username;
100  // Get the user login name
101  vpIoTools::getUserName(username);
102 
103  // Create a log filename to save velocities...
104  std::string logdirname;
105  logdirname = "/tmp/" + username;
106 
107  // Test if the output path exist. If no try to create it
108  if (vpIoTools::checkDirectory(logdirname) == false) {
109  try {
110  // Create the dirname
111  vpIoTools::makeDirectory(logdirname);
112  } catch (...) {
113  std::cerr << std::endl << "ERROR:" << std::endl;
114  std::cerr << " Cannot create " << logdirname << std::endl;
115  exit(-1);
116  }
117  }
118  std::string logfilename;
119  logfilename = logdirname + "/log.dat";
120 
121  // Open the log file name
122  std::ofstream flog(logfilename.c_str());
123 
124  vpRobotAfma4 robot;
125  vpServo task;
126 
128 
129  vp1394TwoGrabber g(false);
131  g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
132  g.open(I);
133 
134  g.acquire(I);
135 
136 #ifdef VISP_HAVE_X11
137  vpDisplayX display(I, 100, 100, "Current image");
138 #elif defined(VISP_HAVE_OPENCV)
139  vpDisplayOpenCV display(I, 100, 100, "Current image");
140 #elif defined(VISP_HAVE_GTK)
141  vpDisplayGTK display(I, 100, 100, "Current image");
142 #endif
143 
145  vpDisplay::flush(I);
146 
147  std::cout << std::endl;
148  std::cout << "-------------------------------------------------------" << std::endl;
149  std::cout << " Test program for vpServo " << std::endl;
150  std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl;
151  std::cout << " Simulation " << std::endl;
152  std::cout << " task : servo a point " << std::endl;
153  std::cout << "-------------------------------------------------------" << std::endl;
154  std::cout << std::endl;
155 
156  vpDot dot;
157 
158  std::cout << "Click on a dot..." << std::endl;
159  dot.initTracking(I);
160 
161  // Get the cog of the dot
162  vpImagePoint cog = dot.getCog();
163 
165  vpDisplay::flush(I);
166 
167  vpCameraParameters cam;
168 
169  vpTRACE("sets the current position of the visual feature ");
170  vpFeaturePoint p;
171  vpFeatureBuilder::create(p, cam, dot); // retrieve x,y and Z of the vpPoint structure
172 
173  vpTRACE("sets the desired position of the visual feature ");
174  vpFeaturePoint pd;
175  pd.buildFrom(0, 0, 1);
176 
177  vpTRACE("define the task");
178  vpTRACE("\t we want an eye-in-hand control law");
179  vpTRACE("\t robot is controlled in the camera frame");
181 
182  vpTRACE("\t we want to see a point on a point..");
183  std::cout << std::endl;
184  task.addFeature(p, pd);
185 
186  vpTRACE("\t set the gain");
187  task.setLambda(0.8);
188 
189  vpTRACE("Display task information ");
190  task.print();
191 
192  robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
193 
194  std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
195  for (;;) {
196  // Acquire a new image from the camera
197  g.acquire(I);
198 
199  // Display this image
201 
202  // Achieve the tracking of the dot in the image
203  dot.track(I);
204 
205  // Get the cog of the dot
206  vpImagePoint cog = dot.getCog();
207 
208  // Display a green cross at the center of gravity position in the image
210 
211  // Update the point feature from the dot location
212  vpFeatureBuilder::create(p, cam, dot);
213 
214  vpColVector v;
215  // Compute the visual servoing skew vector
216  v = task.computeControlLaw();
217 
218  // Display the current and desired feature points in the image display
219  vpServoDisplay::display(task, cam, I);
220 
221  // Apply the computed joint velocities to the robot
222  robot.setVelocity(vpRobot::CAMERA_FRAME, v);
223 
224  // Save velocities applied to the robot in the log file
225  // v[0], v[1], v[2] correspond to camera translation velocities in m/s
226  // v[3], v[4], v[5] correspond to camera rotation velocities in rad/s
227  flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " ";
228 
229  // Get the measured joint velocities of the robot
230  vpColVector qvel;
231  robot.getVelocity(vpRobot::ARTICULAR_FRAME, qvel);
232  // Save measured joint velocities of the robot in the log file:
233  // - qvel[0], qvel[1], qvel[2] correspond to measured joint translation
234  // velocities in m/s
235  // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation
236  // velocities in rad/s
237  flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " ";
238 
239  // Get the measured joint positions of the robot
240  vpColVector q;
241  robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
242  // Save measured joint positions of the robot in the log file
243  // - q[0], q[1], q[2] correspond to measured joint translation
244  // positions in m
245  // - q[3], q[4], q[5] correspond to measured joint rotation
246  // positions in rad
247  flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " ";
248 
249  // Save feature error (s-s*) for the feature point. For this feature
250  // point, we have 2 errors (along x and y axis). This error is
251  // expressed in meters in the camera frame
252  flog << task.getError() << std::endl;
253 
254  // Flush the display
255  vpDisplay::flush(I);
256  }
257 
258  flog.close(); // Close the log file
259 
260  // Display task information
261  task.print();
262 
263  return EXIT_SUCCESS;
264  } catch (const vpException &e) {
265  std::cout << "Catch a ViSP exception: " << e << std::endl;
266  return EXIT_FAILURE;
267  }
268 }
269 
270 #else
271 int main()
272 {
273  std::cout << "You do not have an afma4 robot connected to your computer..." << std::endl;
274  return EXIT_SUCCESS;
275 }
276 #endif
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:482
void buildFrom(double x, double y, double Z)
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:490
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:150
void track(const vpImage< unsigned char > &I)
Definition: vpDot.cpp:770
error that can be emited by ViSP classes.
Definition: vpException.h:71
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
static const vpColor green
Definition: vpColor.h:220
static void flush(const vpImage< unsigned char > &I)
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:332
vpImagePoint getCog() const
Definition: vpDot.h:247
Initialize the velocity controller.
Definition: vpRobot.h:66
vpColVector getError() const
Definition: vpServo.h:278
vpColVector computeControlLaw()
Definition: vpServo.cpp:929
#define vpTRACE
Definition: vpDebug.h:416
static void display(const vpImage< unsigned char > &I)
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
static std::string getUserName()
Definition: vpIoTools.cpp:228
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:134
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
Control of Irisa&#39;s cylindrical robot named Afma4.
Definition: vpRobotAfma4.h:178
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:306
This tracker is meant to track a dot (connected pixels with same gray level) on a vpImage...
Definition: vpDot.h:115
Class for firewire ieee1394 video devices using libdc1394-2.x api.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:218
void initTracking(const vpImage< unsigned char > &I)
Definition: vpDot.cpp:635
static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
static const vpColor blue
Definition: vpColor.h:223