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 articular. The visual feature is the center of gravity of a point.
#include <fstream>
#include <iostream>
#include <sstream>
#include <stdio.h>
#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/core/vpImagePoint.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/vpIoTools.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpPoint.h>
#include <visp3/robot/vpRobotAfma6.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
#include <visp3/core/vpException.h>
#include <visp3/vs/vpServoDisplay.h>
#include <visp3/blob/vpDot.h>
int main()
{
std::string username;
std::string logdirname;
logdirname = "/tmp/" + username;
try {
} catch (...) {
std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
exit(-1);
}
}
std::string logfilename;
logfilename = logdirname + "/log.dat";
std::ofstream flog(logfilename.c_str());
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 joint space" << std::endl;
std::cout << " Use of the Afma6 robot " << std::endl;
std::cout << " task : servo a point " << std::endl;
std::cout << "-------------------------------------------------------" << std::endl;
std::cout << std::endl;
std::cout << "Click on a dot..." << std::endl;
vpTRACE(
"sets the current position of the visual feature ");
vpTRACE(
"sets the desired position of the visual feature ");
vpTRACE(
"\t we want an eye-in-hand control law");
vpTRACE(
"\t articular velocity are computed");
vpTRACE(
"Set the position of the camera in the end-effector frame ");
std::cout << cVe << std::endl;
vpTRACE(
"Set the Jacobian (expressed in the end-effector frame)");
vpTRACE(
"\t we want to see a point on a point..");
std::cout << std::endl;
vpTRACE(
"Display task information ");
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
for (;;) {
flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " ";
flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " ";
flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " ";
flog << (task.
getError()).t() << std::endl;
}
flog.close();
vpTRACE(
"Display task information ");
return EXIT_SUCCESS;
}
flog.close();
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