 |
Visual Servoing Platform
version 3.2.0
|
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;
}
}
void plot(const unsigned int graphNum, const unsigned int curveNum, const double x, const double y)
void buildFrom(const double x, const double y, const double Z, const double LogZoverZstar)
void get_cVe(vpVelocityTwistMatrix &cVe) const
void buildFrom(const double x, const double y, const double Z)
double get_Z() const
Get the point Z coordinate in the camera frame.
void set_eJe(const vpMatrix &eJe_)
vpImage< unsigned char > I
void insert(const vpRotationMatrix &R)
double getSamplingTime() const
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void set_cVe(const vpVelocityTwistMatrix &cVe_)
void saveData(const unsigned int graphNum, const std::string &dataFile, const std::string &title_prefix="")
Implementation of column vector and the associated operations.
Implementation of a matrix and operations on matrices.
void setServo(const vpServoType &servo_type)
vpHomogeneousMatrix getPosition() const
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
static unsigned int selectX()
void setTitle(const unsigned int graphNum, const std::string &title)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Implementation of a rotation matrix and operations on such kind of matrices.
unsigned int getHeight() const
vpColVector getError() const
Class that defines a 3D point visual feature which is composed by one parameters that is that defin...
void get_eJe(vpMatrix &eJe)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
vpColVector computeControlLaw()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
static void flush(const vpImage< unsigned char > &I)
virtual void setSamplingTime(const double &delta_t)
vpHomogeneousMatrix inverse() const
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Class that defines what is a point.
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setLegend(const unsigned int graphNum, const unsigned int curveNum, const std::string &legend)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
error that can be emited by ViSP classes.
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
void track(const vpHomogeneousMatrix &cMo)
Class that defines the Pioneer mobile robot simulator equipped with a static camera.
unsigned int getWidth() const