 |
Visual Servoing Platform
version 3.2.0
|
57 #include <visp3/core/vpConfig.h>
58 #include <visp3/core/vpDebug.h>
59 #if (defined(VISP_HAVE_AFMA4) && defined(VISP_HAVE_DC1394))
61 #include <visp3/core/vpDisplay.h>
62 #include <visp3/core/vpImage.h>
63 #include <visp3/gui/vpDisplayGTK.h>
64 #include <visp3/gui/vpDisplayOpenCV.h>
65 #include <visp3/gui/vpDisplayX.h>
66 #include <visp3/sensor/vp1394TwoGrabber.h>
68 #include <visp3/blob/vpDot2.h>
69 #include <visp3/core/vpException.h>
70 #include <visp3/core/vpHomogeneousMatrix.h>
71 #include <visp3/core/vpIoTools.h>
72 #include <visp3/core/vpLinearKalmanFilterInstantiation.h>
73 #include <visp3/core/vpMath.h>
74 #include <visp3/core/vpPoint.h>
75 #include <visp3/io/vpParseArgv.h>
76 #include <visp3/robot/vpRobotAfma4.h>
77 #include <visp3/visual_features/vpFeatureBuilder.h>
78 #include <visp3/visual_features/vpFeaturePoint.h>
79 #include <visp3/vs/vpAdaptiveGain.h>
80 #include <visp3/vs/vpServo.h>
81 #include <visp3/vs/vpServoDisplay.h>
84 #define GETOPTARGS "hK:l:"
86 typedef enum { K_NONE, K_VELOCITY, K_ACCELERATION } KalmanType;
97 void usage(
const char *name,
const char *badparam, KalmanType &kalman)
100 Tests a control law with the following characteristics:\n\
101 - eye-in-hand control\n\
102 - camera velocity are computed\n\
103 - servo on 1 points.\n\
104 - Kalman filtering\n\
107 %s [-K <0|1|2|3>] [-h]\n", name);
112 Set the constant gain. By default adaptive gain. \n\
118 2: acceleration model\n\
121 Print the help.\n", (
int)kalman);
124 fprintf(stderr,
"ERROR: \n");
125 fprintf(stderr,
"\nBad parameter [%s]\n", badparam);
143 bool getOptions(
int argc,
const char **argv, KalmanType &kalman,
bool &doAdaptativeGain,
152 kalman = (KalmanType)atoi(optarg);
155 doAdaptativeGain =
false;
159 usage(argv[0], NULL, kalman);
164 usage(argv[0], optarg, kalman);
170 if ((c == 1) || (c == -1)) {
172 usage(argv[0], NULL, kalman);
173 std::cerr <<
"ERROR: " << std::endl;
174 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
181 int main(
int argc,
const char **argv)
184 KalmanType opt_kalman = K_NONE;
186 bool doAdaptativeGain =
true;
188 int opt_cam_frequency = 60;
191 if (getOptions(argc, argv, opt_kalman, doAdaptativeGain, lambda) ==
false) {
201 std::string username;
206 std::string logdirname;
207 logdirname =
"/tmp/" + username;
215 std::cerr << std::endl <<
"ERROR:" << std::endl;
216 std::cerr <<
" Cannot create " << logdirname << std::endl;
220 std::string logfilename;
221 logfilename = logdirname +
"/log.dat";
224 std::ofstream flog(logfilename.c_str());
231 switch (opt_cam_frequency) {
244 for (
int i = 0; i < 10; i++)
248 vpDisplayX display(I, 100, 100,
"Current image");
249 #elif defined(VISP_HAVE_OPENCV)
251 #elif defined(VISP_HAVE_GTK)
258 std::cout << std::endl;
259 std::cout <<
"-------------------------------------------------------" << std::endl;
260 std::cout <<
"Test program for target motion compensation using a Kalman "
263 std::cout <<
"Eye-in-hand task control, velocity computed in the camera frame" << std::endl;
264 std::cout <<
"Task : servo a point \n" << std::endl;
267 switch (opt_kalman) {
269 std::cout <<
"Servo with no target motion compensation (see -K option)\n";
272 std::cout <<
"Servo with target motion compensation using a Kalman filter\n"
273 <<
"with constant velocity modelization (see -K option)\n";
276 std::cout <<
"Servo with target motion compensation using a Kalman filter\n"
277 <<
"with constant acceleration modelization (see -K option)\n";
280 std::cout <<
"-------------------------------------------------------" << std::endl;
281 std::cout << std::endl;
285 std::cout <<
"Click on the dot..." << std::endl;
316 std::cout << std::endl;
333 unsigned int nsignal = 2;
337 unsigned int state_size = 0;
339 switch (opt_kalman) {
344 sigma_state.
resize(state_size * nsignal);
345 sigma_state = 0.00001;
346 sigma_measure = 0.05;
348 kalman.
initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
352 case K_ACCELERATION: {
356 sigma_state.
resize(state_size * nsignal);
357 sigma_state = 0.00001;
358 sigma_measure = 0.05;
359 double dt = 1. / opt_cam_frequency;
360 kalman.
initFilter(nsignal, sigma_state, sigma_measure, rho, dt);
388 std::cout <<
"\nHit CTRL-C to stop the loop...\n" << std::flush;
392 double Tv = (double)(t_0 - t_1) / 1000.0;
441 dedt_mes = (err_0 - err_1) / (Tv_1)-task.
J1 * vm_0;
452 switch (opt_kalman) {
459 for (
unsigned int i = 0; i < nsignal; i++) {
460 dedt_filt[i] = kalman.
Xest[i * state_size];
467 v2 = -J1p * dedt_filt;
490 flog << v[0] <<
" " << v[1] <<
" " << v[2] <<
" " << v[3] <<
" " << v[4] <<
" " << v[5] <<
" ";
495 flog << task.
error[0] <<
" " << task.
error[1] <<
" ";
501 flog << dedt_mes[0] <<
" " << dedt_mes[1] <<
" ";
504 flog << dedt_filt[0] <<
" " << dedt_filt[1] <<
" ";
524 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
532 std::cout <<
"You do not have an afma4 robot connected to your computer..." << std::endl;
Initialize the velocity controller.
void initFromConstant(double c)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
void resize(const unsigned int i, const bool flagNullify=true)
vpImagePoint getCog() const
void buildFrom(const double x, const double y, const double Z)
Generic class defining intrinsic camera parameters.
vpMatrix J1
Task Jacobian .
void track(const vpImage< unsigned char > &I)
vpMatrix getTaskJacobianPseudoInverse() const
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Implementation of column vector and the associated operations.
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Implementation of a matrix and operations on matrices.
void setServo(const vpServoType &servo_type)
VISP_EXPORT double measureTimeMs()
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
void initStandard(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
static const vpColor green
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
static void display(const vpImage< unsigned char > &I)
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Adaptive gain computation.
unsigned int getHeight() const
void initFilter(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho, double dt)
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
Class for firewire ieee1394 video devices using libdc1394-2.x api.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void setStateModel(vpStateModel model)
Control of Irisa's cylindrical robot named Afma4.
void setGraphics(const bool activate)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
static const vpColor blue
vpColVector computeControlLaw()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
static void flush(const vpImage< unsigned char > &I)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
This class provides an implementation of some specific linear Kalman filters.
void filter(vpColVector &z)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
error that can be emited by ViSP classes.
unsigned int getWidth() const
unsigned int getStateSize()