ViSP
Main Page
Related Pages
Modules
Classes
Examples
All
Classes
Functions
Variables
Enumerations
Enumerator
Friends
Groups
Pages
tutorial-ibvs-4pts.cpp
1
2
#include <visp/vpFeatureBuilder.h>
3
#include <visp/vpServo.h>
4
#include <visp/vpSimulatorCamera.h>
5
6
int
main()
7
{
8
vpHomogeneousMatrix
cdMo(0, 0, 0.75, 0, 0, 0);
9
vpHomogeneousMatrix
cMo(0.15, -0.1, 1.,
10
vpMath::rad
(10),
vpMath::rad
(-10),
vpMath::rad
(50));
11
12
vpPoint
point[4] ;
13
point[0].
setWorldCoordinates
(-0.1,-0.1, 0);
14
point[1].
setWorldCoordinates
( 0.1,-0.1, 0);
15
point[2].
setWorldCoordinates
( 0.1, 0.1, 0);
16
point[3].
setWorldCoordinates
(-0.1, 0.1, 0);
17
18
vpServo
task ;
19
task.
setServo
(
vpServo::EYEINHAND_CAMERA
);
20
task.
setInteractionMatrixType
(
vpServo::CURRENT
);
21
task.
setLambda
(0.5);
22
23
vpFeaturePoint
p[4], pd[4] ;
24
for
(
int
i = 0 ; i < 4 ; i++) {
25
point[i].
track
(cdMo);
26
vpFeatureBuilder::create
(pd[i], point[i]);
27
point[i].
track
(cMo);
28
vpFeatureBuilder::create
(p[i], point[i]);
29
task.
addFeature
(p[i], pd[i]);
30
}
31
32
vpHomogeneousMatrix
wMc, wMo;
33
vpSimulatorCamera
robot;
34
robot.
setSamplingTime
(0.040);
35
robot.
getPosition
(wMc);
36
wMo = wMc * cMo;
37
38
for
(
unsigned
int
iter=0; iter < 150; iter ++) {
39
robot.
getPosition
(wMc);
40
cMo = wMc.
inverse
() * wMo;
41
for
(
int
i = 0 ; i < 4 ; i++) {
42
point[i].
track
(cMo);
43
vpFeatureBuilder::create
(p[i], point[i]);
44
}
45
vpColVector
v = task.
computeControlLaw
();
46
robot.
setVelocity
(
vpRobot::CAMERA_FRAME
, v);
47
}
48
49
task.
kill
();
50
}
51
tutorial
visual-servo
ibvs
tutorial-ibvs-4pts.cpp
Generated on Fri Sep 27 2013 21:09:18 for ViSP by
1.8.4