Visual Servoing Platform  version 3.6.1 under development (2024-08-05)
tutorial-simu-pioneer.cpp

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 abscise of the point measured at each iteration
• x* the desired abscise 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/vpConfig.h>
#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()
{
#if defined(ENABLE_VISP_NAMESPACE)
using namespace VISP_NAMESPACE_NAME;
#endif
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.;
vpRotationMatrix cRo(0, atan2(cMo[0][3], cMo[1][3]), 0);
cMo.insert(cRo);
robot.setSamplingTime(0.04);
robot.getPosition(wMc);
wMo = wMc * cMo;
vpPoint point(0, 0, 0);
point.track(cMo);
cVe = robot.get_cVe();
vpMatrix eJe;
robot.get_eJe(eJe);
vpFeaturePoint s_x, s_xd;
s_xd.build(0, 0, cdMo[2][3]);
vpFeatureDepth s_Z, s_Zd;
double Z = point.get_Z();
double Zd = cdMo[2][3];
s_Z.build(s_x.get_x(), s_x.get_y(), Z, log(Z / Zd));
s_Zd.build(0, 0, Zd, 0);
#ifdef VISP_HAVE_DISPLAY
// Create a window (800 by 500) at position (400, 10) with 3 graphics
vpPlot graph(3, 800, 500, 400, 10, "Curves...");
// Init the curve plotter
graph.initGraph(0, 2);
graph.initGraph(1, 2);
graph.initGraph(2, 1);
graph.setTitle(0, "Velocities");
graph.setTitle(1, "Error s-s*");
graph.setTitle(2, "Depth");
graph.setLegend(0, 0, "vx");
graph.setLegend(0, 1, "wz");
graph.setLegend(1, 0, "x");
graph.setLegend(1, 1, "log(Z/Z*)");
graph.setLegend(2, 0, "Z");
#endif
int iter = 0;
for (;;) {
robot.getPosition(wMc);
cMo = wMc.inverse() * wMo;
point.track(cMo);
Z = point.get_Z();
s_Z.build(s_x.get_x(), s_x.get_y(), Z, log(Z / Zd));
robot.get_cVe(cVe);
robot.get_eJe(eJe);
#ifdef VISP_HAVE_DISPLAY
graph.plot(0, iter, v); // plot velocities applied to the robot
graph.plot(1, iter, task.getError()); // plot error vector
graph.plot(2, 0, iter, Z); // plot the depth
#endif
iter++;
std::cout << "Reached a small error. We stop the loop... " << std::endl;
break;
}
}
#ifdef VISP_HAVE_DISPLAY
graph.saveData(0, "./v2.dat");
graph.saveData(1, "./error2.dat");
const char *legend = "Click to quit...";
vpDisplay::displayText(graph.I, (int)graph.I.getHeight() - 60, (int)graph.I.getWidth() - 150, legend, vpColor::red);
#endif
}
catch (const vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
}
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
double sumSquare() const
static const vpColor red
Definition: vpColor.h:217
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emitted by ViSP classes.
Definition: vpException.h:60
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 3D point visual feature which is composed by one parameters that is that defin...
vpFeatureDepth & build(const double &x, const double &y, const double &Z, const double &LogZoverZstar)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
static unsigned int selectX()
double get_y() const
double get_x() const
vpFeaturePoint & build(const double &x, const double &y, const double &Z)
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
void insert(const vpRotationMatrix &R)
unsigned int getWidth() const
Definition: vpImage.h:242
unsigned int getHeight() const
Definition: vpImage.h:181
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition: vpPlot.h:112
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Definition: vpPlot.cpp:203
vpImage< unsigned char > I
Definition: vpPlot.h:114
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
Definition: vpPlot.cpp:552
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
Definition: vpPlot.cpp:270
void setTitle(unsigned int graphNum, const std::string &title)
Definition: vpPlot.cpp:510
void saveData(unsigned int graphNum, const std::string &dataFile, const std::string &title_prefix="")
Definition: vpPlot.cpp:655
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:79
double get_Z() const
Get the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:406
void get_eJe(vpMatrix &eJe) VP_OVERRIDE
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) VP_OVERRIDE
@ ARTICULAR_FRAME
Definition: vpRobot.h:80
Implementation of a rotation matrix and operations on such kind of matrices.
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:380
@ EYEINHAND_L_cVe_eJe
Definition: vpServo.h:168
void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:331
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:1038
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:171
void setLambda(double c)
Definition: vpServo.h:986
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:1101
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:134
vpColVector getError() const
Definition: vpServo.h:510
@ PSEUDO_INVERSE
Definition: vpServo.h:235
vpColVector computeControlLaw()
Definition: vpServo.cpp:705
@ DESIRED
Definition: vpServo.h:208
Class that defines the Pioneer mobile robot simulator equipped with a static camera.
vpVelocityTwistMatrix get_cVe() const
Definition: vpUnicycle.h:72