ViSP
 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 ;
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();
47  }
48 
49  task.kill();
50 }
51