57 #include <visp/vpConfig.h>
58 #include <visp/vpDebug.h>
66 #if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394_2))
68 #include <visp/vp1394TwoGrabber.h>
69 #include <visp/vpImage.h>
70 #include <visp/vpMath.h>
71 #include <visp/vpHomogeneousMatrix.h>
72 #include <visp/vpFeaturePoint.h>
73 #include <visp/vpPoint.h>
74 #include <visp/vpServo.h>
75 #include <visp/vpFeatureBuilder.h>
76 #include <visp/vpRobotViper850.h>
77 #include <visp/vpIoTools.h>
78 #include <visp/vpException.h>
79 #include <visp/vpMatrixException.h>
80 #include <visp/vpServoDisplay.h>
81 #include <visp/vpImageIo.h>
82 #include <visp/vpDot2.h>
83 #include <visp/vpAdaptiveGain.h>
84 #include <visp/vpLinearKalmanFilterInstantiation.h>
85 #include <visp/vpDisplay.h>
86 #include <visp/vpDisplayX.h>
102 std::string logdirname;
103 logdirname =
"/tmp/" + username;
112 std::cerr << std::endl
113 <<
"ERROR:" << std::endl;
114 std::cerr <<
" Cannot create " << logdirname << std::endl;
118 std::string logfilename;
119 logfilename = logdirname +
"/log.dat";
122 std::ofstream flog(logfilename.c_str());
131 unsigned int nsignal = 2;
135 unsigned int state_size = 0;
139 sigma_state.
resize(state_size*nsignal);
140 sigma_state = 0.00001;
141 sigma_measure = 0.05;
143 kalman.
initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
163 double Tloop = 1./80.f;
185 for (
int i=0; i< 10; i++)
188 std::cout <<
"Click on a dot..." << std::endl;
214 task.addFeature(p,pd) ;
219 task.setLambda(lambda) ;
227 std::cout <<
"\nHit CTRL-C to stop the loop...\n" << std::flush;
234 dc1394video_frame_t *frame = NULL;
243 Tv = (double)(t_0 - t_1) / 1000.0;
251 frame = g.dequeue(I);
269 v1 = task.computeControlLaw() ;
272 err = task.getError();
281 vpMatrix J1 = task.getTaskJacobian();
282 dedt_mes = (err_1 - err)/(Tv) - J1 *vm;
290 for (
unsigned int i=0; i < nsignal; i++) {
291 dedt_filt[i] = kalman.
Xest[i*state_size];
296 vpMatrix J1p = task.getTaskJacobianPseudoInverse();
297 v2 = - J1p*dedt_filt;
315 std::cout <<
"Tracking failed... Stop the robot." << std::endl;
327 flog << v[0] <<
" " << v[1] <<
" " << v[2] <<
" "
328 << v[3] <<
" " << v[4] <<
" " << v[5] <<
" ";
338 flog << qvel[0] <<
" " << qvel[1] <<
" " << qvel[2] <<
" "
339 << qvel[3] <<
" " << qvel[4] <<
" " << qvel[5] <<
" ";
349 flog << q[0] <<
" " << q[1] <<
" " << q[2] <<
" "
350 << q[3] <<
" " << q[4] <<
" " << q[5] <<
" ";
355 flog << ( task.getError() ).t() << std::endl;
386 vpERROR_TRACE(
"You do not have a Viper robot or a firewire framegrabber connected to your computer...");
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
Definition of the vpMatrix class.
Adaptive gain computation.
unsigned int getStateSize()
static void display(vpServo &s, const vpCameraParameters &cam, vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
unsigned int getWidth() const
void initFilter(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho, double dt)
Control of Irisa's Viper S850 robot named Viper850.
void filter(vpColVector &z)
Define the X11 console to display images.
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
static double measureTimeMs()
static int wait(double t0, double t)
static const vpColor green
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
void track(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
vpImagePoint getCog() const
Initialize the velocity controller.
static void display(const vpImage< unsigned char > &I)
virtual void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)=0
Generic class defining intrinsic camera parameters.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void buildFrom(const double x, const double y, const double Z)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
void setStateModel(vpStateModel model)
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 ...
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class required to compute the visual servoing control law.
void initStandard(double en_zero, double en_infini, double pente_en_zero)
void setGraphics(const bool activate)
static const vpColor blue
void resize(const unsigned int i, const bool flagNullify=true)