Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoSimuCircle2DCamVelocityDisplay.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 circle.
33 *
34*****************************************************************************/
35
46#include <visp3/core/vpConfig.h>
47#include <visp3/core/vpDebug.h>
48
49#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) && \
50 (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
51
52#include <stdio.h>
53#include <stdlib.h>
54
55#include <visp3/core/vpCameraParameters.h>
56#include <visp3/core/vpCircle.h>
57#include <visp3/core/vpHomogeneousMatrix.h>
58#include <visp3/core/vpImage.h>
59#include <visp3/core/vpMath.h>
60#include <visp3/gui/vpDisplayGDI.h>
61#include <visp3/gui/vpDisplayGTK.h>
62#include <visp3/gui/vpDisplayOpenCV.h>
63#include <visp3/gui/vpDisplayX.h>
64#include <visp3/io/vpParseArgv.h>
65#include <visp3/robot/vpSimulatorCamera.h>
66#include <visp3/visual_features/vpFeatureBuilder.h>
67#include <visp3/visual_features/vpFeatureLine.h>
68#include <visp3/vs/vpServo.h>
69#include <visp3/vs/vpServoDisplay.h>
70
71// List of allowed command line options
72#define GETOPTARGS "cdh"
73
74void usage(const char *name, const char *badparam);
75bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
76
85void usage(const char *name, const char *badparam)
86{
87 fprintf(stdout, "\n\
88Simulation of a 2D visual servoing on a circle:\n\
89- eye-in-hand control law,\n\
90- velocity computed in the camera frame,\n\
91- display the camera view.\n\
92 \n\
93SYNOPSIS\n\
94 %s [-c] [-d] [-h]\n",
95 name);
96
97 fprintf(stdout, "\n\
98OPTIONS: Default\n\
99 \n\
100 -c\n\
101 Disable the mouse click. Useful to automate the \n\
102 execution of this program without human intervention.\n\
103 \n\
104 -d \n\
105 Turn off the display.\n\
106 \n\
107 -h\n\
108 Print the help.\n");
109
110 if (badparam) {
111 fprintf(stderr, "ERROR: \n");
112 fprintf(stderr, "\nBad parameter [%s]\n", badparam);
113 }
114}
115
128bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
129{
130 const char *optarg_;
131 int c;
132 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
133
134 switch (c) {
135 case 'c':
136 click_allowed = false;
137 break;
138 case 'd':
139 display = 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_click_allowed = true;
167
168 // Read the command line options
169 if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
170 return (EXIT_FAILURE);
171 }
172
173 vpImage<unsigned char> I(512, 512, 0);
174
175 // We open a window using either X11, GTK or GDI.
176#if defined(VISP_HAVE_X11)
177 vpDisplayX display;
178#elif defined(VISP_HAVE_GTK)
179 vpDisplayGTK display;
180#elif defined(VISP_HAVE_GDI)
181 vpDisplayGDI display;
182#elif defined(HAVE_OPENCV_HIGHGUI)
183 vpDisplayOpenCV display;
184#endif
185
186 if (opt_display) {
187 // Display size is automatically defined by the image (I) size
188 display.init(I, 100, 100, "Camera view...");
189 // Display the image
190 // The image class has a member that specify a pointer toward
191 // the display that has been initialized in the display declaration
192 // therefore is is no longer necessary to make a reference to the
193 // display variable.
196 }
197
198 double px = 600, py = 600;
199 double u0 = I.getWidth() / 2., v0 = I.getHeight() / 2.;
200
201 vpCameraParameters cam(px, py, u0, v0);
202
203 vpServo task;
204 vpSimulatorCamera robot;
205
206 // sets the initial camera location
207 vpHomogeneousMatrix cMo(0, 0, 1, vpMath::rad(0), vpMath::rad(80), vpMath::rad(30));
208 vpHomogeneousMatrix wMc, wMo;
209 robot.getPosition(wMc);
210 wMo = wMc * cMo; // Compute the position of the object in the world frame
211
212 vpHomogeneousMatrix cMod(-0.1, -0.1, 0.7, vpMath::rad(40), vpMath::rad(10), vpMath::rad(30));
213
214 // sets the circle coordinates in the world frame
215 vpCircle circle;
216 circle.setWorldCoordinates(0, 0, 1, 0, 0, 0, 0.1);
217
218 // sets the desired position of the visual feature
220 circle.track(cMod);
221 vpFeatureBuilder::create(pd, circle);
222
223 // project : computes the circle coordinates in the camera frame and its
224 // 2D coordinates sets the current position of the visual feature
226 circle.track(cMo);
227 vpFeatureBuilder::create(p, circle);
228
229 // define the task
230 // - we want an eye-in-hand control law
231 // - robot is controlled in the camera frame
234 // - we want to see a circle on a circle
235 task.addFeature(p, pd);
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 circle position
254 // retrieve x,y and Z of the vpCircle structure
255 circle.track(cMo);
256 vpFeatureBuilder::create(p, circle);
257 circle.print();
258 p.print();
259
260 if (opt_display) {
262 vpServoDisplay::display(task, cam, I);
264 }
265
266 // compute the control law
267 v = task.computeControlLaw();
268 std::cout << "task rank: " << task.getTaskRank() << std::endl;
269 // send the camera velocity to the controller
271
272 std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
273 }
274
275 // Display task information
276 task.print();
277
278 if (opt_display && opt_click_allowed) {
279 vpDisplay::displayText(I, 20, 20, "Click to quit...", vpColor::white);
282 }
283 return EXIT_SUCCESS;
284 }
285 catch (const vpException &e) {
286 std::cout << "Catch a ViSP exception: " << e << std::endl;
287 return EXIT_FAILURE;
288 }
289}
290#elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
291int main()
292{
293 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
294 return EXIT_SUCCESS;
295}
296#else
297int main()
298{
299 std::cout << "You do not have X11, or GTK, or GDI (Graphical Device Interface) or OpenCV functionalities to display "
300 "images..."
301 << std::endl;
302 std::cout << "Tip if you are on a unix-like system:" << std::endl;
303 std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
304 std::cout << "Tip if you are on a windows-like system:" << std::endl;
305 std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
306 return EXIT_SUCCESS;
307}
308#endif
Generic class defining intrinsic camera parameters.
Class that defines a 3D circle in the object frame and allows forward projection of a 3D circle in th...
Definition vpCircle.h:87
void setWorldCoordinates(const vpColVector &oP)
Definition vpCircle.cpp:57
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 print(unsigned int select=FEATURE_ALL) const
print the name of the feature
virtual void print() const
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)
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
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
@ 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.