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