ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
tutorial-simu-pioneer.cpp
1 
18 #include <iostream>
19 
20 #include <visp/vpFeatureBuilder.h>
21 #include <visp/vpFeatureDepth.h>
22 #include <visp/vpFeaturePoint.h>
23 #include <visp/vpHomogeneousMatrix.h>
24 #include <visp/vpPlot.h>
25 #include <visp/vpServo.h>
26 #include <visp/vpSimulatorPioneer.h>
27 #include <visp/vpVelocityTwistMatrix.h>
28 
29 int main()
30 {
32  cdMo[1][3] = 1.2;
33  cdMo[2][3] = 0.5;
34 
36  cMo[0][3] = 0.3;
37  cMo[1][3] = cdMo[1][3];
38  cMo[2][3] = 1.;
39  vpRotationMatrix cRo(0, atan2( cMo[0][3], cMo[1][3]), 0);
40  cMo.insert(cRo);
41 
42  vpSimulatorPioneer robot ;
43  robot.setSamplingTime(0.04);
44  vpHomogeneousMatrix wMc, wMo;
45  robot.getPosition(wMc);
46  wMo = wMc * cMo;
47 
48  vpPoint point;
49  point.setWorldCoordinates(0,0,0);
50  point.track(cMo);
51 
52  vpServo task;
55  task.setLambda(0.2);
57  cVe = robot.get_cVe();
58  task.set_cVe(cVe);
59 
60  vpMatrix eJe;
61  robot.get_eJe(eJe);
62  task.set_eJe(eJe);
63 
64  vpFeaturePoint s_x, s_xd;
65  vpFeatureBuilder::create(s_x, point);
66  s_xd.buildFrom(0, 0, cdMo[2][3]);
67  task.addFeature(s_x, s_xd, vpFeaturePoint::selectX());
68 
69  vpFeatureDepth s_Z, s_Zd;
70  double Z = point.get_Z();
71  double Zd = cdMo[2][3];
72  s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z/Zd));
73  s_Zd.buildFrom(0, 0, Zd, 0);
74  task.addFeature(s_Z, s_Zd);
75 
76 #ifdef VISP_HAVE_DISPLAY
77  // Create a window (800 by 500) at position (400, 10) with 3 graphics
78  vpPlot graph(3, 800, 500, 400, 10, "Curves...");
79 
80  // Init the curve plotter
81  graph.initGraph(0,2);
82  graph.initGraph(1,2);
83  graph.initGraph(2,1);
84  graph.setTitle(0, "Velocities");
85  graph.setTitle(1, "Error s-s*");
86  graph.setTitle(2, "Depth");
87  graph.setLegend(0, 0, "vx");
88  graph.setLegend(0, 1, "wz");
89  graph.setLegend(1, 0, "x");
90  graph.setLegend(1, 1, "log(Z/Z*)");
91  graph.setLegend(2, 0, "Z");
92 #endif
93 
94  try
95  {
96  int iter = 1;
97  for (; ;)
98  {
99  robot.getPosition(wMc) ;
100  cMo = wMc.inverse() * wMo;
101 
102  point.track(cMo);
103 
104  vpFeatureBuilder::create(s_x, point);
105 
106  Z = point.get_Z() ;
107  s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z/Zd)) ;
108 
109  robot.get_cVe(cVe);
110  task.set_cVe(cVe);
111  robot.get_eJe(eJe);
112  task.set_eJe(eJe);
113 
114  vpColVector v = task.computeControlLaw();
116 
117 #ifdef VISP_HAVE_DISPLAY
118  graph.plot(0, iter, v); // plot velocities applied to the robot
119  graph.plot(1, iter, task.getError()); // plot error vector
120  graph.plot(2, 0, iter, Z); // plot the depth
121 #endif
122 
123  iter ++;
124 
125  if (task.getError().sumSquare() < 0.0001) {
126  std::cout << "Reached a small error. We stop the loop... " << std::endl;
127  break;
128  }
129  }
130 #ifdef VISP_HAVE_DISPLAY
131  graph.saveData(0, "./v2.dat");
132  graph.saveData(1, "./error2.dat");
133 
134  const char *legend = "Click to quit...";
135  vpDisplay::displayCharString(graph.I, graph.I.getHeight()-60, graph.I.getWidth()-150, legend, vpColor::red);
136  vpDisplay::flush(graph.I);
137  vpDisplay::getClick(graph.I);
138 #endif
139  }
140  catch(...)
141  {
142  }
143 
144  // Kill the servo task
145  task.print() ;
146  task.kill();
147 }