58#include <visp3/core/vpConfig.h>
59#include <visp3/core/vpDebug.h>
60#include <visp3/core/vpTime.h>
61#if (defined(VISP_HAVE_BICLOPS) && (defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_DIRECTSHOW)))
63#ifdef VISP_HAVE_PTHREAD
67#include <visp3/core/vpDisplay.h>
68#include <visp3/core/vpImage.h>
69#include <visp3/gui/vpDisplayGDI.h>
70#include <visp3/gui/vpDisplayGTK.h>
71#include <visp3/gui/vpDisplayX.h>
72#include <visp3/sensor/vp1394TwoGrabber.h>
73#include <visp3/sensor/vpDirectShowGrabber.h>
75#include <visp3/blob/vpDot.h>
76#include <visp3/core/vpHomogeneousMatrix.h>
77#include <visp3/core/vpIoTools.h>
78#include <visp3/core/vpMath.h>
79#include <visp3/core/vpPoint.h>
80#include <visp3/io/vpParseArgv.h>
81#include <visp3/robot/vpRobotBiclops.h>
82#include <visp3/visual_features/vpFeatureBuilder.h>
83#include <visp3/visual_features/vpFeaturePoint.h>
84#include <visp3/vs/vpServo.h>
85#include <visp3/vs/vpServoDisplay.h>
88#include <visp3/core/vpException.h>
90#ifdef VISP_HAVE_PTHREAD
91pthread_mutex_t mutexEndLoop = PTHREAD_MUTEX_INITIALIZER;
96#ifdef VISP_HAVE_PTHREAD
97 pthread_mutex_unlock(&mutexEndLoop);
104#define GETOPTARGS "c:d:h"
117void usage(
const char *name,
const char *badparam, std::string &conf, std::string &debugdir, std::string &user)
120 Example of eye-in-hand control law. We control here a real robot, the biclops\n\
121 robot (pan-tilt head provided by Traclabs). The velocity is\n\
122 computed in articular. The visual feature is the center of gravity of a\n\
126 %s [-c <Biclops configuration file>] [-d <debug file directory>] [-h]\n",
131 -c <Biclops configuration file> %s\n\
132 Sets the biclops robot configuration file.\n\n\
133 -d <debug file directory> %s\n\
134 Sets the debug file directory.\n\
135 From this directory, creates the\"%s\"\n\
136 subdirectory depending on the username, where\n\
137 it writes biclops.txt file.\n",
138 conf.c_str(), debugdir.c_str(), user.c_str());
141 fprintf(stderr,
"ERROR: \n");
142 fprintf(stderr,
"\nBad parameter [%s]\n", badparam);
158bool getOptions(
int argc,
const char **argv, std::string &conf, std::string &debugdir, std::string &user)
172 usage(argv[0], NULL, conf, debugdir, user);
177 usage(argv[0], optarg_, conf, debugdir, user);
183 if ((c == 1) || (c == -1)) {
185 usage(argv[0], NULL, conf, debugdir, user);
186 std::cerr <<
"ERROR: " << std::endl;
187 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
194int main(
int argc,
const char **argv)
196 std::cout << std::endl;
197 std::cout <<
"-------------------------------------------------------" << std::endl;
198 std::cout <<
" Test program for vpServo " << std::endl;
199 std::cout <<
" Eye-in-hand task control, velocity computed in the camera frame" << std::endl;
200 std::cout <<
" Simulation " << std::endl;
201 std::cout <<
" task : servo a point " << std::endl;
202 std::cout <<
"-------------------------------------------------------" << std::endl;
203 std::cout << std::endl;
207#ifdef VISP_HAVE_PTHREAD
208 pthread_mutex_lock(&mutexEndLoop);
210 signal(SIGINT, &signalCtrC);
213 std::string opt_conf =
"/usr/share/BiclopsDefault.cfg";
215 std::string username;
216 std::string debugdir;
217 std::string opt_debugdir;
220#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
221 opt_debugdir =
"/tmp";
223 opt_debugdir =
"C:/temp";
230 if (getOptions(argc, argv, opt_conf, opt_debugdir, username) ==
false) {
235 if (!opt_debugdir.empty())
236 debugdir = opt_debugdir;
239 std::string dirname = debugdir +
"/" + username;
247 usage(argv[0], NULL, opt_conf, debugdir, username);
248 std::cerr << std::endl <<
"ERROR:" << std::endl;
249 std::cerr <<
" Cannot create " << dirname << std::endl;
250 std::cerr <<
" Check your -d " << debugdir <<
" option " << std::endl;
256 std::string filename;
257 filename = debugdir +
"/biclops.txt";
258 FILE *fd = fopen(filename.c_str(),
"w");
272#if defined VISP_HAVE_DC1394
274#elif defined VISP_HAVE_DIRECTSHOW
289#if defined(VISP_HAVE_X11)
290 vpDisplayX display(I, 100, 100,
"Display X...");
291#elif defined(VISP_HAVE_GTK)
310 std::cout <<
"Click on a dot to initialize the tracking..." << std::endl;
337 vpTRACE(
"Set the position of the end-effector frame in the camera frame");
343 std::cout << cVe << std::endl;
346 std::cout <<
"Click in the image to start the servoing..." << std::endl;
365 unsigned int iter = 0;
367#ifdef VISP_HAVE_PTHREAD
368 while (0 != pthread_mutex_trylock(&mutexEndLoop))
373 std::cout <<
"---------------------------------------------" << iter << std::endl;
394 std::cout <<
"v: " << v.
t();
397 std::cout <<
"|| s - s* || = " << (task.
getError()).sumSquare() << std::endl;
401 s_minus_sStar = task.
s - task.
sStar;
402 fprintf(fd,
"%f %f %f %f %f\n", v[0], v[1], s_minus_sStar[0], s_minus_sStar[1], (task.
getError()).sumSquare());
406 std::cout <<
"Display task information " << std::endl;
413 std::cout <<
"Catch an exception: " << e.
getMessage() << std::endl;
421 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 emitted 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.
Definition of the vpImage class member functions.
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)