53 #include <visp3/core/vpConfig.h> 54 #include <visp3/core/vpDebug.h> 62 #if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394)) 64 #include <visp3/sensor/vp1394TwoGrabber.h> 65 #include <visp3/core/vpImage.h> 66 #include <visp3/core/vpMath.h> 67 #include <visp3/core/vpHomogeneousMatrix.h> 68 #include <visp3/visual_features/vpFeaturePoint.h> 69 #include <visp3/core/vpPoint.h> 70 #include <visp3/vs/vpServo.h> 71 #include <visp3/visual_features/vpFeatureBuilder.h> 72 #include <visp3/robot/vpRobotViper850.h> 73 #include <visp3/core/vpIoTools.h> 74 #include <visp3/core/vpException.h> 75 #include <visp3/vs/vpServoDisplay.h> 76 #include <visp3/io/vpImageIo.h> 77 #include <visp3/blob/vpDot2.h> 78 #include <visp3/vs/vpAdaptiveGain.h> 79 #include <visp3/core/vpLinearKalmanFilterInstantiation.h> 80 #include <visp3/core/vpDisplay.h> 81 #include <visp3/gui/vpDisplayX.h> 82 #include <visp3/gui/vpDisplayOpenCV.h> 83 #include <visp3/gui/vpDisplayGTK.h> 99 std::string logdirname;
100 logdirname =
"/tmp/" + username;
109 std::cerr << std::endl
110 <<
"ERROR:" << std::endl;
111 std::cerr <<
" Cannot create " << logdirname << std::endl;
115 std::string logfilename;
116 logfilename = logdirname +
"/log.dat";
119 std::ofstream flog(logfilename.c_str());
128 unsigned int nsignal = 2;
132 unsigned int state_size = 0;
136 sigma_state.
resize(state_size*nsignal);
137 sigma_state = 0.00001;
138 sigma_measure = 0.05;
140 kalman.
initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
160 double Tloop = 1./80.f;
174 #elif defined(VISP_HAVE_OPENCV) 176 #elif defined(VISP_HAVE_GTK) 188 for (
int i=0; i< 10; i++)
191 std::cout <<
"Click on a dot..." << std::endl;
218 task.addFeature(p,pd) ;
223 task.setLambda(lambda) ;
231 std::cout <<
"\nHit CTRL-C to stop the loop...\n" << std::flush;
238 dc1394video_frame_t *frame = NULL;
247 Tv = (double)(t_0 - t_1) / 1000.0;
255 frame = g.dequeue(I);
273 v1 = task.computeControlLaw() ;
276 err = task.getError();
284 vpMatrix J1 = task.getTaskJacobian();
285 dedt_mes = (err - err_1)/(Tv) - J1 *vm;
294 for (
unsigned int i=0; i < nsignal; i++) {
295 dedt_filt[i] = kalman.
Xest[i*state_size];
300 vpMatrix J1p = task.getTaskJacobianPseudoInverse();
301 v2 = - J1p*dedt_filt;
319 std::cout <<
"Tracking failed... Stop the robot." << std::endl;
331 flog << v[0] <<
" " << v[1] <<
" " << v[2] <<
" " 332 << v[3] <<
" " << v[4] <<
" " << v[5] <<
" ";
342 flog << qvel[0] <<
" " << qvel[1] <<
" " << qvel[2] <<
" " 343 << qvel[3] <<
" " << qvel[4] <<
" " << qvel[5] <<
" ";
353 flog << q[0] <<
" " << q[1] <<
" " << q[2] <<
" " 354 << q[3] <<
" " << q[4] <<
" " << q[5] <<
" ";
359 flog << ( task.getError() ).t() << std::endl;
389 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)
Implementation of a matrix and operations on matrices.
VISP_EXPORT int wait(double t0, double t)
Adaptive gain computation.
unsigned int getStateSize()
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) 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)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
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 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)
VISP_EXPORT double measureTimeMs()
void initStandard(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
Initialize the velocity controller.
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.
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void buildFrom(const double x, const double y, const double Z)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
Implementation of column vector and the associated operations.
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
This class provides an implementation of some specific linear Kalman filters.
vpImagePoint getCog() const
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)
unsigned int getWidth() const
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)
void setGraphics(const bool activate)
static const vpColor blue
void resize(const unsigned int i, const bool flagNullify=true)