Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
tutorial-simu-pioneer-pan.cpp
1 
19 #include <iostream>
20 
21 #include <visp3/visual_features/vpFeatureBuilder.h>
22 #include <visp3/visual_features/vpFeatureDepth.h>
23 #include <visp3/visual_features/vpFeaturePoint.h>
24 #include <visp3/core/vpHomogeneousMatrix.h>
25 #include <visp3/gui/vpPlot.h>
26 #include <visp3/vs/vpServo.h>
27 #include <visp3/robot/vpSimulatorPioneerPan.h>
28 #include <visp3/core/vpVelocityTwistMatrix.h>
29 
30 int main()
31 {
32  try {
33  // Set the position the camera has to reach
34  vpHomogeneousMatrix cdMo ;
35  cdMo[1][3] = 1.2; // t_y should be different from zero to be non singular
36  cdMo[2][3] = 0.5;
37 
38  // Set the initial camera position
40  cMo[0][3] = 0.3;
41  cMo[1][3] = cdMo[1][3];
42  cMo[2][3] = 1.;
43  vpRotationMatrix cdRo(0, atan2(cMo[0][3], cMo[1][3]), 0);
44  cMo.insert(cdRo);
45 
46  vpSimulatorPioneerPan robot ;
47  robot.setSamplingTime(0.04);
48  vpHomogeneousMatrix wMc, wMo;
49 
50  // Get robot position world frame
51  robot.getPosition(wMc);
52 
53  // Compute the position of the object in the world frame
54  wMo = wMc * cMo;
55 
56  // Define the target
57  vpPoint point(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  int iter = 0;
117  for (; ;)
118  {
119  robot.getPosition(wMc) ;
120  cMo = wMc.inverse() * wMo;
121 
122  point.track(cMo);
123 
124  // Update the current x feature
125  vpFeatureBuilder::create(s_x, point);
126 
127  // Update log(Z/Z*) feature. Since the depth Z change, we need to update the intection matrix
128  Z = point.get_Z() ;
129  s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z/Zd));
130 
131  robot.get_cVe(cVe);
132  task.set_cVe(cVe);
133  robot.get_eJe(eJe);
134  task.set_eJe(eJe);
135 
136  // Compute the control law. Velocities are computed in the mobile robot reference frame
137  vpColVector v = task.computeControlLaw();
138 
139  // Send the velocity to the robot
141 
142 #ifdef VISP_HAVE_DISPLAY
143  graph.plot(0, iter, v); // plot velocities applied to the robot
144  graph.plot(1, iter, task.getError()); // plot error vector
145  graph.plot(2, 0, iter, Z); // plot the depth
146 #endif
147  iter ++;
148 
149  if (task.getError().sumSquare() < 0.0001) {
150  std::cout << "Reached a small error. We stop the loop... " << std::endl;
151  break;
152  }
153  }
154 #ifdef VISP_HAVE_DISPLAY
155  const char *legend = "Click to quit...";
156  vpDisplay::displayText(graph.I, (int)graph.I.getHeight()-60, (int)graph.I.getWidth()-150, legend, vpColor::red);
157  vpDisplay::flush(graph.I);
158  vpDisplay::getClick(graph.I);
159 #endif
160 
161  // Kill the servo task
162  task.print();
163  task.kill();
164  }
165  catch(vpException &e) {
166  std::cout << "Catch an exception: " << e << std::endl;
167  }
168 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:460
vpVelocityTwistMatrix get_cVe() const
Definition: vpUnicycle.h:87
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
void buildFrom(const double x, const double y, const double Z, const double LogZoverZstar)
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:512
Class that defines a 3D point visual feature which is composed by one parameters that is that defin...
error that can be emited by ViSP classes.
Definition: vpException.h:73
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
static void flush(const vpImage< unsigned char > &I)
static const vpColor red
Definition: vpColor.h:163
Class that defines what is a point.
Definition: vpPoint.h:59
Implementation of a rotation matrix and operations on such kind of matrices.
virtual void setSamplingTime(const double &delta_t)
void insert(const vpRotationMatrix &R)
void kill()
Definition: vpServo.cpp:191
vpColVector getError() const
Definition: vpServo.h:271
vpColVector computeControlLaw()
Definition: vpServo.cpp:954
static unsigned int selectX()
void setLambda(double c)
Definition: vpServo.h:391
Implementation of a velocity twist matrix and operations on such kind of matrices.
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:585
void buildFrom(const double x, const double y, const double Z)
double sumSquare() const
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
double get_y() const
double get_x() const
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:435
vpHomogeneousMatrix inverse() const
Class that defines the Pioneer mobile robot simulator equipped with a camera able to move in pan...
void getPosition(vpHomogeneousMatrix &wMc) const
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:314
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition: vpPlot.h:113
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:222