53#include <visp3/core/vpConfig.h>
54#include <visp3/core/vpDebug.h>
56#if ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD)) && \
57 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GDI)) && \
58 (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
67#include <visp3/core/vpCameraParameters.h>
68#include <visp3/core/vpHomogeneousMatrix.h>
69#include <visp3/core/vpImage.h>
70#include <visp3/core/vpImagePoint.h>
71#include <visp3/core/vpIoTools.h>
72#include <visp3/core/vpMath.h>
73#include <visp3/core/vpMeterPixelConversion.h>
74#include <visp3/gui/vpDisplayGDI.h>
75#include <visp3/gui/vpDisplayGTK.h>
76#include <visp3/gui/vpDisplayX.h>
77#include <visp3/io/vpParseArgv.h>
78#include <visp3/robot/vpSimulatorAfma6.h>
79#include <visp3/visual_features/vpFeatureBuilder.h>
80#include <visp3/visual_features/vpFeaturePoint.h>
81#include <visp3/vs/vpServo.h>
84#define GETOPTARGS "cdh"
86void usage(
const char *name,
const char *badparam);
87bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display);
97void usage(
const char *name,
const char *badparam)
100Tests a control law with the following characteristics:\n\
101 - eye-in-hand control\n\
102 - articular velocity are computed\n\
103 - servo on 4 points,\n\
104 - internal and external camera view displays.\n\
107 %s [-c] [-d] [-h]\n",
113 Disable the mouse click. Useful to automate the \n\
114 execution of this program without human intervention.\n\
117 Turn off the display.\n\
123 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
137bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display)
145 click_allowed =
false;
151 usage(argv[0], NULL);
156 usage(argv[0], optarg_);
162 if ((c == 1) || (c == -1)) {
164 usage(argv[0], NULL);
165 std::cerr <<
"ERROR: " << std::endl;
166 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
173int main(
int argc,
const char **argv)
176 bool opt_click_allowed =
true;
177 bool opt_display =
true;
180 if (getOptions(argc, argv, opt_click_allowed, opt_display) ==
false) {
186#if defined(VISP_HAVE_X11)
188#elif defined(VISP_HAVE_GDI)
190#elif defined(HAVE_OPENCV_HIGHGUI)
198 displayInt.
init(Iint, 700, 0,
"Internal view");
203 std::cout << std::endl;
204 std::cout <<
"----------------------------------------------" << std::endl;
205 std::cout <<
" Test program for vpServo " << std::endl;
206 std::cout <<
" Eye-in-hand task control, articular velocity are computed" << std::endl;
207 std::cout <<
" Simulation " << std::endl;
208 std::cout <<
" task : servo 4 points " << std::endl;
209 std::cout <<
"----------------------------------------------" << std::endl;
210 std::cout << std::endl;
224 for (
unsigned int i = 0; i < 4; i++)
229 for (
unsigned int i = 0; i < 4; i++)
239 for (
unsigned int i = 0; i < 4; i++)
240 point[i].track(cdMo);
242 for (
unsigned int i = 0; i < 4; i++)
252 for (
unsigned int i = 0; i < 4; i++)
270 robot.initialiseObjectRelativeToCamera(cMo);
273 robot.setDesiredCameraPosition(cdMo);
277 robot.getCameraParameters(cam, Iint);
282 robot.getInternalView(Iint);
289 unsigned int iter = 0;
291 while (iter++ < 500) {
292 std::cout <<
"---------------------------------------------" << iter << std::endl;
299 cMo = robot.get_cMo();
302 std::cout <<
"Initial robot position with respect to the object frame:\n";
307 for (
unsigned int i = 0; i < 4; i++) {
316 robot.getInternalView(Iint);
320 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;
341 std::cout <<
"Final robot position with respect to the object frame:\n";
344 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;
357#elif !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
360 std::cout <<
"You do not have X11, or GDI (Graphical Device Interface) of OpenCV functionalities to display images..."
362 std::cout <<
"Tip if you are on a unix-like system:" << std::endl;
363 std::cout <<
"- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
364 std::cout <<
"Tip if you are on a windows-like system:" << std::endl;
365 std::cout <<
"- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
368#elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
371 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
377 std::cout <<
"You do not have threading capabilities" << std::endl;
378 std::cout <<
"Tip:" << std::endl;
379 std::cout <<
"- Install pthread, configure again ViSP using cmake and build again this example" << std::endl;
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
Implementation of column vector and the associated operations.
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
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)
error that can be emitted by ViSP classes.
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 track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition of the vpImage class member functions.
static double rad(double deg)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
void setWorldCoordinates(double oX, double oY, double oZ)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
void setServo(const vpServoType &servo_type)
vpColVector getError() const
vpColVector computeControlLaw()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Simulator of Irisa's gantry robot named Afma6.
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()