Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoSimuSphere2DCamVelocityDisplay.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
45#include <stdio.h>
46#include <stdlib.h>
47
48#include <visp3/core/vpHomogeneousMatrix.h>
49#include <visp3/core/vpMath.h>
50#include <visp3/core/vpSphere.h>
51#include <visp3/gui/vpDisplayGDI.h>
52#include <visp3/gui/vpDisplayGTK.h>
53#include <visp3/gui/vpDisplayOpenCV.h>
54#include <visp3/gui/vpDisplayX.h>
55#include <visp3/io/vpParseArgv.h>
56#include <visp3/robot/vpSimulatorCamera.h>
57#include <visp3/visual_features/vpFeatureBuilder.h>
58#include <visp3/visual_features/vpFeatureEllipse.h>
59#include <visp3/vs/vpServo.h>
60#include <visp3/vs/vpServoDisplay.h>
61
62// List of allowed command line options
63#define GETOPTARGS "cdh"
64
65void usage(const char *name, const char *badparam);
66bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
67
76void usage(const char *name, const char *badparam)
77{
78 fprintf(stdout, "\n\
79Simulation of a 2D visual servoing on a sphere:\n\
80- eye-in-hand control law,\n\
81- velocity computed in the camera frame,\n\
82- display the camera view.\n\
83 \n\
84SYNOPSIS\n\
85 %s [-c] [-d] [-h]\n",
86 name);
87
88 fprintf(stdout, "\n\
89OPTIONS: Default\n\
90 \n\
91 -c\n\
92 Disable the mouse click. Useful to automate the \n\
93 execution of this program without human intervention.\n\
94 \n\
95 -d \n\
96 Turn off the display.\n\
97 \n\
98 -h\n\
99 Print the help.\n");
100
101 if (badparam)
102 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
103}
104
117bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
118{
119 const char *optarg_;
120 int c;
121 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
122
123 switch (c) {
124 case 'c':
125 click_allowed = false;
126 break;
127 case 'd':
128 display = false;
129 break;
130 case 'h':
131 usage(argv[0], NULL);
132 return false;
133
134 default:
135 usage(argv[0], optarg_);
136 return false;
137 }
138 }
139
140 if ((c == 1) || (c == -1)) {
141 // standalone param or error
142 usage(argv[0], NULL);
143 std::cerr << "ERROR: " << std::endl;
144 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
145 return false;
146 }
147
148 return true;
149}
150
151int main(int argc, const char **argv)
152{
153#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
154 try {
155 bool opt_display = true;
156 bool opt_click_allowed = true;
157
158 // Read the command line options
159 if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
160 return (EXIT_FAILURE);
161 }
162
163 vpImage<unsigned char> I(512, 512, 0);
164
165// We open a window using either X11, GTK or GDI.
166#if defined(VISP_HAVE_X11)
167 vpDisplayX display;
168#elif defined(VISP_HAVE_GTK)
169 vpDisplayGTK display;
170#elif defined(VISP_HAVE_GDI)
171 vpDisplayGDI display;
172#elif defined(HAVE_OPENCV_HIGHGUI)
173 vpDisplayOpenCV display;
174#endif
175
176 if (opt_display) {
177#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
178 // Display size is automatically defined by the image (I) size
179 display.init(I, 100, 100, "Camera view...");
180#endif
181 // Display the image
182 // The image class has a member that specify a pointer toward
183 // the display that has been initialized in the display declaration
184 // therefore is is no longer necessary to make a reference to the
185 // display variable.
188 }
189
190 double px = 600, py = 600;
191 double u0 = I.getWidth() / 2., v0 = I.getHeight() / 2.;
192
193 vpCameraParameters cam(px, py, u0, v0);
194
195 vpServo task;
196 vpSimulatorCamera robot;
197
198 // sets the initial camera location
200 cMo[0][3] = 0.1;
201 cMo[1][3] = 0.2;
202 cMo[2][3] = 2;
203 // Compute the position of the object in the world frame
204 vpHomogeneousMatrix wMc, wMo;
205 robot.getPosition(wMc);
206 wMo = wMc * cMo;
207
209 cMod[0][3] = 0;
210 cMod[1][3] = 0;
211 cMod[2][3] = 1;
212
213 // sets the sphere coordinates in the world frame
214 vpSphere sphere;
215 sphere.setWorldCoordinates(0, 0, 0, 0.1);
216
217 // sets the desired position of the visual feature
219 sphere.track(cMod);
220 vpFeatureBuilder::create(pd, sphere);
221
222 // computes the sphere coordinates in the camera frame and its 2D
223 // coordinates sets the current position of the visual feature
225 sphere.track(cMo);
226 vpFeatureBuilder::create(p, sphere);
227
228 // define the task
229 // - we want an eye-in-hand control law
230 // - robot is controlled in the camera frame
232
233 // we want to see a sphere on a sphere
234 task.addFeature(p, pd);
235
236 // set the gain
237 task.setLambda(1);
238
239 // Display task information
240 task.print();
241
242 unsigned int iter = 0;
243 // loop
244 while (iter++ < 200) {
245 std::cout << "---------------------------------------------" << iter << std::endl;
246 vpColVector v;
247
248 // get the robot position
249 robot.getPosition(wMc);
250 // Compute the position of the object frame in the camera frame
251 cMo = wMc.inverse() * wMo;
252
253 // new sphere position: retrieve x,y and Z of the vpSphere structure
254 sphere.track(cMo);
255 vpFeatureBuilder::create(p, sphere);
256
257 if (opt_display) {
259 vpServoDisplay::display(task, cam, I);
261 }
262
263 // compute the control law
264 v = task.computeControlLaw();
265
266 std::cout << "Task rank: " << task.getTaskRank() << std::endl;
267 // send the camera velocity to the controller
269
270 std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
271 }
272
273 if (opt_display && opt_click_allowed) {
274 vpDisplay::displayText(I, 20, 20, "Click to quit...", vpColor::white);
277 }
278
279 // Display task information
280 task.print();
281 return EXIT_SUCCESS;
282 } catch (const vpException &e) {
283 std::cout << "Catch a ViSP exception: " << e << std::endl;
284 return EXIT_FAILURE;
285 }
286#else
287 (void)argc;
288 (void)argv;
289 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
290 return EXIT_SUCCESS;
291#endif
292}
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
static const vpColor white
Definition vpColor.h:206
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
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 bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
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
unsigned int getTaskRank() const
Definition vpServo.cpp:1796
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
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