Visual Servoing Platform version 3.5.0
simulateCircle2DCamVelocity.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 visual servoing with visualization.
33 *
34 * Authors:
35 * Eric Marchand
36 * Fabien Spindler
37 *
38 *****************************************************************************/
39
52#include <visp3/core/vpConfig.h>
53#include <visp3/core/vpDebug.h>
54
55#ifdef VISP_HAVE_COIN3D_AND_GUI
56#include <visp3/ar/vpSimulator.h>
57#include <visp3/core/vpCameraParameters.h>
58#include <visp3/core/vpCircle.h>
59#include <visp3/core/vpHomogeneousMatrix.h>
60#include <visp3/core/vpImage.h>
61#include <visp3/core/vpIoTools.h>
62#include <visp3/core/vpMath.h>
63#include <visp3/core/vpTime.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/vpFeatureEllipse.h>
68#include <visp3/vs/vpServo.h>
69
70#define GETOPTARGS "cdi:h"
71#define SAVE 0
72
82void usage(const char *name, const char *badparam, std::string ipath)
83{
84 fprintf(stdout, "\n\
85Simulation Servo Circle\n\
86 \n\
87SYNOPSIS\n\
88 %s [-i <input image path>] [-d] [-h]\n", name);
89
90 fprintf(stdout, "\n\
91OPTIONS: Default\n\
92 -i <input image path> %s\n\
93 Set image input path.\n\
94 From this path read \"iv/4points.iv\"\n\
95 cad model.\n\
96 Setting the VISP_INPUT_IMAGE_PATH environment\n\
97 variable produces the same behaviour than using\n\
98 this option.\n\
99 \n\
100 -d \n\
101 Disable the image display. This can be useful \n\
102 for automatic tests using crontab under Unix or \n\
103 using the task manager under Windows.\n\
104 \n\
105 -h\n\
106 Print the help.\n\n", ipath.c_str());
107
108 if (badparam)
109 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
110}
111
127bool getOptions(int argc, const char **argv, std::string &ipath, bool &display)
128{
129 const char *optarg;
130 int c;
131 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
132
133 switch (c) {
134 case 'i':
135 ipath = optarg;
136 break;
137 case 'd':
138 display = false;
139 break;
140 case 'h':
141 usage(argv[0], NULL, ipath);
142 return false;
143 break;
144
145 default:
146 usage(argv[0], optarg, ipath);
147 return false;
148 break;
149 }
150 }
151
152 if ((c == 1) || (c == -1)) {
153 // standalone param or error
154 usage(argv[0], NULL, ipath);
155 std::cerr << "ERROR: " << std::endl;
156 std::cerr << " Bad argument " << optarg << std::endl << std::endl;
157 return false;
158 }
159
160 return true;
161}
162
163static void *mainLoop(void *_simu)
164{
165 vpSimulator *simu = static_cast<vpSimulator *>(_simu);
166 simu->initMainApplication();
167
168 vpPoseVector vcMo;
169
170 vcMo[0] = 0.3;
171 vcMo[1] = 0.2;
172 vcMo[2] = 3;
173 vcMo[3] = 0;
174 vcMo[4] = vpMath::rad(45);
175 vcMo[5] = vpMath::rad(40);
176 vpHomogeneousMatrix cMo(vcMo);
177 vpHomogeneousMatrix wMo; // Set to identity
178 vpHomogeneousMatrix wMc; // Robot (=camera) location in the world frame
179
181 cMod[0][3] = 0;
182 cMod[1][3] = 0;
183 cMod[2][3] = 1;
184
185 int it = 0;
186 unsigned int pos = 2;
187 while (pos != 0) {
188 vpServo task;
189 vpSimulatorCamera robot;
190
191 float sampling_time = 0.040f; // Sampling period in second
192 robot.setSamplingTime(sampling_time);
194
195 // Sets the initial camera location
196 wMc = wMo * cMo.inverse();
197 robot.setPosition(wMc);
198 simu->setCameraPosition(cMo);
199
200 if (pos == 1)
201 cMod[2][3] = 0.32;
202
203 // Sets the circle coordinates in the world frame
204 vpCircle circle;
205 circle.setWorldCoordinates(0, 0, 1, 0, 0, 0, 0.1);
206
207 // Sets the desired position of the visual feature
209 circle.track(cMod);
210 vpFeatureBuilder::create(pd, circle);
211
212 // Project : computes the circle coordinates in the camera frame and its
213 // 2D coordinates Sets the current position of the visual feature
215 circle.track(cMo);
216 vpFeatureBuilder::create(p, circle);
217
218 // Define the task
219 // We want an eye-in-hand control law
220 // Robot is controlled in the camera frame
223
224 // We want to see a circle on a circle
225 std::cout << std::endl;
226 task.addFeature(p, pd);
227
228 // Set the gain
229 task.setLambda(1.0);
230
231 // Display task information
232 task.print();
233
234 vpTime::wait(1000); // Sleep 1s
235
236 unsigned int iter = 0;
237 // Visual servoing loop
238 unsigned int itermax;
239 if (pos == 2)
240 itermax = 75;
241 else
242 itermax = 100;
243 while (iter++ < itermax) {
244 double t = vpTime::measureTimeMs();
245
246 if (iter == 1)
247 std::cout << "get the robot position" << std::endl;
248 wMc = robot.getPosition();
249 if (iter == 1)
250 std::cout << "new circle position" << std::endl;
251 // retrieve x,y and Z of the vpCircle structure
252
253 cMo = wMc.inverse() * wMo;
254 circle.track(cMo);
255 vpFeatureBuilder::create(p, circle);
256
257 if (iter == 1)
258 std::cout << "compute the control law" << std::endl;
260 if (iter == 1) {
261 std::cout << "Task rank: " << task.getTaskRank() << std::endl;
262 std::cout << "send the camera velocity to the controller" << std::endl;
263 }
265
266 simu->setCameraPosition(cMo);
267
268 if (SAVE == 1) {
269 char name[FILENAME_MAX];
270 sprintf(name, "/tmp/image.%04d.external.png", it);
271 std::cout << "Save " << name << std::endl;
272 simu->write(name);
273 sprintf(name, "/tmp/image.%04u.internal.png", iter);
274 std::cout << "Save " << name << std::endl;
275 simu->write(name);
276 it++;
277 }
278 // std::cout << "\t\t || s - s* || "
279 // std::cout << ( task.getError() ).sumSquare() <<std::endl ; ;
280 vpTime::wait(t, sampling_time * 1000); // Wait 40 ms
281 }
282 pos--;
283 }
284
285 simu->closeMainApplication();
286
287 void *a = NULL;
288 return a;
289}
290
291int main(int argc, const char **argv)
292{
293 try {
294 std::string env_ipath;
295 std::string opt_ipath;
296 std::string ipath;
297 std::string filename;
298 bool opt_display = true;
299
300 // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
301 // environment variable value
303
304 // Set the default input path
305 if (!env_ipath.empty())
306 ipath = env_ipath;
307
308 // Read the command line options
309 if (getOptions(argc, argv, opt_ipath, opt_display) == false) {
310 exit(-1);
311 }
312
313 // Get the option values
314 if (!opt_ipath.empty())
315 ipath = opt_ipath;
316
317 // Compare ipath and env_ipath. If they differ, we take into account
318 // the input path comming from the command line option
319 if (!opt_ipath.empty() && !env_ipath.empty()) {
320 if (ipath != env_ipath) {
321 std::cout << std::endl << "WARNING: " << std::endl;
322 std::cout << " Since -i <visp image path=" << ipath << "> "
323 << " is different from VISP_INPUT_IMAGE_PATH=" << env_ipath << std::endl
324 << " we skip the environment variable." << std::endl;
325 }
326 }
327
328 // Test if an input path is set
329 if (opt_ipath.empty() && env_ipath.empty()) {
330 usage(argv[0], NULL, ipath);
331 std::cerr << std::endl << "ERROR:" << std::endl;
332 std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
333 << " environment variable to specify the location of the " << std::endl
334 << " image path where test images are located." << std::endl
335 << std::endl;
336 exit(-1);
337 }
338
341 fMo[2][3] = 0;
342
343 if (opt_display) {
344
345 vpSimulator simu;
346 simu.initInternalViewer(300, 300);
347 simu.initExternalViewer(300, 300);
348
349 vpTime::wait(1000);
350 simu.setZoomFactor(1.0f);
351 simu.addAbsoluteFrame();
352
353 // Load the cad model
354 filename = vpIoTools::createFilePath(ipath, "iv/circle.iv");
355 simu.load(filename.c_str(), fMo);
356
358
359 simu.initApplication(&mainLoop);
360 simu.mainLoop();
361 }
362 return EXIT_SUCCESS;
363 } catch (const vpException &e) {
364 std::cout << "Catch an exception: " << e << std::endl;
365 return EXIT_FAILURE;
366 }
367}
368
369#else
370int main()
371{
372 std::cout << "You do not have Coin3D and SoQT or SoWin or SoXt functionalities enabled..." << std::endl;
373 std::cout << "Tip:" << std::endl;
374 std::cout << "- Install Coin3D and SoQT or SoWin or SoXt, configure ViSP again using cmake and build again this example" << std::endl;
375 return EXIT_SUCCESS;
376}
377#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:92
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCircle.cpp:60
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
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 2D ellipse visual feature.
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1365
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:1670
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
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ CAMERA_FRAME
Definition: vpRobot.h:82
void setMaxTranslationVelocity(double maxVt)
Definition: vpRobot.cpp:239
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:567
@ EYEINHAND_CAMERA
Definition: vpServo.h:155
unsigned int getTaskRank() const
Definition: vpServo.cpp:1786
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
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:218
vpColVector computeControlLaw()
Definition: vpServo.cpp:929
@ CURRENT
Definition: vpServo.h:182
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.
Implementation of a simulator based on Coin3d (www.coin3d.org).
Definition: vpSimulator.h:100
void load(const char *file_name)
load an iv file
void setInternalCameraParameters(vpCameraParameters &cam)
set internal camera parameters
virtual void mainLoop()
activate the mainloop
void initMainApplication()
perform some initialization in the main program thread
void initApplication(void *(*start_routine)(void *))
begin the main program
void setZoomFactor(float zoom)
set the size of the camera/frame
void setCameraPosition(vpHomogeneousMatrix &cMf)
set the camera position (from an homogeneous matrix)
void initExternalViewer(unsigned int nlig, unsigned int ncol)
initialize the external view
void write(const char *fileName)
virtual void initInternalViewer(unsigned int nlig, unsigned int ncol)
initialize the camera view
void closeMainApplication()
void addAbsoluteFrame(float zoom=1)
Add the representation of the absolute frame.
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()