Visual Servoing Platform version 3.5.0
servoSimuCylinder2DCamVelocityDisplaySecondaryTask.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 on a cylinder.
33 *
34 * Authors:
35 * Nicolas Melchior
36 *
37 *****************************************************************************/
54#include <iostream>
55#include <stdio.h>
56#include <stdlib.h>
57
58#include <visp3/core/vpCameraParameters.h>
59#include <visp3/core/vpCylinder.h>
60#include <visp3/core/vpHomogeneousMatrix.h>
61#include <visp3/core/vpImage.h>
62#include <visp3/core/vpMath.h>
63#include <visp3/gui/vpDisplayD3D.h>
64#include <visp3/gui/vpDisplayGDI.h>
65#include <visp3/gui/vpDisplayGTK.h>
66#include <visp3/gui/vpDisplayOpenCV.h>
67#include <visp3/gui/vpDisplayX.h>
68#include <visp3/gui/vpProjectionDisplay.h>
69#include <visp3/io/vpParseArgv.h>
70#include <visp3/robot/vpSimulatorCamera.h>
71#include <visp3/visual_features/vpFeatureBuilder.h>
72#include <visp3/visual_features/vpFeatureLine.h>
73#include <visp3/vs/vpServo.h>
74#include <visp3/vs/vpServoDisplay.h>
75
76// List of allowed command line options
77#define GETOPTARGS "cdho"
78
79void usage(const char *name, const char *badparam);
80bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
81
90void usage(const char *name, const char *badparam)
91{
92 fprintf(stdout, "\n\
93Simulation of a 2D visual servoing on a cylinder:\n\
94- eye-in-hand control law,\n\
95- velocity computed in the camera frame,\n\
96- display the camera view.\n\
97 \n\
98SYNOPSIS\n\
99 %s [-c] [-d] [-o] [-h]\n", name);
100
101 fprintf(stdout, "\n\
102OPTIONS: Default\n\
103 \n\
104 -c\n\
105 Disable the mouse click. Useful to automaze the \n\
106 execution of this program without humain intervention.\n\
107 \n\
108 -d \n\
109 Turn off the display.\n\
110 \n\
111 -o \n\
112 Disable new projection operator usage for secondary task.\n\
113 \n\
114 -h\n\
115 Print the help.\n");
116
117 if (badparam)
118 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
119}
120
133bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display, bool &new_proj_operator)
134{
135 const char *optarg_;
136 int c;
137 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
138
139 switch (c) {
140 case 'c':
141 click_allowed = false;
142 break;
143 case 'd':
144 display = false;
145 break;
146 case 'o':
147 new_proj_operator = false;
148 break;
149 case 'h':
150 usage(argv[0], NULL);
151 return false;
152
153 default:
154 usage(argv[0], optarg_);
155 return false;
156 }
157 }
158
159 if ((c == 1) || (c == -1)) {
160 // standalone param or error
161 usage(argv[0], NULL);
162 std::cerr << "ERROR: " << std::endl;
163 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
164 return false;
165 }
166
167 return true;
168}
169
170int main(int argc, const char **argv)
171{
172#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
173 try {
174 bool opt_display = true;
175 bool opt_click_allowed = true;
176 bool opt_new_proj_operator = true;
177
178 // Read the command line options
179 if (getOptions(argc, argv, opt_click_allowed, opt_display, opt_new_proj_operator) == false) {
180 exit(-1);
181 }
182
183 vpImage<unsigned char> Iint(512, 512, 0);
184 vpImage<unsigned char> Iext(512, 512, 0);
185
186// We open a window if a display is available
187#ifdef VISP_HAVE_DISPLAY
188# if defined VISP_HAVE_X11
189 vpDisplayX displayInt;
190 vpDisplayX displayExt;
191# elif defined VISP_HAVE_GTK
192 vpDisplayGTK displayInt;
193 vpDisplayGTK displayExt;
194# elif defined VISP_HAVE_GDI
195 vpDisplayGDI displayInt;
196 vpDisplayGDI displayExt;
197# elif defined VISP_HAVE_OPENCV
198 vpDisplayOpenCV displayInt;
199 vpDisplayOpenCV displayExt;
200# elif defined VISP_HAVE_D3D9
201 vpDisplayD3D displayInt;
202 vpDisplayD3D displayExt;
203#endif
204#endif
205
206 if (opt_display) {
207#ifdef VISP_HAVE_DISPLAY
208 // Display size is automatically defined by the image (Iint) and
209 // (Iext) size
210 displayInt.init(Iint, 100, 100, "Internal view");
211 displayExt.init(Iext, 130 + static_cast<int>(Iint.getWidth()), 100, "External view");
212#endif
213 // Display the image
214 // The image class has a member that specify a pointer toward
215 // the display that has been initialized in the display declaration
216 // therefore is is no longuer necessary to make a reference to the
217 // display variable.
218 vpDisplay::display(Iint);
219 vpDisplay::display(Iext);
220 vpDisplay::flush(Iint);
221 vpDisplay::flush(Iext);
222 }
223
224#ifdef VISP_HAVE_DISPLAY
225 vpProjectionDisplay externalview;
226#endif
227
228 // Set the camera parameters
229 double px, py;
230 px = py = 600;
231 double u0, v0;
232 u0 = v0 = 256;
233
234 vpCameraParameters cam(px, py, u0, v0);
235
236 vpServo task;
237 vpSimulatorCamera robot;
238
239 // sets the initial camera location
240 vpHomogeneousMatrix cMo(-0.2, 0.1, 2, vpMath::rad(5), vpMath::rad(5), vpMath::rad(20));
241
242 vpHomogeneousMatrix wMc, wMo;
243 robot.getPosition(wMc);
244 wMo = wMc * cMo; // Compute the position of the object in the world frame
245
246 // sets the final camera location (for simulation purpose)
247 vpHomogeneousMatrix cMod(0, 0, 1, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
248
249 // sets the cylinder coordinates in the world frame
250 vpCylinder cylinder(0, 1, 0, // direction
251 0, 0, 0, // point of the axis
252 0.1); // radius
253
254#ifdef VISP_HAVE_DISPLAY
255 externalview.insert(cylinder);
256#endif
257 // sets the desired position of the visual feature
258 cylinder.track(cMod);
259 cylinder.print();
260
261 // Build the desired line features thanks to the cylinder and especially
262 // its paramaters in the image frame
263 vpFeatureLine ld[2];
264 for (unsigned int i = 0; i < 2; i++)
265 vpFeatureBuilder::create(ld[i], cylinder, i);
266
267 // computes the cylinder coordinates in the camera frame and its 2D
268 // coordinates sets the current position of the visual feature
269 cylinder.track(cMo);
270 cylinder.print();
271
272 // Build the current line features thanks to the cylinder and especially
273 // its paramaters in the image frame
274 vpFeatureLine l[2];
275 for (unsigned int i = 0; i < 2; i++) {
276 vpFeatureBuilder::create(l[i], cylinder, i);
277 l[i].print();
278 }
279
280 // define the task
281 // - we want an eye-in-hand control law
282 // - robot is controlled in the camera frame
285 // it can also be interesting to test these possibilities
286 // task.setInteractionMatrixType(vpServo::CURRENT,vpServo::PSEUDO_INVERSE)
287 // ; task.setInteractionMatrixType(vpServo::MEAN, vpServo::PSEUDO_INVERSE)
288 // ; task.setInteractionMatrixType(vpServo::CURRENT,
289 // vpServo::PSEUDO_INVERSE) ;
290 // task.setInteractionMatrixType(vpServo::DESIRED, vpServo::TRANSPOSE) ;
291 // task.setInteractionMatrixType(vpServo::CURRENT, vpServo::TRANSPOSE) ;
292
293 // we want to see 2 lines on 2 lines
294 task.addFeature(l[0], ld[0]);
295 task.addFeature(l[1], ld[1]);
296
297 // Set the point of view of the external view
298 vpHomogeneousMatrix cextMo(0, 0, 6, vpMath::rad(40), vpMath::rad(10), vpMath::rad(60));
299
300 // Display the initial scene
301 vpServoDisplay::display(task, cam, Iint);
302#ifdef VISP_HAVE_DISPLAY
303 externalview.display(Iext, cextMo, cMo, cam, vpColor::red);
304#endif
305 vpDisplay::flush(Iint);
306 vpDisplay::flush(Iext);
307
308 // Display task information
309 task.print();
310
311 if (opt_display && opt_click_allowed) {
312 vpDisplay::displayText(Iint, 20, 20, "Click to start visual servo...", vpColor::white);
313 vpDisplay::flush(Iint);
315 }
316
317 // set the gain
318 task.setLambda(1);
319
320 // Display task information
321 task.print();
322
323 unsigned int iter = 0;
324 bool stop = false;
325 bool start_secondary_task = false;
326
327 while (!stop) {
328 std::cout << "---------------------------------------------" << iter++ << std::endl;
329
330 // get the robot position
331 robot.getPosition(wMc);
332 // Compute the position of the object frame in the camera frame
333 cMo = wMc.inverse() * wMo;
334
335 // new line position
336 // retrieve x,y and Z of the vpLine structure
337 // Compute the parameters of the cylinder in the camera frame and in the
338 // image frame
339 cylinder.track(cMo);
340
341 // Build the current line features thanks to the cylinder and especially
342 // its paramaters in the image frame
343 for (unsigned int i = 0; i < 2; i++) {
344 vpFeatureBuilder::create(l[i], cylinder, i);
345 }
346
347 // Display the current scene
348 if (opt_display) {
349 vpDisplay::display(Iint);
350 vpDisplay::display(Iext);
351 vpServoDisplay::display(task, cam, Iint);
352#ifdef VISP_HAVE_DISPLAY
353 externalview.display(Iext, cextMo, cMo, cam, vpColor::red);
354#endif
355 }
356
357 // compute the control law
359
360 // Wait primary task convergence before considering secondary task
361 if (task.getError().sumSquare() < 1e-6) {
362 start_secondary_task = true;
363 }
364
365 if (start_secondary_task) {
366 // In this example the secondary task is cut in four
367 // steps. The first one consists in imposing a movement of the robot along
368 // the x axis of the object frame with a velocity of 0.5. The second one
369 // consists in imposing a movement of the robot along the y axis of the
370 // object frame with a velocity of 0.5. The third one consists in imposing a
371 // movement of the robot along the x axis of the object frame with a
372 // velocity of -0.5. The last one consists in imposing a movement of the
373 // robot along the y axis of the object frame with a velocity of -0.5.
374 // Each steps is made during 200 iterations.
375 vpColVector e1(6);
376 vpColVector e2(6);
377 vpColVector proj_e1;
378 vpColVector proj_e2;
379 static unsigned int iter_sec = 0;
380 double rapport = 0;
381 double vitesse = 0.5;
382 unsigned int tempo = 800;
383
384 if (iter_sec > tempo) {
385 stop = true;
386 }
387
388 if (iter_sec % tempo < 200) {
389 e2 = 0;
390 e1[0] = fabs(vitesse);
391 proj_e1 = task.secondaryTask(e1, opt_new_proj_operator);
392 rapport = vitesse / proj_e1[0];
393 proj_e1 *= rapport;
394 v += proj_e1;
395 }
396
397 if (iter_sec % tempo < 400 && iter_sec % tempo >= 200) {
398 e1 = 0;
399 e2[1] = fabs(vitesse);
400 proj_e2 = task.secondaryTask(e2, opt_new_proj_operator);
401 rapport = vitesse / proj_e2[1];
402 proj_e2 *= rapport;
403 v += proj_e2;
404 }
405
406 if (iter_sec % tempo < 600 && iter_sec % tempo >= 400) {
407 e2 = 0;
408 e1[0] = -fabs(vitesse);
409 proj_e1 = task.secondaryTask(e1, opt_new_proj_operator);
410 rapport = -vitesse / proj_e1[0];
411 proj_e1 *= rapport;
412 v += proj_e1;
413 }
414
415 if (iter_sec % tempo < 800 && iter_sec % tempo >= 600) {
416 e1 = 0;
417 e2[1] = -fabs(vitesse);
418 proj_e2 = task.secondaryTask(e2, opt_new_proj_operator);
419 rapport = -vitesse / proj_e2[1];
420 proj_e2 *= rapport;
421 v += proj_e2;
422 }
423
424 if (opt_display && opt_click_allowed) {
425 std::stringstream ss;
426 ss << std::string("New projection operator: ") + (opt_new_proj_operator ? std::string("yes (use option -o to use old one)") : std::string("no"));
427 vpDisplay::displayText(Iint, 20, 20, "Secondary task enabled: yes", vpColor::white);
428 vpDisplay::displayText(Iint, 40, 20, ss.str(), vpColor::white);
429 }
430
431 iter_sec ++;
432 }
433 else {
434 if (opt_display && opt_click_allowed) {
435 vpDisplay::displayText(Iint, 20, 20, "Secondary task: no", vpColor::white);
436 }
437 }
438
439 // send the camera velocity to the controller
441
442 std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
443
444 if (opt_display) {
445 vpDisplay::displayText(Iint, 60, 20, "Click to stop visual servo...", vpColor::white);
446 if (vpDisplay::getClick(Iint, false)) {
447 stop = true;
448 }
449 vpDisplay::flush(Iint);
450 vpDisplay::flush(Iext);
451 }
452
453 iter++;
454 }
455
456 if (opt_display && opt_click_allowed) {
457 vpDisplay::display(Iint);
458 vpServoDisplay::display(task, cam, Iint);
459 vpDisplay::displayText(Iint, 20, 20, "Click to quit...", vpColor::white);
460 vpDisplay::flush(Iint);
462 }
463
464 // Display task information
465 task.print();
466 return EXIT_SUCCESS;
467 } catch (const vpException &e) {
468 std::cout << "Catch a ViSP exception: " << e << std::endl;
469 return EXIT_FAILURE;
470 }
471#else
472 (void)argc;
473 (void)argv;
474 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
475 return EXIT_SUCCESS;
476#endif
477}
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
double sumSquare() const
static const vpColor white
Definition: vpColor.h:212
static const vpColor red
Definition: vpColor.h:217
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
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)
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 ,...
void print(unsigned int select=FEATURE_ALL) const
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
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
interface with the image for feature display
void display(vpImage< unsigned char > &I, const vpHomogeneousMatrix &cextMo, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color, const bool &displayTraj=false, unsigned int thickness=1)
void insert(vpForwardProjection &fp)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ CAMERA_FRAME
Definition: vpRobot.h:82
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)
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
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
@ PSEUDO_INVERSE
Definition: vpServo.h:202
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.