Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
manSimu4Dots.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 visual servoing with visualization and image generation.
33 *
34*****************************************************************************/
35
48#include <visp3/core/vpConfig.h>
49#include <visp3/core/vpDebug.h>
50
51#if (defined(VISP_HAVE_COIN3D_AND_GUI) && (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)))
52
53#include <visp3/ar/vpSimulator.h>
54#include <visp3/core/vpCameraParameters.h>
55#include <visp3/core/vpImage.h>
56#include <visp3/core/vpImageConvert.h>
57#include <visp3/core/vpTime.h>
58
59#if defined(VISP_HAVE_X11)
60#include <visp3/gui/vpDisplayX.h>
61#elif defined(VISP_HAVE_GDI)
62#include <visp3/gui/vpDisplayGDI.h>
63#elif defined(VISP_HAVE_GTK)
64#include <visp3/gui/vpDisplayGTK.h>
65#endif
66// You may have strange compiler issues using the simulator based on SoQt
67// and the vpDisplayGTK. In that case prefer to use another display like
68// vpDisplayX under linux or vpDisplayGDI under Windows
69#include <visp3/blob/vpDot2.h>
70#include <visp3/core/vpHomogeneousMatrix.h>
71#include <visp3/core/vpIoTools.h>
72#include <visp3/core/vpMath.h>
73#include <visp3/robot/vpSimulatorCamera.h>
74#include <visp3/vision/vpPose.h>
75#include <visp3/visual_features/vpFeatureBuilder.h>
76#include <visp3/visual_features/vpFeaturePoint.h>
77#include <visp3/vs/vpServo.h>
78#include <visp3/vs/vpServoDisplay.h>
79
80static void *mainLoop(void *_simu)
81{
82 // pointer copy of the vpSimulator instance
83 vpSimulator *simu = static_cast<vpSimulator *>(_simu);
84
85 // Simulation initialization
86 simu->initMainApplication();
87
89 // Set the initial camera location
90 vpHomogeneousMatrix cMo(0.3, 0.2, 3, vpMath::rad(0), vpMath::rad(0), vpMath::rad(40));
91 vpHomogeneousMatrix wMo; // Set to identity
92 vpHomogeneousMatrix wMc; // Camera position in the world frame
93
95 // Initialize the robot
97 robot.setSamplingTime(0.04); // 40ms
98 wMc = wMo * cMo.inverse();
99 robot.setPosition(wMc);
100 // Send the robot position to the visualizator
101 simu->setCameraPosition(cMo);
102 // Initialize the camera parameters
104 simu->getCameraParameters(cam);
105
107 // Desired visual features initialization
108
109 // sets the points coordinates in the object frame (in meter)
110 vpPoint point[4];
111 point[0].setWorldCoordinates(-0.1, -0.1, 0);
112 point[1].setWorldCoordinates(0.1, -0.1, 0);
113 point[2].setWorldCoordinates(0.1, 0.1, 0);
114 point[3].setWorldCoordinates(-0.1, 0.1, 0);
115
116 // sets the desired camera location
117 vpHomogeneousMatrix cMo_d(0, 0, 1, 0, 0, 0);
118
119 // computes the 3D point coordinates in the camera frame and its 2D
120 // coordinates
121 for (int i = 0; i < 4; i++)
122 point[i].project(cMo_d);
123
124 // creates the associated features
125 vpFeaturePoint pd[4];
126 for (int i = 0; i < 4; i++)
127 vpFeatureBuilder::create(pd[i], point[i]);
128
130 // Current visual features initialization
131 unsigned int height = simu->getInternalHeight();
132 unsigned int width = simu->getInternalWidth();
133
134 // Create a greyscale image
135 vpImage<unsigned char> I(height, width);
136
137 // Display initialization
138#if defined(VISP_HAVE_X11)
139 vpDisplayX disp;
140#elif defined(VISP_HAVE_GDI)
141 vpDisplayGDI disp;
142#elif defined(VISP_HAVE_GTK)
143 vpDisplayGTK disp;
144#endif
145 disp.init(I, 100, 100, "Simulation display");
146 // disp(I);
147 // Get the current image
148 vpTime::wait(500); // wait to be sure the image is generated
149 simu->getInternalImage(I);
150
151 // Display the current image
154
155 // Initialize the four dots tracker
156 std::cout << "A click in the four dots clockwise. " << std::endl;
157 vpDot2 dot[4];
158 vpFeaturePoint p[4];
159 for (int i = 0; i < 4; i++) {
160 dot[i].setGraphics(true);
161 // Call for a click
162 std::cout << "A click in the dot " << i << std::endl;
163 dot[i].initTracking(I);
164 // Create the associated feature
165 vpFeatureBuilder::create(p[i], cam, dot[i]);
166 // flush the display
168 }
169
171 // Task defintion
172 vpServo task;
173 // we want an eye-in-hand control law ;
176
177 // Set the position of the end-effector frame in the camera frame as identity
179 vpVelocityTwistMatrix cVe(cMe);
180 task.set_cVe(cVe);
181 // Set the Jacobian (expressed in the end-effector frame)
182 vpMatrix eJe;
183 robot.get_eJe(eJe);
184 task.set_eJe(eJe);
185
186 // we want to see a point on a point
187 for (int i = 0; i < 4; i++)
188 task.addFeature(p[i], pd[i]);
189 // Set the gain
190 task.setLambda(1.0);
191 // Print the current information about the task
192 task.print();
193
194 vpTime::wait(500);
195
197 // The control loop
198 int k = 0;
199 while (k++ < 200) {
200 double t = vpTime::measureTimeMs();
201
202 // Get the current internal camera view and display it
203 simu->getInternalImage(I);
205
206 // Track the four dots and update the associated visual features
207 for (int i = 0; i < 4; i++) {
208 dot[i].track(I);
209 vpFeatureBuilder::create(p[i], cam, dot[i]);
210 }
211
212 // Display the desired and current visual features
213 vpServoDisplay::display(task, cam, I);
215
216 // Update the robot Jacobian
217 robot.get_eJe(eJe);
218 task.set_eJe(eJe);
219
220 // Compute the control law
222
223 // Send the computed velocity to the robot and compute the new robot
224 // position
226 wMc = robot.getPosition();
227 cMo = wMc.inverse() * wMo;
228
229 // Send the robot position to the visualizator
230 simu->setCameraPosition(cMo);
231
232 // Wait 40 ms
233 vpTime::wait(t, 40);
234 }
235 // Print information about the task
236 task.print();
237 simu->closeMainApplication();
238
239 void *a = NULL;
240 return a;
241}
242
243int main()
244{
245 try {
246 vpSimulator simu;
247
248 // Internal view initialization : view from the robot camera
249 simu.initInternalViewer(480, 360);
250 // External view initialization : view from an external camera
251 simu.initExternalViewer(300, 300);
252
253 // Inernal camera parameters initialization
254 vpCameraParameters cam(800, 800, 240, 180);
256
257 vpTime::wait(500);
258 // Load the scene
259
260 // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
261 // environment variable value
262 std::string ipath = vpIoTools::getViSPImagesDataPath();
263 std::string filename = "./4points.iv";
264
265 // Set the default input path
266 if (!ipath.empty())
267 filename = vpIoTools::createFilePath(ipath, "iv/4points.iv");
268
269 std::cout << "Load : " << filename << std::endl << "This file should be in the working directory" << std::endl;
270
271 simu.load(filename.c_str());
272
273 // Run the main loop
274 simu.initApplication(&mainLoop);
275 // Run the simulator
276 simu.mainLoop();
277 return EXIT_SUCCESS;
278 }
279 catch (const vpException &e) {
280 std::cout << "Catch an exception: " << e << std::endl;
281 return EXIT_FAILURE;
282 }
283}
284
285#else
286int main()
287{
288 std::cout
289 << "You do not have X11, GTK, or OpenCV, or GDI (Graphical Device Interface) functionalities to display images..."
290 << std::endl;
291 std::cout << "Tip if you are on a unix-like system:" << std::endl;
292 std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
293 std::cout << "Tip if you are on a windows-like system:" << std::endl;
294 std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
295 return EXIT_SUCCESS;
296}
297#endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
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...
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 void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
Definition vpDot2.h:124
void track(const vpImage< unsigned char > &I, bool canMakeTheWindowGrow=true)
Definition vpDot2.cpp:441
void setGraphics(bool activate)
Definition vpDot2.h:311
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
Definition vpDot2.cpp:252
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 point visual feature which is composed by two parameters that are the cartes...
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 std::string getViSPImagesDataPath()
static std::string createFilePath(const std::string &parent, const std::string &child)
static double rad(double deg)
Definition vpMath.h:116
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
void setWorldCoordinates(double oX, double oY, double oZ)
Definition vpPoint.cpp:110
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void get_eJe(vpMatrix &eJe)
@ ARTICULAR_FRAME
Definition vpRobot.h:76
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_L_cVe_eJe
Definition vpServo.h:155
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition vpServo.h:448
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 set_eJe(const vpMatrix &eJe_)
Definition vpServo.h:506
void setServo(const vpServoType &servo_type)
Definition vpServo.cpp:210
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.
Implementation of a simulator based on Coin3d (www.coin3d.org).
Definition vpSimulator.h:99
unsigned int getInternalWidth() const
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 getCameraParameters(vpCameraParameters &cam)
get the intrinsic parameters of the camera
void getInternalImage(vpImage< unsigned char > &I)
get an Image of the internal view
void initMainApplication()
perform some initialization in the main program thread
void initApplication(void *(*start_routine)(void *))
begin the main program
void setCameraPosition(vpHomogeneousMatrix &cMf)
set the camera position (from an homogeneous matrix)
void initExternalViewer(unsigned int nlig, unsigned int ncol)
initialize the external view
unsigned int getInternalHeight() const
virtual void initInternalViewer(unsigned int nlig, unsigned int ncol)
initialize the camera view
void closeMainApplication()
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()