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>
40 cMo[1][3] = cdMo[1][3];
88 double Z = point.
get_Z();
90 double Zd = cdMo[2][3];
97 #ifdef VISP_HAVE_DISPLAY
99 vpPlot graph(3, 800, 500, 400, 10,
"Curves...");
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");
144 #ifdef VISP_HAVE_DISPLAY
145 graph.plot(0, iter, v);
146 graph.plot(1, iter, task.
getError());
147 graph.plot(2, 0, iter, Z);
152 std::cout <<
"Reached a small error. We stop the loop... " << std::endl;
156 #ifdef VISP_HAVE_DISPLAY
157 const char *legend =
"Click to quit...";