Visual Servoing Platform version 3.5.0
servoSimuAfma6FourPoints2DCamVelocity.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 * Simulation of a 2D visual servoing using 4 points with cartesian
33 * coordinates as visual feature.
34 *
35 * Authors:
36 * Fabien Spindler
37 *
38 *****************************************************************************/
39
56#include <visp3/core/vpConfig.h>
57#include <visp3/core/vpDebug.h>
58
59#if ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD)) \
60 && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GDI)) \
61 && (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
62
63// We need to use threading capabilities. Thus on Unix-like
64// platforms, the libpthread third-party library need to be
65// installed. On Windows, we use the native threading capabilities.
66
67#include <stdio.h>
68#include <stdlib.h>
69
70#include <visp3/core/vpCameraParameters.h>
71#include <visp3/core/vpHomogeneousMatrix.h>
72#include <visp3/core/vpImage.h>
73#include <visp3/core/vpImagePoint.h>
74#include <visp3/core/vpIoTools.h>
75#include <visp3/core/vpMath.h>
76#include <visp3/core/vpMeterPixelConversion.h>
77#include <visp3/gui/vpDisplayGDI.h>
78#include <visp3/gui/vpDisplayGTK.h>
79#include <visp3/gui/vpDisplayX.h>
80#include <visp3/io/vpParseArgv.h>
81#include <visp3/robot/vpSimulatorAfma6.h>
82#include <visp3/visual_features/vpFeatureBuilder.h>
83#include <visp3/visual_features/vpFeaturePoint.h>
84#include <visp3/vs/vpServo.h>
85
86// List of allowed command line options
87#define GETOPTARGS "cdh"
88
89void usage(const char *name, const char *badparam);
90bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
91
100void usage(const char *name, const char *badparam)
101{
102 fprintf(stdout, "\n\
103Tests a control law with the following characteristics:\n\
104 - eye-in-hand control\n\
105 - articular velocity are computed\n\
106 - servo on 4 points,\n\
107 - internal and external camera view displays.\n\
108 \n\
109SYNOPSIS\n\
110 %s [-c] [-d] [-h]\n", name);
111
112 fprintf(stdout, "\n\
113OPTIONS: Default\n\
114 -c\n\
115 Disable the mouse click. Useful to automaze the \n\
116 execution of this program without humain intervention.\n\
117 \n\
118 -d \n\
119 Turn off the display.\n\
120 \n\
121 -h\n\
122 Print the help.\n");
123
124 if (badparam)
125 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
126}
139bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
140{
141 const char *optarg_;
142 int c;
143 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
144
145 switch (c) {
146 case 'c':
147 click_allowed = false;
148 break;
149 case 'd':
150 display = false;
151 break;
152 case 'h':
153 usage(argv[0], NULL);
154 return false;
155 break;
156
157 default:
158 usage(argv[0], optarg_);
159 return false;
160 break;
161 }
162 }
163
164 if ((c == 1) || (c == -1)) {
165 // standalone param or error
166 usage(argv[0], NULL);
167 std::cerr << "ERROR: " << std::endl;
168 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
169 return false;
170 }
171
172 return true;
173}
174
175int main(int argc, const char **argv)
176{
177 try {
178 bool opt_click_allowed = true;
179 bool opt_display = true;
180
181 // Read the command line options
182 if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
183 exit(-1);
184 }
185
186 // We open two displays, one for the internal camera view, the other one for
187 // the external view, using either X11, GTK or GDI.
188#if defined VISP_HAVE_X11
189 vpDisplayX displayInt;
190#elif defined VISP_HAVE_GDI
191 vpDisplayGDI displayInt;
192#elif defined VISP_HAVE_OPENCV
193 vpDisplayOpenCV displayInt;
194#endif
195
196 vpImage<unsigned char> Iint(480, 640, 255);
197
198 if (opt_display) {
199 // open a display for the visualization
200 displayInt.init(Iint, 700, 0, "Internal view");
201 }
202
203 vpServo task;
204
205 std::cout << std::endl;
206 std::cout << "----------------------------------------------" << std::endl;
207 std::cout << " Test program for vpServo " << std::endl;
208 std::cout << " Eye-in-hand task control, articular velocity are computed" << std::endl;
209 std::cout << " Simulation " << std::endl;
210 std::cout << " task : servo 4 points " << std::endl;
211 std::cout << "----------------------------------------------" << std::endl;
212 std::cout << std::endl;
213
214 // sets the initial camera location
215 vpHomogeneousMatrix cMo(-0.05, -0.05, 0.7, vpMath::rad(10), vpMath::rad(10), vpMath::rad(-30));
216
217 // sets the point coordinates in the object frame
218 vpPoint point[4];
219 point[0].setWorldCoordinates(-0.045, -0.045, 0);
220 point[3].setWorldCoordinates(-0.045, 0.045, 0);
221 point[2].setWorldCoordinates(0.045, 0.045, 0);
222 point[1].setWorldCoordinates(0.045, -0.045, 0);
223
224 // computes the point coordinates in the camera frame and its 2D
225 // coordinates
226 for (unsigned int i = 0; i < 4; i++)
227 point[i].track(cMo);
228
229 // sets the desired position of the point
230 vpFeaturePoint p[4];
231 for (unsigned int i = 0; i < 4; i++)
232 vpFeatureBuilder::create(p[i], point[i]); // retrieve x,y and Z of the vpPoint structure
233
234 // sets the desired position of the feature point s*
235 vpFeaturePoint pd[4];
236
237 // Desired pose
239
240 // Projection of the points
241 for (unsigned int i = 0; i < 4; i++)
242 point[i].track(cdMo);
243
244 for (unsigned int i = 0; i < 4; i++)
245 vpFeatureBuilder::create(pd[i], point[i]);
246
247 // define the task
248 // - we want an eye-in-hand control law
249 // - articular velocity are computed
252
253 // we want to see a point on a point
254 for (unsigned int i = 0; i < 4; i++)
255 task.addFeature(p[i], pd[i]);
256
257 // set the gain
258 task.setLambda(0.8);
259
260 // Declaration of the robot
261 vpSimulatorAfma6 robot(opt_display);
262
263 // Initialise the robot and especially the camera
266
267 // Initialise the object for the display part*/
269
270 // Initialise the position of the object relative to the pose of the
271 // robot's camera
272 robot.initialiseObjectRelativeToCamera(cMo);
273
274 // Set the desired position (for the displaypart)
275 robot.setDesiredCameraPosition(cdMo);
276
277 // Get the internal robot's camera parameters
279 robot.getCameraParameters(cam, Iint);
280
281 if (opt_display) {
282 // Get the internal view
283 vpDisplay::display(Iint);
284 robot.getInternalView(Iint);
285 vpDisplay::flush(Iint);
286 }
287
288 // Display task information
289 task.print();
290
291 unsigned int iter = 0;
292 vpTRACE("\t loop");
293 while (iter++ < 500) {
294 std::cout << "---------------------------------------------" << iter << std::endl;
295 vpColVector v;
296
297 // Get the Time at the beginning of the loop
298 double t = vpTime::measureTimeMs();
299
300 // Get the current pose of the camera
301 cMo = robot.get_cMo();
302
303 if (iter == 1) {
304 std::cout << "Initial robot position with respect to the object frame:\n";
305 cMo.print();
306 }
307
308 // new point position
309 for (unsigned int i = 0; i < 4; i++) {
310 point[i].track(cMo);
311 // retrieve x,y and Z of the vpPoint structure
312 vpFeatureBuilder::create(p[i], point[i]);
313 }
314
315 if (opt_display) {
316 // Get the internal view and display it
317 vpDisplay::display(Iint);
318 robot.getInternalView(Iint);
319 vpDisplay::flush(Iint);
320 }
321
322 if (opt_display && opt_click_allowed && iter == 1) {
323 // suppressed for automate test
324 std::cout << "Click in the internal view window to continue..." << std::endl;
326 }
327
328 // compute the control law
329 v = task.computeControlLaw();
330
331 // send the camera velocity to the controller
333
334 std::cout << "|| s - s* || " << (task.getError()).sumSquare() << std::endl;
335
336 // The main loop has a duration of 10 ms at minimum
337 vpTime::wait(t, 10);
338 }
339
340 // Display task information
341 task.print();
342
343 std::cout << "Final robot position with respect to the object frame:\n";
344 cMo.print();
345
346 if (opt_display && opt_click_allowed) {
347 // suppressed for automate test
348 std::cout << "Click in the internal view window to end..." << std::endl;
350 }
351 return EXIT_SUCCESS;
352 }
353 catch (const vpException &e) {
354 std::cout << "Catch a ViSP exception: " << e << std::endl;
355 return EXIT_FAILURE;
356 }
357 return EXIT_SUCCESS;
358}
359#elif !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
360int main()
361{
362 std::cout << "You do not have X11, or GDI (Graphical Device Interface) of OpenCV functionalities to display images..." << std::endl;
363 std::cout << "Tip if you are on a unix-like system:" << std::endl;
364 std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
365 std::cout << "Tip if you are on a windows-like system:" << std::endl;
366 std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
367 return EXIT_SUCCESS;
368}
369#elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
370int main()
371{
372 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
373 return EXIT_SUCCESS;
374}
375#else
376int main()
377{
378 std::cout << "You do not have threading capabilities" << std::endl;
379 std::cout << "Tip:" << std::endl;
380 std::cout << "- Install pthread, configure again ViSP using cmake and build again this example" << std::endl;
381 return EXIT_SUCCESS;
382}
383#endif
@ TOOL_CCMOP
Definition: vpAfma6.h:127
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:135
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
error that can be emited by ViSP classes.
Definition: vpException.h:72
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
static double rad(double deg)
Definition: vpMath.h:110
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:113
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ CAMERA_FRAME
Definition: vpRobot.h:82
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:66
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:567
@ EYEINHAND_CAMERA
Definition: vpServo.h:155
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:306
void setLambda(double c)
Definition: vpServo.h:404
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:218
vpColVector getError() const
Definition: vpServo.h:278
vpColVector computeControlLaw()
Definition: vpServo.cpp:929
@ DESIRED
Definition: vpServo.h:186
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:490
Simulator of Irisa's gantry robot named Afma6.
#define vpTRACE
Definition: vpDebug.h:416
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()