Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoSimuSphere2DCamVelocityDisplaySecondaryTask.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 sphere.
33 *
34*****************************************************************************/
35
46#include <stdio.h>
47#include <stdlib.h>
48
49#include <visp3/core/vpHomogeneousMatrix.h>
50#include <visp3/core/vpMath.h>
51#include <visp3/core/vpSphere.h>
52#include <visp3/gui/vpDisplayD3D.h>
53#include <visp3/gui/vpDisplayGDI.h>
54#include <visp3/gui/vpDisplayGTK.h>
55#include <visp3/gui/vpDisplayOpenCV.h>
56#include <visp3/gui/vpDisplayX.h>
57#include <visp3/gui/vpProjectionDisplay.h>
58#include <visp3/io/vpParseArgv.h>
59#include <visp3/robot/vpSimulatorCamera.h>
60#include <visp3/visual_features/vpFeatureBuilder.h>
61#include <visp3/visual_features/vpFeatureEllipse.h>
62#include <visp3/vs/vpServo.h>
63#include <visp3/vs/vpServoDisplay.h>
64
65// List of allowed command line options
66#define GETOPTARGS "cdho"
67
68void usage(const char *name, const char *badparam);
69bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
70
79void usage(const char *name, const char *badparam)
80{
81 fprintf(stdout, "\n\
82Simulation of a 2D visual servoing on a sphere:\n\
83- eye-in-hand control law,\n\
84- velocity computed in the camera frame,\n\
85- display the camera view,\n\
86- a secondary task is the added.\n\
87 \n\
88SYNOPSIS\n\
89 %s [-c] [-d] [-o] [-h]\n",
90 name);
91
92 fprintf(stdout, "\n\
93OPTIONS: Default\n\
94 \n\
95 -c\n\
96 Disable the mouse click. Useful to automate the \n\
97 execution of this program without human intervention.\n\
98 \n\
99 -d \n\
100 Turn off the display.\n\
101 \n\
102 -o \n\
103 Disable new projection operator usage for secondary task.\n\
104 \n\
105 -h\n\
106 Print the help.\n");
107
108 if (badparam)
109 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
110}
111
125bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display, bool &new_proj_operator)
126{
127 const char *optarg_;
128 int c;
129 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
130
131 switch (c) {
132 case 'c':
133 click_allowed = false;
134 break;
135 case 'd':
136 display = false;
137 break;
138 case 'o':
139 new_proj_operator = 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#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
165 try {
166 bool opt_display = true;
167 bool opt_click_allowed = true;
168 bool opt_new_proj_operator = true;
169
170 // Read the command line options
171 if (getOptions(argc, argv, opt_click_allowed, opt_display, opt_new_proj_operator) == false) {
172 return (EXIT_FAILURE);
173 }
174
175 vpImage<unsigned char> I(512, 512, 0);
176 vpImage<unsigned char> Iext(512, 512, 0);
177
178 // We open a window if a display is available
179#ifdef VISP_HAVE_DISPLAY
180#if defined(VISP_HAVE_X11)
181 vpDisplayX displayI;
182 vpDisplayX displayExt;
183#elif defined(VISP_HAVE_GTK)
184 vpDisplayGTK displayI;
185 vpDisplayGTK displayExt;
186#elif defined(VISP_HAVE_GDI)
187 vpDisplayGDI displayI;
188 vpDisplayGDI displayExt;
189#elif defined(HAVE_OPENCV_HIGHGUI)
190 vpDisplayOpenCV displayI;
191 vpDisplayOpenCV displayExt;
192#elif defined(VISP_HAVE_D3D9)
193 vpDisplayD3D displayI;
194 vpDisplayD3D displayExt;
195#endif
196#endif
197
198 if (opt_display) {
199#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
200 // Display size is automatically defined by the image (I) size
201 displayI.init(I, 100, 100, "Camera view...");
202 displayExt.init(Iext, 130 + static_cast<int>(I.getWidth()), 100, "External view");
203#endif
204 // Display the image
205 // The image class has a member that specify a pointer toward
206 // the display that has been initialized in the display declaration
207 // therefore is is no longer necessary to make a reference to the
208 // display variable.
210 vpDisplay::display(Iext);
212 vpDisplay::flush(Iext);
213 }
214
215#ifdef VISP_HAVE_DISPLAY
216 vpProjectionDisplay externalview;
217#endif
218
219 double px = 600, py = 600;
220 double u0 = I.getWidth() / 2., v0 = I.getHeight() / 2.;
221
222 vpCameraParameters cam(px, py, u0, v0);
223
224 vpServo task;
225 vpSimulatorCamera robot;
226
227 // sets the initial camera location
229 cMo[0][3] = 0.1;
230 cMo[1][3] = 0.2;
231 cMo[2][3] = 2;
232 // Compute the position of the object in the world frame
233 vpHomogeneousMatrix wMc, wMo;
234 robot.getPosition(wMc);
235 wMo = wMc * cMo;
236
238 cMod[0][3] = 0;
239 cMod[1][3] = 0;
240 cMod[2][3] = 1;
241
242 // sets the sphere coordinates in the world frame
243 vpSphere sphere;
244 sphere.setWorldCoordinates(0, 0, 0, 0.1);
245
246#ifdef VISP_HAVE_DISPLAY
247 externalview.insert(sphere);
248#endif
249 // sets the desired position of the visual feature
251 sphere.track(cMod);
252 vpFeatureBuilder::create(pd, sphere);
253
254 // computes the sphere coordinates in the camera frame and its 2D
255 // coordinates sets the current position of the visual feature
257 sphere.track(cMo);
258 vpFeatureBuilder::create(p, sphere);
259
260 // define the task
261 // - we want an eye-in-hand control law
262 // - robot is controlled in the camera frame
264
265 // we want to see a sphere on a sphere
266 std::cout << std::endl;
267 task.addFeature(p, pd);
268
269 // set the gain
270 task.setLambda(1);
271
272 // Set the point of view of the external view
273 vpHomogeneousMatrix cextMo(0, 0, 4, vpMath::rad(40), vpMath::rad(10), vpMath::rad(60));
274
275 // Display the initial scene
276 vpServoDisplay::display(task, cam, I);
277#ifdef VISP_HAVE_DISPLAY
278 externalview.display(Iext, cextMo, cMo, cam, vpColor::red);
279#endif
281 vpDisplay::flush(Iext);
282
283 // Display task information
284 task.print();
285
286 if (opt_display && opt_click_allowed) {
287 vpDisplay::displayText(I, 20, 20, "Click to start visual servo...", vpColor::white);
290 }
291
292 unsigned int iter = 0;
293 bool stop = false;
294 bool start_secondary_task = false;
295
296 // loop
297 while (iter++ < 2000 && !stop) {
298 std::cout << "---------------------------------------------" << iter << std::endl;
299
300 // get the robot position
301 robot.getPosition(wMc);
302 // Compute the position of the object frame in the camera frame
303 cMo = wMc.inverse() * wMo;
304
305 // new sphere position: retrieve x,y and Z of the vpSphere structure
306 sphere.track(cMo);
307 vpFeatureBuilder::create(p, sphere);
308
309 if (opt_display) {
311 vpDisplay::display(Iext);
312 vpServoDisplay::display(task, cam, I);
313#ifdef VISP_HAVE_DISPLAY
314 externalview.display(Iext, cextMo, cMo, cam, vpColor::red);
315#endif
316 }
317
318 // compute the control law
320
321 // Wait primary task convergence before considering secondary task
322 if (task.getError().sumSquare() < 1e-6) {
323 start_secondary_task = true;
324 }
325
326 if (start_secondary_task) {
327 // Only 3 dof are required to achieve primary task: vz, wx, wy
328 // It remains 3 free dof (vx, vy, wz) that could be used in a secondary task for example to move around the
329 // sphere
330 vpColVector de2dt(6);
331 de2dt[0] = 0.50; // vx = 0.50 m/s should also generate a motion on wy = (I-WpW)de2dt[4]
332 de2dt[1] = 0.25; // vy = 0.25 m/s should generate a motion on wx = (I-WpW)de2dt[3]
333 de2dt[2] = 1; // vz = 1 m/s should be zero in vz = (I-WpW)de2dt[2]
334 de2dt[5] = vpMath::rad(10); // wz = 10 rad/s should generate a motion on (I-WpW)de2dt[5]
335
336 std::cout << "de2dt :" << de2dt.t() << std::endl;
337 vpColVector sec = task.secondaryTask(de2dt, opt_new_proj_operator);
338 std::cout << "(I-WpW)de2dt :" << sec.t() << std::endl;
339
340 v += sec;
341
342 if (opt_display && opt_click_allowed) {
343 std::stringstream ss;
344 ss << std::string("New projection operator: ") +
345 (opt_new_proj_operator ? std::string("yes (use option -o to use old one)") : std::string("no"));
346 vpDisplay::displayText(I, 20, 20, "Secondary task enabled: yes", vpColor::white);
347 vpDisplay::displayText(I, 40, 20, ss.str(), vpColor::white);
348 }
349 } else {
350 if (opt_display && opt_click_allowed) {
351 vpDisplay::displayText(I, 20, 20, "Secondary task enabled: no", vpColor::white);
352 }
353 }
354
355 // send the camera velocity to the controller
357
358 std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
359
360 if (opt_display) {
361 vpDisplay::displayText(I, 60, 20, "Click to stop visual servo...", vpColor::white);
362 if (vpDisplay::getClick(I, false)) {
363 stop = true;
364 }
366 vpDisplay::flush(Iext);
367 }
368 }
369
370 if (opt_display && opt_click_allowed) {
372 vpServoDisplay::display(task, cam, I);
373 vpDisplay::displayText(I, 20, 20, "Click to quit...", vpColor::white);
376 }
377
378 // Display task information
379 task.print();
380 return EXIT_SUCCESS;
381 } catch (const vpException &e) {
382 std::cout << "Catch a ViSP exception: " << e << std::endl;
383 return EXIT_FAILURE;
384 }
385#else
386 (void)argc;
387 (void)argv;
388 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
389 return EXIT_SUCCESS;
390#endif
391}
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
double sumSquare() const
vpRowVector t() const
static const vpColor white
Definition vpColor.h:206
static const vpColor red
Definition vpColor.h:211
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 2D ellipse visual feature.
void track(const vpHomogeneousMatrix &cMo)
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
unsigned int getWidth() const
Definition vpImage.h:242
unsigned int getHeight() const
Definition vpImage.h:184
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)
@ 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
vpColVector computeControlLaw()
Definition vpServo.cpp:930
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.
Class that defines a 3D sphere in the object frame and allows forward projection of a 3D sphere in th...
Definition vpSphere.h:78
void setWorldCoordinates(const vpColVector &oP)
Definition vpSphere.cpp:59