53 #include <visp/vpConfig.h>
54 #include <visp/vpDebug.h>
57 #ifdef VISP_HAVE_COIN_AND_GUI
59 #include <visp/vpImage.h>
60 #include <visp/vpCameraParameters.h>
61 #include <visp/vpTime.h>
62 #include <visp/vpSimulator.h>
65 #include <visp/vpMath.h>
66 #include <visp/vpHomogeneousMatrix.h>
67 #include <visp/vpFeaturePointPolar.h>
68 #include <visp/vpServo.h>
69 #include <visp/vpRobotCamera.h>
70 #include <visp/vpFeatureBuilder.h>
71 #include <visp/vpParseArgv.h>
72 #include <visp/vpIoTools.h>
74 #define GETOPTARGS "di:h"
86 void usage(
const char *name,
const char *badparam, std::string ipath)
89 Simulation Servo 4points.\n\
92 %s [-i <input image path>] [-d] [-h]\n", name);
96 -i <input image path> %s\n\
97 Set image input path.\n\
98 From this path read \"ViSP-images/iv/4points.iv\"\n\
100 Setting the VISP_INPUT_IMAGE_PATH environment\n\
101 variable produces the same behaviour than using\n\
105 Disable the image display. This can be useful \n\
106 for automatic tests using crontab under Unix or \n\
107 using the task manager under Windows.\n\
110 Print the help.\n\n", ipath.c_str());
113 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
131 bool getOptions(
int argc,
const char **argv, std::string &ipath,
bool &display)
138 case 'i': ipath = optarg;
break;
139 case 'd': display =
false;
break;
140 case 'h': usage(argv[0], NULL, ipath);
return false;
break;
143 usage(argv[0], optarg, ipath);
return false;
break;
147 if ((c == 1) || (c == -1)) {
149 usage(argv[0], NULL, ipath);
150 std::cerr <<
"ERROR: " << std::endl;
151 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
159 void *mainLoop (
void *_simu)
168 float sampling_time = 0.040f;
171 std::cout << std::endl ;
172 std::cout <<
"-------------------------------------------------------" << std::endl ;
173 std::cout <<
" Test program for vpServo " <<std::endl ;
174 std::cout <<
" Eye-in-hand task control, articular velocity are computed" << std::endl ;
175 std::cout <<
" Simulation " << std::endl ;
176 std::cout <<
" task : servo 4 points " << std::endl ;
177 std::cout <<
"-------------------------------------------------------" << std::endl ;
178 std::cout << std::endl ;
181 vpTRACE(
"sets the initial camera location " ) ;
200 vpTRACE(
"sets the point coordinates in the world frame " ) ;
207 vpTRACE(
"project : computes the point coordinates in the camera frame and its 2D coordinates" ) ;
208 for (
int i = 0 ; i < 4 ; i++) {
213 vpTRACE(
"sets the desired position of the point ") ;
215 for (
int i = 0 ; i < 4 ; i++)
218 std::cout <<
"s: \n";
219 for (
int i=0; i < 4; i ++) {
220 printf(
"[%d] rho %f theta %f Z %f\n",
221 i, p[i].get_rho(), p[i].get_theta(), p[i].get_Z());
224 vpTRACE(
"sets the desired position of the point ") ;
240 for (
int i=0; i < 4; i ++) {
246 std::cout <<
"s*: \n";
247 for (
int i=0; i < 4; i ++) {
248 printf(
"[%d] rho %f theta %f Z %f\n",
249 i, pd[i].get_rho(), pd[i].get_theta(), pd[i].get_Z());
253 vpTRACE(
"\t we want an eye-in-hand control law") ;
254 vpTRACE(
"\t articular velocity are computed") ;
258 vpTRACE(
"Set the position of the camera in the end-effector frame ") ;
263 vpTRACE(
"Set the Jacobian (expressed in the end-effector frame)") ;
268 vpTRACE(
"\t we want to see a point on a point..") ;
269 for (
int i = 0 ; i < 4 ; i++)
276 vpTRACE(
"Display task information " ) ;
280 std::cout <<
"\nEnter a character to continue or CTRL-C to quit... "
282 {
char a ; std::cin >> a ; }
285 char name[FILENAME_MAX];
286 unsigned int iter=0 ;
288 while(iter++ < 300) {
297 for (
int i = 0 ; i < 4 ; i++)
299 point[i].
track(cMo) ;
314 sprintf(name,
"/tmp/image.%04d.external.png",iter) ;
315 std::cout << name << std::endl ;
317 sprintf(name,
"/tmp/image.%04d.internal.png",iter) ;
324 vpTRACE(
"Display task information " ) ;
328 std::cout <<
"cMo:\n" << cMo << std::endl;
330 std::cout <<
"final pose:\n" << pose.t() << std::endl;
332 std::cout <<
"\nEnter a character to continue..." <<std::endl ;
333 {
char a ; std::cin >> a ; }
346 main(
int argc,
const char ** argv)
348 std::string env_ipath;
349 std::string opt_ipath;
351 std::string filename;
352 std::string username;
353 bool opt_display =
true;
356 char *ptenv = getenv(
"VISP_INPUT_IMAGE_PATH");
361 if (! env_ipath.empty())
365 if (getOptions(argc, argv, opt_ipath, opt_display) ==
false) {
370 if (!opt_ipath.empty())
375 if (!opt_ipath.empty() && !env_ipath.empty()) {
376 if (ipath != env_ipath) {
377 std::cout << std::endl
378 <<
"WARNING: " << std::endl;
379 std::cout <<
" Since -i <visp image path=" << ipath <<
"> "
380 <<
" is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
381 <<
" we skip the environment variable." << std::endl;
386 if (opt_ipath.empty() && env_ipath.empty()){
387 usage(argv[0], NULL, ipath);
388 std::cerr << std::endl
389 <<
"ERROR:" << std::endl;
390 std::cerr <<
" Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
392 <<
" environment variable to specify the location of the " << std::endl
393 <<
" image path where test images are located." << std::endl << std::endl;
411 simu.
load(filename.c_str()) ;
424 {
vpTRACE(
"You should install Coin3D and SoQT or SoWin or SoXt") ;
Definition of the vpMatrix class.
virtual void initInternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the camera view
void write(const char *fileName)
void setExternalCameraParameters(vpCameraParameters &cam)
set external camera parameters
void setCameraPosition(vpHomogeneousMatrix &cMf)
set the camera position (from an homogeneous matrix)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void setPosition(const vpHomogeneousMatrix &cMw)
Implementation of a simulator based on Coin3d (www.coin3d.org).
void closeMainApplication()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
create a new ste of two visual features
void setLambda(double _lambda)
set the gain lambda
void track(const vpHomogeneousMatrix &cMo)
static double measureTimeMs()
virtual void mainLoop()
activate the mainloop
static int wait(double t0, double t)
void set_cVe(vpVelocityTwistMatrix &_cVe)
Class that defines 2D image point visual feature with polar coordinates described in ...
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Class that defines what is a point.
virtual void setSamplingTime(const double &delta_t)
void kill()
destruction (memory deallocation if required)
void initApplication(void *(*start_routine)(void *))
begin the main program
vpColVector computeControlLaw()
compute the desired control law
void getCameraPosition(vpHomogeneousMatrix &_cMf)
get the camera position (from an homogeneous matrix)
Class that defines the simplest robot: a free flying camera.
void setInternalCameraParameters(vpCameraParameters &cam)
set internal camera parameters
void set_eJe(vpMatrix &_eJe)
Generic class defining intrinsic camera parameters.
void load(const char *file_name)
load an iv file
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Set the type of the interaction matrix (current, mean, desired, user).
static double rad(double deg)
void getPosition(vpHomogeneousMatrix &cMw) const
void initMainApplication()
perform some initialization in the main program thread
Class that provides a data structure for the column vectors as well as a set of operations on these v...
void get_eJe(vpMatrix &eJe)
The pose is a complete representation of every rigid motion in the euclidian space.
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP)
Class required to compute the visual servoing control law descbribed in and .
void initExternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the external view
void setZoomFactor(const float zoom)
set the size of the camera/frame
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
void setServo(vpServoType _servo_type)
Choice of the visual servoing control law.
void setWorldCoordinates(const double ox, const double oy, const double oz)
Set the point world coordinates. We mean here the coordinates of the point in the object frame...