Example that shows how to simulate a visual servoing on a Pioneer mobile robot equipped with a camera. The current visual features that are used are s = (x, log(Z/Z*)). The desired one are s* = (x*, 0), with:
- x the abscisse of the point measured at each iteration
- x* the desired abscisse position of the point (x* = 0)
- Z the depth of the point measured at each iteration
- Z* the desired depth of the point equal to the initial one.
The degrees of freedom that are controlled are (vx, wz), where wz is the rotational velocity and vx the translational velocity of the mobile platform at point M located at the middle between the two wheels.
The feature x allows to control wy, while log(Z/Z*) allows to control vz.
#include <iostream>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/gui/vpPlot.h>
#include <visp3/robot/vpSimulatorPioneer.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureDepth.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
int main()
{
try {
cdMo[1][3] = 1.2;
cdMo[2][3] = 0.5;
cMo[0][3] = 0.3;
cMo[1][3] = cdMo[1][3];
cMo[2][3] = 1.;
wMo = wMc * cMo;
double Z = point.
get_Z();
double Zd = cdMo[2][3];
#ifdef VISP_HAVE_DISPLAY
vpPlot graph(3, 800, 500, 400, 10,
"Curves...");
#endif
int iter = 0;
for (;;) {
#ifdef VISP_HAVE_DISPLAY
graph.
plot(2, 0, iter, Z);
#endif
iter++;
std::cout << "Reached a small error. We stop the loop... " << std::endl;
break;
}
}
#ifdef VISP_HAVE_DISPLAY
const char *legend = "Click to quit...";
#endif
std::cout << "Catch an exception: " << e << std::endl;
}
}