Example of eye-in-hand control law. We control here a real robot, the Afma6 robot (cartesian robot, with 6 degrees of freedom). The velocity is computed in the camera frame. The used visual feature is a circle.
#include <cmath>
#include <limits>
#include <stdlib.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
#if (defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394))
#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpImage.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMath.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureEllipse.h>
#include <visp3/vs/vpServo.h>
#include <visp3/robot/vpRobotAfma6.h>
#include <visp3/core/vpException.h>
#include <visp3/vs/vpServoDisplay.h>
#include <visp3/blob/vpDot.h>
int main()
{
try {
#ifdef VISP_HAVE_X11
#elif defined(VISP_HAVE_OPENCV)
#elif defined(VISP_HAVE_GTK)
#endif
std::cout << std::endl;
std::cout << "-------------------------------------------------------" << std::endl;
std::cout << " Test program for vpServo " << std::endl;
std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl;
std::cout << " Simulation " << std::endl;
std::cout << " task : servo a point " << std::endl;
std::cout << "-------------------------------------------------------" << std::endl;
std::cout << std::endl;
std::cout << "Click on an ellipse..." << std::endl;
vpTRACE(
"sets the current position of the visual feature ");
std::cout << " Learning 0/1 " << std::endl;
int learning;
std::cin >> learning;
char name[FILENAME_MAX];
sprintf(name, "dat/ellipse.dat");
if (learning == 1) {
vpTRACE(
"Save the location of the object in a file dat/ellipse.dat");
std::ofstream f(name);
f.close();
exit(1);
}
vpTRACE(
"sets the desired position of the visual feature ");
std::ifstream f("dat/ellipse.dat");
double x, y, mu20, mu11, mu02;
f >> x;
f >> y;
f >> mu20;
f >> mu11;
f >> mu02;
f.close();
unsigned int iter = 0;
double lambda_av = 0.01;
double alpha = 0.1;
double beta = 3;
std::cout << "alpha 0.7" << std::endl;
std::cin >> alpha;
std::cout << "beta 5" << std::endl;
std::cin >> beta;
for (;;) {
std::cout << "---------------------------------------------" << iter++ << std::endl;
double gain;
if (iter > 2) {
if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
gain = lambda_av;
else {
gain = alpha * exp(-beta * (task.
getError()).sumSquare()) + lambda_av;
}
} else
gain = lambda_av;
std::cout <<
"rank " << task.
getTaskRank() << std::endl;
}
vpTRACE(
"Display task information ");
return EXIT_SUCCESS;
}
std::cout << "Test failed with exception: " << e << std::endl;
return EXIT_FAILURE;
}
}
#else
int main()
{
std::cout << "You do not have an afma6 robot connected to your computer..." << std::endl;
return EXIT_SUCCESS;
}
#endif