Visual Servoing Platform version 3.5.0
servoSimuCylinder.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 * Demonstration of the wireframe simulator with a simple visual servoing
33 *
34 * Authors:
35 * Nicolas Melchior
36 *
37 *****************************************************************************/
38
45#include <stdlib.h>
46
47#include <visp3/core/vpCameraParameters.h>
48#include <visp3/core/vpCylinder.h>
49#include <visp3/core/vpHomogeneousMatrix.h>
50#include <visp3/core/vpImage.h>
51#include <visp3/core/vpIoTools.h>
52#include <visp3/core/vpMath.h>
53#include <visp3/core/vpTime.h>
54#include <visp3/core/vpVelocityTwistMatrix.h>
55#include <visp3/gui/vpDisplayD3D.h>
56#include <visp3/gui/vpDisplayGDI.h>
57#include <visp3/gui/vpDisplayGTK.h>
58#include <visp3/gui/vpDisplayOpenCV.h>
59#include <visp3/gui/vpDisplayX.h>
60#include <visp3/gui/vpPlot.h>
61#include <visp3/io/vpImageIo.h>
62#include <visp3/io/vpParseArgv.h>
63#include <visp3/robot/vpSimulatorCamera.h>
64#include <visp3/robot/vpWireFrameSimulator.h>
65#include <visp3/visual_features/vpFeatureBuilder.h>
66#include <visp3/vs/vpServo.h>
67
68#define GETOPTARGS "dhp"
69
70#if defined(VISP_HAVE_DISPLAY) \
71 && (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
72
81void usage(const char *name, const char *badparam)
82{
83 fprintf(stdout, "\n\
84Demonstration of the wireframe simulator with a simple visual servoing.\n\
85 \n\
86The visual servoing consists in bringing the camera at a desired position\n\
87from the object.\n\
88 \n\
89The visual features used to compute the pose of the camera and \n\
90thus the control law are two lines. These features are computed thanks \n\
91to the equation of a cylinder.\n\
92 \n\
93This demonstration explains also how to move the object around a world \n\
94reference frame. Here, the movment is a rotation around the x and y axis \n\
95at a given distance from the world frame. In fact the object trajectory \n\
96is on a sphere whose center is the origin of the world frame.\n\
97 \n\
98SYNOPSIS\n\
99 %s [-d] [-p] [-h]\n", name);
100
101 fprintf(stdout, "\n\
102OPTIONS: \n\
103 -d \n\
104 Turn off the display.\n\
105 \n\
106 -p \n\
107 Turn off the plotter.\n\
108 \n\
109 -h\n\
110 Print the help.\n");
111
112 if (badparam)
113 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
114}
115
128bool getOptions(int argc, const char **argv, bool &display, bool &plot)
129{
130 const char *optarg_;
131 int c;
132 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
133
134 switch (c) {
135 case 'd':
136 display = false;
137 break;
138 case 'p':
139 plot = false;
140 break;
141 case 'h':
142 usage(argv[0], NULL);
143 return false;
144
145 default:
146 usage(argv[0], optarg_);
147 return false;
148 }
149 }
150
151 if ((c == 1) || (c == -1)) {
152 // standalone param or error
153 usage(argv[0], NULL);
154 std::cerr << "ERROR: " << std::endl;
155 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
156 return false;
157 }
158
159 return true;
160}
161
162int main(int argc, const char **argv)
163{
164 try {
165 bool opt_display = true;
166 bool opt_plot = true;
167
168 // Read the command line options
169 if (getOptions(argc, argv, opt_display, opt_plot) == false) {
170 exit(-1);
171 }
172
173 vpImage<vpRGBa> Iint(480, 640, 255);
174 vpImage<vpRGBa> Iext(480, 640, 255);
175
176#if defined VISP_HAVE_X11
177 vpDisplayX display[2];
178#elif defined VISP_HAVE_OPENCV
179 vpDisplayOpenCV display[2];
180#elif defined VISP_HAVE_GDI
181 vpDisplayGDI display[2];
182#elif defined VISP_HAVE_D3D9
183 vpDisplayD3D display[2];
184#elif defined VISP_HAVE_GTK
185 vpDisplayGTK display[2];
186#endif
187
188 if (opt_display) {
189 // Display size is automatically defined by the image (I) size
190 display[0].init(Iint, 100, 100, "The internal view");
191 display[1].init(Iext, 100, 100, "The first external view");
193 vpDisplay::setWindowPosition(Iext, 750, 0);
194 vpDisplay::display(Iint);
195 vpDisplay::flush(Iint);
196 vpDisplay::display(Iext);
197 vpDisplay::flush(Iext);
198 }
199
200 vpPlot *plotter = NULL;
201
202 vpServo task;
203 vpSimulatorCamera robot;
204 float sampling_time = 0.020f; // Sampling period in second
205 robot.setSamplingTime(sampling_time);
206
207 // Set initial position of the object in the camera frame
208 vpHomogeneousMatrix cMo(0, 0.1, 0.3, vpMath::rad(35), vpMath::rad(25), vpMath::rad(75));
209 // Set desired position of the object in the camera frame
210 vpHomogeneousMatrix cdMo(0.0, 0.0, 0.5, vpMath::rad(90), vpMath::rad(0), vpMath::rad(0));
211 // Set initial position of the object in the world frame
212 vpHomogeneousMatrix wMo(0.0, 0.0, 0, 0, 0, 0);
213 // Position of the camera in the world frame
215 wMc = wMo * cMo.inverse();
216
217 // Create a cylinder
218 vpCylinder cylinder(0, 0, 1, 0, 0, 0, 0.1);
219
220 // Projection of the cylinder
221 cylinder.track(cMo);
222
223 // Set the current visual feature
224 vpFeatureLine l[2];
227
228 // Projection of the cylinder
229 cylinder.track(cdMo);
230
231 vpFeatureLine ld[2];
234
237
239 vpVelocityTwistMatrix cVe(cMe);
240 task.set_cVe(cVe);
241
242 vpMatrix eJe;
243 robot.get_eJe(eJe);
244 task.set_eJe(eJe);
245
246 for (int i = 0; i < 2; i++)
247 task.addFeature(l[i], ld[i]);
248
249 if (opt_plot) {
250 plotter = new vpPlot(2, 480, 640, 750, 550, "Real time curves plotter");
251 plotter->setTitle(0, "Visual features error");
252 plotter->setTitle(1, "Camera velocities");
253 plotter->initGraph(0, task.getDimension());
254 plotter->initGraph(1, 6);
255 plotter->setLegend(0, 0, "error_feat_l1_rho");
256 plotter->setLegend(0, 1, "error_feat_l1_theta");
257 plotter->setLegend(0, 2, "error_feat_l2_rho");
258 plotter->setLegend(0, 3, "error_feat_l2_theta");
259 plotter->setLegend(1, 0, "vc_x");
260 plotter->setLegend(1, 1, "vc_y");
261 plotter->setLegend(1, 2, "vc_z");
262 plotter->setLegend(1, 3, "wc_x");
263 plotter->setLegend(1, 4, "wc_y");
264 plotter->setLegend(1, 5, "wc_z");
265 }
266
267 task.setLambda(1);
268
270
271 // Set the scene
273
274 // Initialize simulator frames
275 sim.set_fMo(wMo); // Position of the object in the world reference frame
276 sim.setCameraPositionRelObj(cMo); // Initial position of the object in the camera frame
277 sim.setDesiredCameraPosition(cdMo); // Desired position of the object in the camera frame
278
279 // Set the External camera position
280 vpHomogeneousMatrix camMf(vpHomogeneousMatrix(0.0, 0, 3.5, vpMath::rad(0), vpMath::rad(30), 0));
281 sim.setExternalCameraPosition(camMf);
282
283 // Set the parameters of the cameras (internal and external)
284 vpCameraParameters camera(1000, 1000, 320, 240);
285 sim.setInternalCameraParameters(camera);
286 sim.setExternalCameraParameters(camera);
287
288 int max_iter = 10;
289
290 if (opt_display) {
291 max_iter = 2500;
292
293 // Get the internal and external views
294 sim.getInternalImage(Iint);
295 sim.getExternalImage(Iext);
296
297 // Display the object frame (current and desired position)
298 vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
299 vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
300
301 // Display the object frame the world reference frame and the camera
302 // frame
303 vpDisplay::displayFrame(Iext, camMf * sim.get_fMo() * cMo.inverse(), camera, 0.2, vpColor::none);
304 vpDisplay::displayFrame(Iext, camMf * sim.get_fMo(), camera, 0.2, vpColor::none);
305 vpDisplay::displayFrame(Iext, camMf, camera, 0.2, vpColor::none);
306
307 vpDisplay::displayText(Iint, 20, 20, "Click to start visual servo", vpColor::red);
308
309 vpDisplay::flush(Iint);
310 vpDisplay::flush(Iext);
311
312 std::cout << "Click on a display" << std::endl;
313 while (!vpDisplay::getClick(Iint, false) && !vpDisplay::getClick(Iext, false)) {
314 };
315 }
316
317 robot.setPosition(wMc);
318
319 // Print the task
320 task.print();
321
322 int iter = 0;
323 bool stop = false;
324 vpColVector v;
325
326 // Set the secondary task parameters
327 vpColVector e1(6, 0);
328 vpColVector e2(6, 0);
329 vpColVector proj_e1;
330 vpColVector proj_e2;
331 double rapport = 0;
332 double vitesse = 0.3;
333 int tempo = 600;
334
335 double t_prev, t = vpTime::measureTimeMs();
336
337 while (iter++ < max_iter && !stop) {
338 t_prev = t;
340
341 if (opt_display) {
342 vpDisplay::display(Iint);
343 vpDisplay::display(Iext);
344 }
345
346 robot.get_eJe(eJe);
347 task.set_eJe(eJe);
348
349 wMc = robot.getPosition();
350 cMo = wMc.inverse() * wMo;
351
352 cylinder.track(cMo);
355
356 v = task.computeControlLaw();
357
358 // Compute the velocity with the secondary task
359 if (iter % tempo < 200 && iter % tempo >= 0) {
360 e2 = 0;
361 e1[0] = -fabs(vitesse);
362 proj_e1 = task.secondaryTask(e1, true);
363 rapport = -vitesse / proj_e1[0];
364 proj_e1 *= rapport;
365 v += proj_e1;
366 }
367
368 else if (iter % tempo < 300 && iter % tempo >= 200) {
369 e1 = 0;
370 e2[1] = -fabs(vitesse);
371 proj_e2 = task.secondaryTask(e2, true);
372 rapport = -vitesse / proj_e2[1];
373 proj_e2 *= rapport;
374 v += proj_e2;
375 }
376
377 else if (iter % tempo < 500 && iter % tempo >= 300) {
378 e2 = 0;
379 e1[0] = -fabs(vitesse);
380 proj_e1 = task.secondaryTask(e1, true);
381 rapport = vitesse / proj_e1[0];
382 proj_e1 *= rapport;
383 v += proj_e1;
384 }
385
386 else if (iter % tempo < 600 && iter % tempo >= 500) {
387 e1 = 0;
388 e2[1] = -fabs(vitesse);
389 proj_e2 = task.secondaryTask(e2, true);
390 rapport = vitesse / proj_e2[1];
391 proj_e2 *= rapport;
392 v += proj_e2;
393 }
394
396
397 // Update the simulator frames
398 sim.set_fMo(wMo); // This line is not really requested since the object
399 // doesn't move
401
402 if (opt_plot) {
403 plotter->plot(0, iter, task.getError());
404 plotter->plot(1, iter, v);
405 }
406
407 if (opt_display) {
408 // Get the internal and external views
409 sim.getInternalImage(Iint);
410 sim.getExternalImage(Iext);
411
412 // Display the object frame (current and desired position)
413 vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
414 vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
415
416 // Display the object frame the world reference frame and the camera
417 // frame
418 vpDisplay::displayFrame(Iext, sim.getExternalCameraPosition() * sim.get_fMo() * cMo.inverse(), camera, 0.2,
422
423 vpDisplay::displayText(Iint, 20, 20, "Click to stop visual servo", vpColor::red);
424
425 std::stringstream ss;
426 ss << "Loop time: " << t - t_prev << " ms";
427 vpDisplay::displayText(Iint, 40, 20, ss.str(), vpColor::red);
428
429 if (vpDisplay::getClick(Iint, false)) {
430 stop = true;
431 }
432
433 vpDisplay::flush(Iext);
434 vpDisplay::flush(Iint);
435
436 vpTime::wait(t, sampling_time * 1000); // Wait ms
437 }
438
439 std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
440 }
441
442 if (opt_plot && plotter != NULL) {
443 vpDisplay::display(Iint);
444 sim.getInternalImage(Iint);
445 vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
446 vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
447 vpDisplay::displayText(Iint, 20, 20, "Click to quit", vpColor::red);
448 if (vpDisplay::getClick(Iint)) {
449 stop = true;
450 }
451 vpDisplay::flush(Iint);
452
453 delete plotter;
454 }
455
456 task.print();
457
458 return EXIT_SUCCESS;
459 } catch (const vpException &e) {
460 std::cout << "Catch an exception: " << e << std::endl;
461 return EXIT_FAILURE;
462 }
463}
464#elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
465int main()
466{
467 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
468 return EXIT_SUCCESS;
469}
470#else
471int main()
472{
473 std::cout << "You do not have X11, or GDI (Graphical Device Interface), or GTK functionalities to display images..." << std::endl;
474 std::cout << "Tip if you are on a unix-like system:" << std::endl;
475 std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
476 std::cout << "Tip if you are on a windows-like system:" << std::endl;
477 std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
478 return EXIT_SUCCESS;
479}
480
481#endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
static const vpColor red
Definition: vpColor.h:217
static const vpColor none
Definition: vpColor.h:229
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
Definition: vpCylinder.h:103
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Definition: vpDisplayD3D.h:107
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:135
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
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)
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0))
static void setWindowPosition(const vpImage< unsigned char > &I, int winx, int winy)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
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 line visual feature which is composed by two parameters that are and ,...
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static double rad(double deg)
Definition: vpMath.h:110
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition: vpPlot.h:116
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Definition: vpPlot.cpp:206
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
Definition: vpPlot.cpp:547
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
Definition: vpPlot.cpp:286
void setTitle(unsigned int graphNum, const std::string &title)
Definition: vpPlot.cpp:498
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void get_eJe(vpMatrix &eJe)
@ CAMERA_FRAME
Definition: vpRobot.h:82
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:567
@ EYEINHAND_L_cVe_eJe
Definition: vpServo.h:159
unsigned int getDimension() const
Return the task dimension.
Definition: vpServo.cpp:553
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:448
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 set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:506
vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator=false)
Definition: vpServo.cpp:1446
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
Class that defines the simplest robot: a free flying camera.
Implementation of a wire frame simulator. Compared to the vpSimulator class, it does not require thir...
vpHomogeneousMatrix getExternalCameraPosition() const
void setCameraPositionRelObj(const vpHomogeneousMatrix &cMo_)
void getInternalImage(vpImage< unsigned char > &I)
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
void setExternalCameraPosition(const vpHomogeneousMatrix &cam_Mf)
void set_fMo(const vpHomogeneousMatrix &fMo_)
vpHomogeneousMatrix get_fMo() const
void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_)
void setInternalCameraParameters(const vpCameraParameters &cam)
void setExternalCameraParameters(const vpCameraParameters &cam)
@ CYLINDER
A cylinder of 80cm length and 10cm radius.
void getExternalImage(vpImage< unsigned char > &I)
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()