Example of eye-in-hand control law. We control here a real robot, the Viper S850 robot (arm with 6 degrees of freedom). The velocity is computed in camera frame. The inverse jacobian that converts cartesian velocities in joint velocities is implemented in the robot low level controller. Visual features are the image coordinates of 4 points. The target is made of 4 dots arranged as a 10cm by 10cm square.
#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_DC1394))
#include <visp3/blob/vpDot2.h>
#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.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/vision/vpPose.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.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;
bool reset = false;
#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] << " ";
}
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