Example of eye-in-hand control law. We control here a real robot, the ptu-46 robot (pan-tilt head provided by Directed Perception). The velocity is computed in articular. The visual feature is the center of gravity of a point.
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
#include <unistd.h>
#endif
#include <signal.h>
#if (defined(VISP_HAVE_PTU46) & defined(VISP_HAVE_DC1394))
#ifdef VISP_HAVE_PTHREAD
#include <pthread.h>
#endif
#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpImage.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpPoint.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
#include <visp3/robot/vpRobotPtu46.h>
#include <visp3/core/vpException.h>
#include <visp3/vs/vpServoDisplay.h>
#include <visp3/blob/vpDot2.h>
#ifdef VISP_HAVE_PTHREAD
pthread_mutex_t mutexEndLoop = PTHREAD_MUTEX_INITIALIZER;
#endif
void signalCtrC(int signumber)
{
(void)(signumber);
#ifdef VISP_HAVE_PTHREAD
pthread_mutex_unlock(&mutexEndLoop);
#endif
usleep(1000 * 10);
}
int main()
{
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;
try {
#ifdef VISP_HAVE_PTHREAD
pthread_mutex_lock(&mutexEndLoop);
#endif
signal(SIGINT, &signalCtrC);
{
q = 0;
}
try {
} catch (...) {
return (-1);
}
vpDisplayX display(I, 100, 100,
"testDisplayX.cpp ");
try {
} catch (...) {
return (-1);
}
try {
} catch (...) {
return (-1);
}
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 ");
unsigned int iter = 0;
#ifdef VISP_HAVE_PTHREAD
while (0 != pthread_mutex_trylock(&mutexEndLoop))
#else
for (;;)
#endif
{
std::cout << "---------------------------------------------" << iter << std::endl;
}
vpTRACE(
"Display task information ");
}
std::cout << "Sorry PtU46 not available. Got exception: " << e << std::endl;
return EXIT_FAILURE
}
return EXIT_SUCCESS;
}
#else
int main()
{
std::cout << "You do not have an PTU46 PT robot connected to your computer..." << std::endl;
}
#endif