 |
Visual Servoing Platform
version 3.3.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 buildFrom(double x, double y, double Z, double LogZoverZstar)
void get_cVe(vpVelocityTwistMatrix &cVe) const
void buildFrom(double x, double y, 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)
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void set_cVe(const vpVelocityTwistMatrix &cVe_)
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 setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
static unsigned int selectX()
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
void saveData(unsigned int graphNum, const std::string &dataFile, const std::string &title_prefix="")
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)
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
void setTitle(unsigned int graphNum, const std::string &title)
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)
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
vpColVector computeControlLaw()
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 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