Visual Servoing Platform version 3.5.0
servoSimuViper850FourPoints2DCamVelocity.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 polar
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/vpSimulatorViper850.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 // open a display for the visualization
197
198 vpImage<unsigned char> Iint(480, 640, 255);
199
200 if (opt_display) {
201 displayInt.init(Iint, 700, 0, "Internal view");
202 }
203
204 vpServo task;
205
206 std::cout << std::endl;
207 std::cout << "----------------------------------------------" << std::endl;
208 std::cout << " Test program for vpServo " << std::endl;
209 std::cout << " Eye-in-hand task control, articular velocity are computed" << std::endl;
210 std::cout << " Simulation " << std::endl;
211 std::cout << " task : servo 4 points " << std::endl;
212 std::cout << "----------------------------------------------" << std::endl;
213 std::cout << std::endl;
214
215 // sets the initial camera location
216 vpHomogeneousMatrix cMo(-0.05, -0.05, 0.7, vpMath::rad(10), vpMath::rad(10), vpMath::rad(-30));
217
218 // sets the point coordinates in the object frame
219 vpPoint point[4];
220 point[0].setWorldCoordinates(-0.045, -0.045, 0);
221 point[3].setWorldCoordinates(-0.045, 0.045, 0);
222 point[2].setWorldCoordinates(0.045, 0.045, 0);
223 point[1].setWorldCoordinates(0.045, -0.045, 0);
224
225 // computes the point coordinates in the camera frame and its 2D
226 // coordinates
227 for (unsigned int i = 0; i < 4; i++)
228 point[i].track(cMo);
229
230 // sets the desired position of the point
231 vpFeaturePoint p[4];
232 for (unsigned int i = 0; i < 4; i++)
233 vpFeatureBuilder::create(p[i], point[i]); // retrieve x,y and Z of the vpPoint structure
234
235 // sets the desired position of the feature point s*
236 vpFeaturePoint pd[4];
237
238 // Desired pose
240
241 // Projection of the points
242 for (unsigned int i = 0; i < 4; i++)
243 point[i].track(cdMo);
244
245 for (unsigned int i = 0; i < 4; i++)
246 vpFeatureBuilder::create(pd[i], point[i]);
247
248 // define the task
249 // - we want an eye-in-hand control law
250 // - articular velocity are computed
253
254 // - we want to see a point on a point
255 for (unsigned int i = 0; i < 4; i++)
256 task.addFeature(p[i], pd[i]);
257
258 // set the gain
259 task.setLambda(0.8);
260
261 // Declaration of the robot
262 vpSimulatorViper850 robot(opt_display);
263
264 // Initialise the robot and especially the camera
267
268 // Initialise the object for the display part
270
271 // Initialise the position of the object relative to the pose of the
272 // robot's camera
273 robot.initialiseObjectRelativeToCamera(cMo);
274
275 // Set the desired position (for the display part)
276 robot.setDesiredCameraPosition(cdMo);
277
278 // Get the internal robot's camera parameters
280 robot.getCameraParameters(cam, Iint);
281
282 if (opt_display) {
283 // Get the internal view
284 vpDisplay::display(Iint);
285 robot.getInternalView(Iint);
286 vpDisplay::flush(Iint);
287 }
288
289 // Display task information
290 task.print();
291
292 unsigned int iter = 0;
293 // loop
294 while (iter++ < 500) {
295 std::cout << "---------------------------------------------" << iter << std::endl;
296 vpColVector v;
297
298 // Get the Time at the beginning of the loop
299 double t = vpTime::measureTimeMs();
300
301 // Get the current pose of the camera
302 cMo = robot.get_cMo();
303
304 if (iter == 1) {
305 std::cout << "Initial robot position with respect to the object frame:\n";
306 cMo.print();
307 }
308
309 // new point position
310 for (unsigned int i = 0; i < 4; i++) {
311 point[i].track(cMo);
312 // retrieve x,y and Z of the vpPoint structure
313 vpFeatureBuilder::create(p[i], point[i]);
314 }
315
316 if (opt_display) {
317 // Get the internal view and display it
318 vpDisplay::display(Iint);
319 robot.getInternalView(Iint);
320 vpDisplay::flush(Iint);
321 }
322
323 if (opt_display && opt_click_allowed && iter == 1) {
324 // suppressed for automate test
325 std::cout << "Click in the internal view window to continue..." << std::endl;
327 }
328
329 // compute the control law
330 v = task.computeControlLaw();
331
332 // send the camera velocity to the controller
334
335 std::cout << "|| s - s* || " << (task.getError()).sumSquare() << std::endl;
336
337 // The main loop has a duration of 10 ms at minimum
338 vpTime::wait(t, 10);
339 }
340
341 // Display task information
342 task.print();
343
344 std::cout << "Final robot position with respect to the object frame:\n";
345 cMo.print();
346
347 if (opt_display && opt_click_allowed) {
348 // suppressed for automate test
349 std::cout << "Click in the internal view window to end..." << std::endl;
351 }
352 return EXIT_SUCCESS;
353 }
354 catch (const vpException &e) {
355 std::cout << "Catch a ViSP exception: " << e << std::endl;
356 return EXIT_FAILURE;
357 }
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
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 Viper S850 robot named Viper850.
@ TOOL_PTGREY_FLEA2_CAMERA
Definition: vpViper850.h:130
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()