Visual Servoing Platform  version 3.6.1 under development (2024-09-11)
tutorial-simu-pioneer-pan.cpp

Example that shows how to simulate a visual servoing on a Pioneer mobile robot equipped with a camera able to move along the pan axis. 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/vpSimulatorPioneerPan.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 {
// Set the position the camera has to reach
cdMo[1][3] = 1.2; // t_y should be different from zero to be non singular
cdMo[2][3] = 0.5;
// Set the initial camera position
cMo[0][3] = 0.3;
cMo[1][3] = cdMo[1][3];
cMo[2][3] = 1.;
vpRotationMatrix cdRo(0, atan2(cMo[0][3], cMo[1][3]), 0);
cMo.insert(cdRo);
robot.setSamplingTime(0.04);
// Get robot position world frame
robot.getPosition(wMc);
// Compute the position of the object in the world frame
wMo = wMc * cMo;
// Define the target
vpPoint point(0, 0, 0); // Coordinates in the object frame
point.track(cMo);
cVe = robot.get_cVe();
vpMatrix eJe;
robot.get_eJe(eJe);
// Current and desired visual feature associated later to the x coordinate
// of the point
vpFeaturePoint s_x, s_xd;
// Create the current x visual feature
// Create the desired x* visual feature
s_xd.build(0, 0, cdMo[2][3]);
// Create the current and desired log(Z/Z*) visual feature
vpFeatureDepth s_Z, s_Zd;
// Initial depth of the target in front of the camera
double Z = point.get_Z();
// Desired depth Z* of the target.
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); // log(Z/Z*) = 0 that's why the last parameter is 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, 3);
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(0, 2, "qdot_pan");
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);
// Update the current x feature
// Update log(Z/Z*) feature. Since the depth Z change, we need to update
// the intection matrix
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);
// Compute the control law. Velocities are computed in the mobile robot
// reference frame
// Send the velocity to the robot
#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
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
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
@ CURRENT
Definition: vpServo.h:202
Class that defines the Pioneer mobile robot simulator equipped with a camera able to move in pan.
vpVelocityTwistMatrix get_cVe() const
Definition: vpUnicycle.h:72