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/vpSimulatorViper850.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 192 displayInt.
init(Iint,700,0,
"Internal view") ;
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, articular velocity are computed" 202 std::cout <<
" Simulation " << std::endl ;
203 std::cout <<
" task : servo 4 points " << std::endl ;
204 std::cout <<
"----------------------------------------------" << std::endl ;
205 std::cout << std::endl ;
220 for (
unsigned int i = 0 ; i < 4 ; i++)
221 point[i].track(cMo) ;
225 for (
unsigned int i = 0 ; i < 4 ; i++)
235 for (
unsigned int i = 0 ; i < 4 ; i++)
236 point[i].track(cdMo);
238 for (
unsigned int i = 0 ; i < 4 ; i++)
248 for (
unsigned int i = 0 ; i < 4 ; i++)
265 robot.initialiseObjectRelativeToCamera(cMo);
268 robot.setDesiredCameraPosition(cdMo);
272 robot.getCameraParameters(cam,Iint);
278 robot.getInternalView(Iint);
285 unsigned int iter=0 ;
289 std::cout <<
"---------------------------------------------" << iter <<std::endl ;
296 cMo = robot.get_cMo();
299 std::cout <<
"Initial robot position with respect to the object frame:\n";
304 for (
unsigned int i = 0 ; i < 4 ; i++)
306 point[i].
track(cMo) ;
315 robot.getInternalView(Iint);
319 if (opt_display && opt_click_allowed && iter == 1)
322 std::cout <<
"Click in the internal view window to continue..." << std::endl;
332 std::cout <<
"|| s - s* || " << ( task.
getError() ).sumSquare() <<std::endl ;
342 std::cout <<
"Final robot position with respect to the object frame:\n";
345 if (opt_display && opt_click_allowed)
348 std::cout <<
"Click in the internal view window to end..." << std::endl;
354 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
362 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.
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
static double rad(double deg)
Simulator of Irisa's Viper S850 robot named Viper850.
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)