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/vpSimulatorViper850.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)
199 displayInt.
init(Iint, 700, 0,
"Internal view");
204 std::cout << std::endl;
205 std::cout <<
"----------------------------------------------" << std::endl;
206 std::cout <<
" Test program for vpServo " << std::endl;
207 std::cout <<
" Eye-in-hand task control, articular velocity are computed" << std::endl;
208 std::cout <<
" Simulation " << std::endl;
209 std::cout <<
" task : servo 4 points " << std::endl;
210 std::cout <<
"----------------------------------------------" << std::endl;
211 std::cout << std::endl;
225 for (
unsigned int i = 0; i < 4; i++)
230 for (
unsigned int i = 0; i < 4; i++)
240 for (
unsigned int i = 0; i < 4; i++)
241 point[i].track(cdMo);
243 for (
unsigned int i = 0; i < 4; i++)
253 for (
unsigned int i = 0; i < 4; i++)
271 robot.initialiseObjectRelativeToCamera(cMo);
274 robot.setDesiredCameraPosition(cdMo);
278 robot.getCameraParameters(cam, Iint);
283 robot.getInternalView(Iint);
290 unsigned int iter = 0;
292 while (iter++ < 500) {
293 std::cout <<
"---------------------------------------------" << iter << std::endl;
300 cMo = robot.get_cMo();
303 std::cout <<
"Initial robot position with respect to the object frame:\n";
308 for (
unsigned int i = 0; i < 4; i++) {
317 robot.getInternalView(Iint);
321 if (opt_display && opt_click_allowed && iter == 1) {
323 std::cout <<
"Click in the internal view window to continue..." << std::endl;
333 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) {
347 std::cout <<
"Click in the internal view window to end..." << std::endl;
352 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
356#elif !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
359 std::cout <<
"You do not have X11, or GDI (Graphical Device Interface) of OpenCV functionalities to display images..."
361 std::cout <<
"Tip if you are on a unix-like system:" << std::endl;
362 std::cout <<
"- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
363 std::cout <<
"Tip if you are on a windows-like system:" << std::endl;
364 std::cout <<
"- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
367#elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
370 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
376 std::cout <<
"You do not have threading capabilities" << std::endl;
377 std::cout <<
"Tip:" << std::endl;
378 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 Viper S850 robot named Viper850.
@ TOOL_PTGREY_FLEA2_CAMERA
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()