This example illustrates in one hand a classical visual servoing with a cylinder. And in the other hand it illustrates the behaviour of the robot when adding a secondary task.
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpCylinder.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpMath.h>
#include <visp3/gui/vpDisplayD3D.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpProjectionDisplay.h>
#include <visp3/io/vpParseArgv.h>
#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureLine.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
#define GETOPTARGS "cdho"
#ifdef ENABLE_VISP_NAMESPACE
#endif
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
void usage(const char *name, const char *badparam)
{
fprintf(stdout, "\n\
Simulation of a 2D visual servoing on a cylinder:\n\
- eye-in-hand control law,\n\
- velocity computed in the camera frame,\n\
- display the camera view.\n\
\n\
SYNOPSIS\n\
%s [-c] [-d] [-o] [-h]\n",
name);
fprintf(stdout, "\n\
OPTIONS: Default\n\
\n\
-c\n\
Disable the mouse click. Useful to automate the \n\
execution of this program without human intervention.\n\
\n\
-d \n\
Turn off the display.\n\
\n\
-o \n\
Disable new projection operator usage for secondary task.\n\
\n\
-h\n\
Print the help.\n");
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
}
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display, bool &new_proj_operator)
{
const char *optarg_;
int c;
switch (c) {
case 'c':
click_allowed = false;
break;
case 'd':
display = false;
break;
case 'o':
new_proj_operator = false;
break;
case 'h':
usage(argv[0], nullptr);
return false;
default:
usage(argv[0], optarg_);
return false;
}
}
if ((c == 1) || (c == -1)) {
usage(argv[0], nullptr);
std::cerr << "ERROR: " << std::endl;
std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
return false;
}
return true;
}
int main(int argc, const char **argv)
{
#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
try {
bool opt_display = true;
bool opt_click_allowed = true;
bool opt_new_proj_operator = true;
if (getOptions(argc, argv, opt_click_allowed, opt_display, opt_new_proj_operator) == false) {
return EXIT_FAILURE;
}
#ifdef VISP_HAVE_DISPLAY
#if defined(VISP_HAVE_X11)
vpDisplayX displayInt;
vpDisplayX displayExt;
#elif defined(VISP_HAVE_GTK)
#elif defined(VISP_HAVE_GDI)
#elif defined(HAVE_OPENCV_HIGHGUI)
#elif defined(VISP_HAVE_D3D9)
#endif
#endif
if (opt_display) {
#ifdef VISP_HAVE_DISPLAY
displayInt.
init(Iint, 100, 100,
"Internal view");
displayExt.init(Iext, 130 + static_cast<int>(Iint.getWidth()), 100, "External view");
#endif
}
#ifdef VISP_HAVE_DISPLAY
#endif
double px, py;
px = py = 600;
double u0, v0;
u0 = v0 = 256;
wMo = wMc * cMo;
0, 0, 0,
0.1);
#ifdef VISP_HAVE_DISPLAY
externalview.
insert(cylinder);
#endif
cylinder.track(cMod);
cylinder.print();
for (unsigned int i = 0; i < 2; i++)
cylinder.track(cMo);
cylinder.print();
for (unsigned int i = 0; i < 2; i++) {
}
#ifdef VISP_HAVE_DISPLAY
#endif
if (opt_display && opt_click_allowed) {
}
unsigned int iter = 0;
bool stop = false;
bool start_secondary_task = false;
while (!stop) {
std::cout << "---------------------------------------------" << iter++ << std::endl;
robot.getPosition(wMc);
cylinder.track(cMo);
for (unsigned int i = 0; i < 2; i++) {
}
if (opt_display) {
#ifdef VISP_HAVE_DISPLAY
#endif
}
start_secondary_task = true;
}
if (start_secondary_task) {
static unsigned int iter_sec = 0;
double rapport = 0;
double vitesse = 0.5;
unsigned int tempo = 800;
if (iter_sec > tempo) {
stop = true;
}
if (iter_sec % tempo < 200) {
e2 = 0;
e1[0] = fabs(vitesse);
rapport = vitesse / proj_e1[0];
proj_e1 *= rapport;
v += proj_e1;
}
if (iter_sec % tempo < 400 && iter_sec % tempo >= 200) {
e1 = 0;
e2[1] = fabs(vitesse);
rapport = vitesse / proj_e2[1];
proj_e2 *= rapport;
v += proj_e2;
}
if (iter_sec % tempo < 600 && iter_sec % tempo >= 400) {
e2 = 0;
e1[0] = -fabs(vitesse);
rapport = -vitesse / proj_e1[0];
proj_e1 *= rapport;
v += proj_e1;
}
if (iter_sec % tempo < 800 && iter_sec % tempo >= 600) {
e1 = 0;
e2[1] = -fabs(vitesse);
rapport = -vitesse / proj_e2[1];
proj_e2 *= rapport;
v += proj_e2;
}
if (opt_display && opt_click_allowed) {
std::stringstream ss;
ss << std::string("New projection operator: ") +
(opt_new_proj_operator ? std::string("yes (use option -o to use old one)") : std::string("no"));
}
iter_sec++;
}
else {
if (opt_display && opt_click_allowed) {
}
}
std::cout <<
"|| s - s* || = " << (task.
getError()).sumSquare() << std::endl;
if (opt_display) {
stop = true;
}
}
iter++;
}
if (opt_display && opt_click_allowed) {
}
return EXIT_SUCCESS;
}
std::cout << "Catch a ViSP exception: " << e << std::endl;
return EXIT_FAILURE;
}
#else
(void)argc;
(void)argv;
std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
return EXIT_SUCCESS;
#endif
}
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
static const vpColor white
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="") VP_OVERRIDE
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
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.
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpImagePoint &t)
Class that defines a 2D line visual feature which is composed by two parameters that are and ,...
void print(unsigned int select=FEATURE_ALL) const VP_OVERRIDE
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static double rad(double deg)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
interface with the image for feature display
void insert(vpForwardProjection &fp)
void display(vpImage< unsigned char > &I, const vpHomogeneousMatrix &cextMo, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color, const bool &displayTraj=false, unsigned int thickness=1)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) VP_OVERRIDE
static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator=false)
void setServo(const vpServoType &servo_type)
vpColVector getError() const
vpColVector computeControlLaw()
Class that defines the simplest robot: a free flying camera.