56 #include <visp3/core/vpDebug.h> 57 #include <visp3/core/vpConfig.h> 59 #if ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD)) && (defined (VISP_HAVE_X11) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GDI)) 68 #include <visp3/core/vpCameraParameters.h> 69 #include <visp3/gui/vpDisplayX.h> 70 #include <visp3/gui/vpDisplayGTK.h> 71 #include <visp3/gui/vpDisplayGDI.h> 72 #include <visp3/visual_features/vpFeatureBuilder.h> 73 #include <visp3/visual_features/vpFeaturePoint.h> 74 #include <visp3/core/vpHomogeneousMatrix.h> 75 #include <visp3/core/vpImage.h> 76 #include <visp3/core/vpImagePoint.h> 77 #include <visp3/core/vpIoTools.h> 78 #include <visp3/core/vpMath.h> 79 #include <visp3/core/vpMeterPixelConversion.h> 80 #include <visp3/io/vpParseArgv.h> 81 #include <visp3/vs/vpServo.h> 82 #include <visp3/robot/vpSimulatorAfma6.h> 85 #define GETOPTARGS "cdh" 87 void usage(
const char *name,
const char *badparam);
88 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display);
98 void usage(
const char *name,
const char *badparam)
101 Tests a control law with the following characteristics:\n\ 102 - eye-in-hand control\n\ 103 - articular velocity are computed\n\ 104 - servo on 4 points,\n\ 105 - internal and external camera view displays.\n\ 108 %s [-c] [-d] [-h]\n", name);
113 Disable the mouse click. Useful to automaze the \n\ 114 execution of this program without humain intervention.\n\ 117 Turn off the display.\n\ 123 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
137 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display)
144 case 'c': click_allowed =
false;
break;
145 case 'd': display =
false;
break;
146 case 'h': usage(argv[0], NULL);
return false;
break;
149 usage(argv[0], optarg_);
154 if ((c == 1) || (c == -1)) {
156 usage(argv[0], NULL);
157 std::cerr <<
"ERROR: " << std::endl;
158 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
166 main(
int argc,
const char ** argv)
169 bool opt_click_allowed =
true;
170 bool opt_display =
true;
173 if (getOptions(argc, argv, opt_click_allowed, opt_display) ==
false) {
179 #if defined VISP_HAVE_X11 181 #elif defined VISP_HAVE_GDI 183 #elif defined VISP_HAVE_OPENCV 191 displayInt.
init(Iint,700,0,
"Internal view") ;
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, articular velocity are computed" 201 std::cout <<
" Simulation " << std::endl ;
202 std::cout <<
" task : servo 4 points " << std::endl ;
203 std::cout <<
"----------------------------------------------" << std::endl ;
204 std::cout << std::endl ;
218 for (
unsigned int i = 0 ; i < 4 ; i++)
219 point[i].track(cMo) ;
223 for (
unsigned int i = 0 ; i < 4 ; i++)
233 for (
unsigned int i = 0 ; i < 4 ; i++)
234 point[i].track(cdMo);
236 for (
unsigned int i = 0 ; i < 4 ; i++)
246 for (
unsigned int i = 0 ; i < 4 ; i++)
263 robot.initialiseObjectRelativeToCamera(cMo);
266 robot.setDesiredCameraPosition(cdMo);
270 robot.getCameraParameters(cam,Iint);
276 robot.getInternalView(Iint);
283 unsigned int iter=0 ;
287 std::cout <<
"---------------------------------------------" << iter <<std::endl ;
294 cMo = robot.get_cMo();
297 std::cout <<
"Initial robot position with respect to the object frame:\n";
302 for (
unsigned int i = 0 ; i < 4 ; i++)
304 point[i].
track(cMo) ;
313 robot.getInternalView(Iint);
317 if (opt_display && opt_click_allowed && iter == 1)
320 std::cout <<
"Click in the internal view window to continue..." << std::endl;
330 std::cout <<
"|| s - s* || " << ( task.
getError() ).sumSquare() <<std::endl ;
340 std::cout <<
"Final robot position with respect to the object frame:\n";
343 if (opt_display && opt_click_allowed)
346 std::cout <<
"Click in the internal view window to end..." << std::endl;
352 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
360 vpERROR_TRACE(
"You do not have X11, OpenCV or GDI display functionalities or threading capabilities...");
The object displayed at the desired position is the same than the scene object defined in vpSceneObje...
VISP_EXPORT int wait(double t0, double t)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Perspective projection without distortion model.
Implementation of an homogeneous matrix and operations on such kind of matrices.
A 40cm by 40cm plate with 4 points at coordinates (-0.1,-0.1,0), (0.1,-0.1,0), (0.1,0.1,0), (0.1,0.1,0). Each point is represented by a circle with 2cm radius.
Display for windows using GDI (available on any windows 32 platform).
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
void print() const
Print the matrix as a pose vector .
error that can be emited by ViSP classes.
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
void track(const vpHomogeneousMatrix &cMo)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
static void flush(const vpImage< unsigned char > &I)
VISP_EXPORT double measureTimeMs()
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Class that defines what is a point.
Initialize the velocity controller.
vpColVector computeControlLaw()
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Generic class defining intrinsic camera parameters.
Simulator of Irisa's gantry robot named Afma6.
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
static double rad(double deg)
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Implementation of column vector and the associated operations.
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
vpColVector getError() const
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void setServo(const vpServoType &servo_type)