ViSP
Main Page
Related Pages
Modules
Classes
Examples
All
Classes
Functions
Variables
Enumerations
Enumerator
Friends
Groups
Pages
manSimu4Dots.cpp
1
/****************************************************************************
2
*
3
* $Id: manSimu4Dots.cpp 4056 2013-01-05 13:04:42Z fspindle $
4
*
5
* This file is part of the ViSP software.
6
* Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7
*
8
* This software is free software; you can redistribute it and/or
9
* modify it under the terms of the GNU General Public License
10
* ("GPL") version 2 as published by the Free Software Foundation.
11
* See the file LICENSE.txt at the root directory of this source
12
* distribution for additional information about the GNU GPL.
13
*
14
* For using ViSP with software that can not be combined with the GNU
15
* GPL, please contact INRIA about acquiring a ViSP Professional
16
* Edition License.
17
*
18
* See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19
*
20
* This software was developed at:
21
* INRIA Rennes - Bretagne Atlantique
22
* Campus Universitaire de Beaulieu
23
* 35042 Rennes Cedex
24
* France
25
* http://www.irisa.fr/lagadic
26
*
27
* If you have questions regarding the use of this file, please contact
28
* INRIA at visp@inria.fr
29
*
30
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32
*
33
* Description:
34
* Simulation of a visual servoing with visualization and image generation.
35
*
36
* Authors:
37
* Eric Marchand
38
* Fabien Spindler
39
*
40
*****************************************************************************/
41
54
#include <visp/vpConfig.h>
55
#include <visp/vpDebug.h>
56
57
58
#if (defined(VISP_HAVE_COIN_AND_GUI) && (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)))
59
60
#include <visp/vpImage.h>
61
#include <visp/vpCameraParameters.h>
62
#include <visp/vpTime.h>
63
#include <visp/vpImage.h>
64
#include <visp/vpImageConvert.h>
65
#include <visp/vpSimulator.h>
66
67
#if defined(VISP_HAVE_X11)
68
# include <visp/vpDisplayX.h>
69
#elif defined(VISP_HAVE_GDI)
70
# include <visp/vpDisplayGDI.h>
71
#elif defined(VISP_HAVE_GTK)
72
# include <visp/vpDisplayGTK.h>
73
#endif
74
// You may have strange compiler issues using the simulator based on SoQt
75
// and the vpDisplayGTK. In that case prefer to use another display like
76
// vpDisplayX under linux or vpDisplayGDI under Windows
77
#include <visp/vpMath.h>
78
#include <visp/vpHomogeneousMatrix.h>
79
#include <visp/vpPose.h>
80
#include <visp/vpFeaturePoint.h>
81
#include <visp/vpDot2.h>
82
#include <visp/vpServo.h>
83
#include <visp/vpServoDisplay.h>
84
#include <visp/vpRobotCamera.h>
85
#include <visp/vpFeatureBuilder.h>
86
#include <visp/vpIoTools.h>
87
88
static
89
void
*mainLoop (
void
*_simu)
90
{
91
// pointer copy of the vpSimulator instance
92
vpSimulator
*simu = (
vpSimulator
*)_simu ;
93
94
// Simulation initialization
95
simu->
initMainApplication
() ;
96
98
// Set the initial camera location
99
vpHomogeneousMatrix
cMo(0.3,0.2,3,
100
vpMath::rad
(0),
vpMath::rad
(0),
vpMath::rad
(40));
101
103
// Initialize the robot
104
vpRobotCamera
robot ;
105
robot.
setSamplingTime
(0.04);
// 40ms
106
robot.
setPosition
(cMo) ;
107
// Send the robot position to the visualizator
108
simu->
setCameraPosition
(cMo) ;
109
// Initialize the camera parameters
110
vpCameraParameters
cam ;
111
simu->
getCameraParameters
(cam);
112
114
// Desired visual features initialization
115
116
// sets the points coordinates in the object frame (in meter)
117
vpPoint
point[4] ;
118
point[0].
setWorldCoordinates
(-0.1,-0.1,0) ;
119
point[1].
setWorldCoordinates
(0.1,-0.1,0) ;
120
point[2].
setWorldCoordinates
(0.1,0.1,0) ;
121
point[3].
setWorldCoordinates
(-0.1,0.1,0) ;
122
123
// sets the desired camera location
124
vpHomogeneousMatrix
cMo_d(0,0,1,0,0,0) ;
125
126
// computes the 3D point coordinates in the camera frame and its 2D coordinates
127
for
(
int
i = 0 ; i < 4 ; i++)
128
point[i].project(cMo_d) ;
129
130
// creates the associated features
131
vpFeaturePoint
pd[4] ;
132
for
(
int
i = 0 ; i < 4 ; i++)
133
vpFeatureBuilder::create
(pd[i],point[i]) ;
134
135
136
138
// Current visual features initialization
139
unsigned
int
height = simu->
getInternalHeight
();
140
unsigned
int
width = simu->
getInternalWidth
();
141
142
// Create a greyscale image
143
vpImage<unsigned char>
I(height,width);
144
145
//Display initialization
146
#if defined(VISP_HAVE_X11)
147
vpDisplayX
disp;
148
#elif defined(VISP_HAVE_GDI)
149
vpDisplayGDI
disp;
150
#elif defined(VISP_HAVE_GTK)
151
vpDisplayGTK
disp;
152
#endif
153
disp.
init
(I,100,100,
"Simulation display"
);
154
// disp(I);
155
// Get the current image
156
vpTime::wait
(500);
// wait to be sure the image is generated
157
simu->
getInternalImage
(I);
158
159
// Display the current image
160
vpDisplay::display
(I);
161
vpDisplay::flush
(I);
162
163
// Initialize the four dots tracker
164
std::cout <<
"A click in the four dots clockwise. "
<< std::endl;
165
vpDot2
dot[4];
166
vpFeaturePoint
p[4];
167
for
(
int
i = 0 ; i < 4 ; i++)
168
{
169
dot[i].
setGraphics
(
true
);
170
// Call for a click
171
std::cout <<
"A click in the dot "
<< i << std::endl;
172
dot[i].
initTracking
(I);
173
// Create the associated feature
174
vpFeatureBuilder::create
(p[i],cam,dot[i]);
175
// flush the display
176
vpDisplay::flush
(I);
177
}
178
179
181
// Task defintion
182
vpServo
task ;
183
// we want an eye-in-hand control law ;
184
task.
setServo
(
vpServo::EYEINHAND_L_cVe_eJe
) ;
185
task.
setInteractionMatrixType
(
vpServo::DESIRED
) ;
186
187
// Set the position of the camera in the end-effector frame
188
vpHomogeneousMatrix
cMe ;
189
vpVelocityTwistMatrix
cVe(cMe) ;
190
task.
set_cVe
(cVe) ;
191
// Set the Jacobian (expressed in the end-effector frame)
192
vpMatrix
eJe ;
193
robot.
get_eJe
(eJe) ;
194
task.
set_eJe
(eJe) ;
195
196
// we want to see a point on a point
197
for
(
int
i = 0 ; i < 4 ; i++)
198
task.
addFeature
(p[i],pd[i]) ;
199
// Set the gain
200
task.
setLambda
(1.0) ;
201
// Print the current information about the task
202
task.
print
();
203
204
vpTime::wait
(500);
205
207
// The control loop
208
int
k = 0;
209
while
(k++ < 200){
210
double
t =
vpTime::measureTimeMs
();
211
212
// Get the current internal camera view and display it
213
simu->
getInternalImage
(I);
214
vpDisplay::display
(I);
215
216
// Track the four dots and update the associated visual features
217
for
(
int
i = 0 ; i < 4 ; i++)
218
{
219
dot[i].
track
(I) ;
220
vpFeatureBuilder::create
(p[i],cam,dot[i]) ;
221
}
222
223
// Display the desired and current visual features
224
vpServoDisplay::display
(task,cam,I) ;
225
vpDisplay::flush
(I);
226
227
// Update the robot Jacobian
228
robot.
get_eJe
(eJe) ;
229
task.
set_eJe
(eJe) ;
230
231
// Compute the control law
232
vpColVector
v = task.
computeControlLaw
() ;
233
234
// Send the computed velocity to the robot and compute the new robot position
235
robot.
setVelocity
(
vpRobot::ARTICULAR_FRAME
, v) ;
236
robot.
getPosition
(cMo) ;
237
238
// Send the robot position to the visualizator
239
simu->
setCameraPosition
(cMo) ;
240
241
242
// Wait 40 ms
243
vpTime::wait
(t,40);
244
}
245
// Print information about the task
246
task.
print
();
247
task.
kill
();
248
simu->
closeMainApplication
() ;
249
250
void
*a=NULL ;
251
return
a ;
252
}
253
254
255
int
256
main()
257
{
258
vpSimulator
simu ;
259
260
// Internal view initialization : view from the robot camera
261
simu.
initInternalViewer
(480, 360) ;
262
// External view initialization : view from an external camera
263
simu.
initExternalViewer
(300, 300) ;
264
265
// Inernal camera paramters initialization
266
vpCameraParameters
cam(800,800,240,180) ;
267
simu.
setInternalCameraParameters
(cam) ;
268
269
vpTime::wait
(500) ;
270
// Load the scene
271
std::cout <<
"Load : ./4Points.iv"
<< std::endl
272
<<
"This file should be in the working directory"
<< std::endl;
273
274
simu.
load
(
"./4points.iv"
) ;
275
276
// Run the main loop
277
simu.
initApplication
(&mainLoop) ;
278
// Run the simulator
279
simu.
mainLoop
() ;
280
}
281
282
#else
283
int
284
main()
285
{
vpTRACE
(
"You should install Coin3D and SoQT or SoWin or SoXt"
) ;
286
287
}
288
#endif
example
manual
simulation
manSimu4Dots.cpp
Generated on Tue Sep 17 2013 00:21:34 for ViSP by
1.8.4