ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
tutorial-simu-pioneer-pan.cpp
1 
19 #include <iostream>
20 
21 #include <visp/vpFeatureBuilder.h>
22 #include <visp/vpFeatureDepth.h>
23 #include <visp/vpFeaturePoint.h>
24 #include <visp/vpHomogeneousMatrix.h>
25 #include <visp/vpPlot.h>
26 #include <visp/vpServo.h>
27 #include <visp/vpSimulatorPioneerPan.h>
28 #include <visp/vpVelocityTwistMatrix.h>
29 
30 int main()
31 {
32  // Set the position the camera has to reach
33  vpHomogeneousMatrix cdMo ;
34  cdMo[1][3] = 1.2; // t_y should be different from zero to be non singular
35  cdMo[2][3] = 0.5;
36 
37  // Set the initial camera position
39  cMo[0][3] = 0.3;
40  cMo[1][3] = cdMo[1][3];
41  cMo[2][3] = 1.;
42  vpRotationMatrix cdRo(0, atan2(cMo[0][3], cMo[1][3]), 0);
43  cMo.insert(cdRo);
44 
45  vpSimulatorPioneerPan robot ;
46  robot.setSamplingTime(0.04);
47  vpHomogeneousMatrix wMc, wMo;
48 
49  // Get robot position world frame
50  robot.getPosition(wMc);
51 
52  // Compute the position of the object in the world frame
53  wMo = wMc * cMo;
54 
55  // Define the target
56  vpPoint point;
57  point.setWorldCoordinates(0,0,0); // Coordinates in the object frame
58  point.track(cMo);
59 
60  vpServo task;
63  task.setLambda(0.2);
64 
66  cVe = robot.get_cVe();
67  task.set_cVe(cVe);
68 
69  vpMatrix eJe;
70  robot.get_eJe(eJe);
71  task.set_eJe(eJe);
72 
73  // Current and desired visual feature associated later to the x coordinate of the point
74  vpFeaturePoint s_x, s_xd;
75 
76  // Create the current x visual feature
77  vpFeatureBuilder::create(s_x, point);
78 
79  // Create the desired x* visual feature
80  s_xd.buildFrom(0, 0, cdMo[2][3]);
81 
82  // Add the feature
83  task.addFeature(s_x, s_xd, vpFeaturePoint::selectX());
84 
85  // Create the current and desired log(Z/Z*) visual feature
86  vpFeatureDepth s_Z, s_Zd;
87  // Initial depth of the target in front of the camera
88  double Z = point.get_Z();
89  // Desired depth Z* of the target.
90  double Zd = cdMo[2][3];
91  s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z/Zd));
92  s_Zd.buildFrom(0, 0, Zd, 0); // log(Z/Z*) = 0 that's why the last parameter is 0
93 
94  // Add the feature
95  task.addFeature(s_Z, s_Zd);
96 
97 #ifdef VISP_HAVE_DISPLAY
98  // Create a window (800 by 500) at position (400, 10) with 3 graphics
99  vpPlot graph(3, 800, 500, 400, 10, "Curves...");
100 
101  // Init the curve plotter
102  graph.initGraph(0,3);
103  graph.initGraph(1,2);
104  graph.initGraph(2,1);
105  graph.setTitle(0, "Velocities");
106  graph.setTitle(1, "Error s-s*");
107  graph.setTitle(2, "Depth");
108  graph.setLegend(0, 0, "vx");
109  graph.setLegend(0, 1, "wz");
110  graph.setLegend(0, 2, "qdot_pan");
111  graph.setLegend(1, 0, "x");
112  graph.setLegend(1, 1, "log(Z/Z*)");
113  graph.setLegend(2, 0, "Z");
114 #endif
115 
116  try
117  {
118  int iter = 1;
119  for (; ;)
120  {
121  robot.getPosition(wMc) ;
122  cMo = wMc.inverse() * wMo;
123 
124  point.track(cMo);
125 
126  // Update the current x feature
127  vpFeatureBuilder::create(s_x, point);
128 
129  // Update log(Z/Z*) feature. Since the depth Z change, we need to update the intection matrix
130  Z = point.get_Z() ;
131  s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z/Zd));
132 
133  robot.get_cVe(cVe);
134  task.set_cVe(cVe);
135  robot.get_eJe(eJe);
136  task.set_eJe(eJe);
137 
138  // Compute the control law. Velocities are computed in the mobile robot reference frame
139  vpColVector v = task.computeControlLaw();
140 
141  // Send the velocity to the robot
143 
144 #ifdef VISP_HAVE_DISPLAY
145  graph.plot(0, iter, v); // plot velocities applied to the robot
146  graph.plot(1, iter, task.getError()); // plot error vector
147  graph.plot(2, 0, iter, Z); // plot the depth
148 #endif
149  iter ++;
150 
151  if (task.getError().sumSquare() < 0.0001) {
152  std::cout << "Reached a small error. We stop the loop... " << std::endl;
153  break;
154  }
155  }
156 #ifdef VISP_HAVE_DISPLAY
157  const char *legend = "Click to quit...";
158  vpDisplay::displayCharString(graph.I, graph.I.getHeight()-60, graph.I.getWidth()-150, legend, vpColor::red);
159  vpDisplay::flush(graph.I);
160  vpDisplay::getClick(graph.I);
161 #endif
162  }
163  catch(...)
164  {
165  }
166 
167  // Kill the servo task
168  task.print();
169  task.kill();
170 }