Example of eye-in-hand control law. We control here a real robot, the Viper850 robot (cartesian robot, with 6 degrees of freedom). A kinect is attached to the hand. The velocity is computed in the kinect camera frame. Visual features are the image coordinates of 4 points.
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
#include <fstream>
#include <iostream>
#include <sstream>
#include <stdio.h>
#include <stdlib.h>
#if (defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES))
#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpPoint.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/robot/vpRobotViper850.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
#include <visp3/sensor/vpKinect.h>
#include <visp3/vision/vpPose.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/vpDot2.h>
#define L 0.05 // to deal with a 10cm by 10cm square
{
for (int i = 0; i < ndot; i++) {
double x = 0, y = 0;
y);
}
if (init == true) {
if (residual_lagrange < residual_dementhon)
cMo = cMo_lagrange;
else
cMo = cMo_dementhon;
} else {
}
}
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;
return (-1);
}
}
std::string logfilename;
logfilename = logdirname + "/log.dat";
std::ofstream flog(logfilename.c_str());
try {
int i;
#ifdef VISP_HAVE_LIBFREENECT_OLD
Freenect::Freenect<vpKinect> freenect;
vpKinect &kinect = freenect.createDevice(0);
#else
Freenect::Freenect freenect;
#endif
#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 space" << std::endl;
std::cout << " Use of the Viper850 robot " << std::endl;
std::cout << " task : servo 4 points on a square with dimention " << L << " meters" << std::endl;
std::cout << "-------------------------------------------------------" << std::endl;
std::cout << std::endl;
std::cout << "Click on the 4 dots clockwise starting from upper/left dot..." << std::endl;
for (i = 0; i < 4; i++) {
}
for (i = 0; i < 4; i++)
for (int i = 0; i < 4; i++) {
}
for (i = 0; i < 4; i++)
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
for (;;) {
try {
for (i = 0; i < 4; i++) {
}
} catch (...) {
flog.close();
vpTRACE(
"Error detected while tracking visual features");
return (1);
}
compute_pose(point, dot, 4, cam, cMo, cto, cro, false);
for (i = 0; i < 4; i++) {
}
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;
}
std::cout << "Display task information: " << std::endl;
flog.close();
return EXIT_SUCCESS;
}
flog.close();
std::cout <<
"Catch an exception: " << e.
getMessage() << std::endl;
return EXIT_FAILURE;
}
}
#else
int main()
{
std::cout << "You do not have an Viper 850 robot connected to your computer..." << std::endl;
return EXIT_SUCCESS;
}
#endif