61 #include <visp3/core/vpConfig.h>
62 #include <visp3/core/vpDebug.h>
63 #include <visp3/core/vpTime.h>
64 #if (defined(VISP_HAVE_BICLOPS) && (defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_DIRECTSHOW)))
66 #ifdef VISP_HAVE_PTHREAD
70 #include <visp3/core/vpDisplay.h>
71 #include <visp3/core/vpImage.h>
72 #include <visp3/gui/vpDisplayGDI.h>
73 #include <visp3/gui/vpDisplayGTK.h>
74 #include <visp3/gui/vpDisplayX.h>
75 #include <visp3/sensor/vp1394TwoGrabber.h>
76 #include <visp3/sensor/vpDirectShowGrabber.h>
78 #include <visp3/blob/vpDot.h>
79 #include <visp3/core/vpHomogeneousMatrix.h>
80 #include <visp3/core/vpIoTools.h>
81 #include <visp3/core/vpMath.h>
82 #include <visp3/core/vpPoint.h>
83 #include <visp3/io/vpParseArgv.h>
84 #include <visp3/robot/vpRobotBiclops.h>
85 #include <visp3/visual_features/vpFeatureBuilder.h>
86 #include <visp3/visual_features/vpFeaturePoint.h>
87 #include <visp3/vs/vpServo.h>
88 #include <visp3/vs/vpServoDisplay.h>
91 #include <visp3/core/vpException.h>
93 #ifdef VISP_HAVE_PTHREAD
94 pthread_mutex_t mutexEndLoop = PTHREAD_MUTEX_INITIALIZER;
99 #ifdef VISP_HAVE_PTHREAD
100 pthread_mutex_unlock(&mutexEndLoop);
107 #define GETOPTARGS "c:d:h"
120 void usage(
const char *name,
const char *badparam, std::string &conf, std::string &debugdir, std::string &user)
123 Example of eye-in-hand control law. We control here a real robot, the biclops\n\
124 robot (pan-tilt head provided by Traclabs). The velocity is\n\
125 computed in articular. The visual feature is the center of gravity of a\n\
129 %s [-c <Biclops configuration file>] [-d <debug file directory>] [-h]\n", name);
133 -c <Biclops configuration file> %s\n\
134 Sets the biclops robot configuration file.\n\n\
135 -d <debug file directory> %s\n\
136 Sets the debug file directory.\n\
137 From this directory, creates the\"%s\"\n\
138 subdirectory depending on the username, where\n\
139 it writes biclops.txt file.\n", conf.c_str(), debugdir.c_str(), user.c_str());
142 fprintf(stderr,
"ERROR: \n");
143 fprintf(stderr,
"\nBad parameter [%s]\n", badparam);
159 bool getOptions(
int argc,
const char **argv, std::string &conf, std::string &debugdir, std::string &user)
173 usage(argv[0], NULL, conf, debugdir, user);
178 usage(argv[0], optarg_, conf, debugdir, user);
184 if ((c == 1) || (c == -1)) {
186 usage(argv[0], NULL, conf, debugdir, user);
187 std::cerr <<
"ERROR: " << std::endl;
188 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
195 int main(
int argc,
const char **argv)
197 std::cout << std::endl;
198 std::cout <<
"-------------------------------------------------------" << std::endl;
199 std::cout <<
" Test program for vpServo " << std::endl;
200 std::cout <<
" Eye-in-hand task control, velocity computed in the camera frame" << std::endl;
201 std::cout <<
" Simulation " << std::endl;
202 std::cout <<
" task : servo a point " << std::endl;
203 std::cout <<
"-------------------------------------------------------" << std::endl;
204 std::cout << std::endl;
208 #ifdef VISP_HAVE_PTHREAD
209 pthread_mutex_lock(&mutexEndLoop);
211 signal(SIGINT, &signalCtrC);
214 std::string opt_conf =
"/usr/share/BiclopsDefault.cfg";
216 std::string username;
217 std::string debugdir;
218 std::string opt_debugdir;
221 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
222 opt_debugdir =
"/tmp";
223 #elif defined(_WIN32)
224 opt_debugdir =
"C:/temp";
231 if (getOptions(argc, argv, opt_conf, opt_debugdir, username) ==
false) {
236 if (!opt_debugdir.empty())
237 debugdir = opt_debugdir;
240 std::string dirname = debugdir +
"/" + username;
248 usage(argv[0], NULL, opt_conf, debugdir, username);
249 std::cerr << std::endl <<
"ERROR:" << std::endl;
250 std::cerr <<
" Cannot create " << dirname << std::endl;
251 std::cerr <<
" Check your -d " << debugdir <<
" option " << std::endl;
257 char filename[FILENAME_MAX];
258 sprintf(filename,
"%s/biclops.txt", debugdir.c_str());
259 FILE *fd = fopen(filename,
"w");
273 #if defined VISP_HAVE_DC1394
275 #elif defined VISP_HAVE_DIRECTSHOW
290 #if defined VISP_HAVE_X11
291 vpDisplayX display(I, 100, 100,
"Display X...");
292 #elif defined VISP_HAVE_GTK
294 #elif defined(_WIN32)
311 std::cout <<
"Click on a dot to initialize the tracking..." << std::endl;
338 vpTRACE(
"Set the position of the end-effector frame in the camera frame");
344 std::cout << cVe << std::endl;
347 std::cout <<
"Click in the image to start the servoing..." << std::endl;
366 unsigned int iter = 0;
368 #ifdef VISP_HAVE_PTHREAD
369 while (0 != pthread_mutex_trylock(&mutexEndLoop))
374 std::cout <<
"---------------------------------------------" << iter << std::endl;
398 std::cout <<
"v: " << v.
t();
401 std::cout <<
"|| s - s* || = " << (task.
getError()).sumSquare() << std::endl;
405 s_minus_sStar = task.
s - task.
sStar;
406 fprintf(fd,
"%f %f %f %f %f\n", v[0], v[1], s_minus_sStar[0], s_minus_sStar[1], (task.
getError()).sumSquare());
410 std::cout <<
"Display task information " << std::endl;
418 std::cout <<
"Catch an exception: " << e.
getMessage() << std::endl;
426 std::cout <<
"You do not have an biclops PT robot connected to your computer..." << std::endl;
Class for firewire ieee1394 video devices using libdc1394-2.x api.
void acquire(vpImage< unsigned char > &I)
void open(vpImage< unsigned char > &I)
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
class for windows direct show devices
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
This tracker is meant to track a dot (connected pixels with same gray level) on a vpImage.
void initTracking(const vpImage< unsigned char > &I)
void setGraphics(bool activate)
void track(const vpImage< unsigned char > &I)
error that can be emited by ViSP classes.
const char * getMessage() const
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void buildFrom(double x, double y, double Z)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a matrix and operations on matrices.
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Interface for the biclops, pan, tilt head control.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void get_eJe(vpMatrix &eJe)
@ STATE_POSITION_CONTROL
Initialize the position controller.
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
void set_cVe(const vpVelocityTwistMatrix &cVe_)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
void set_eJe(const vpMatrix &eJe_)
void setServo(const vpServoType &servo_type)
vpColVector getError() const
vpColVector computeControlLaw()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
vpVelocityTwistMatrix get_cVe() const
VISP_EXPORT int wait(double t0, double t)