62 #include <visp/vpConfig.h>
63 #include <visp/vpDebug.h>
71 #if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394_2))
73 #include <visp/vp1394TwoGrabber.h>
74 #include <visp/vpImage.h>
75 #include <visp/vpDisplay.h>
76 #include <visp/vpDisplayX.h>
77 #include <visp/vpDisplayOpenCV.h>
78 #include <visp/vpDisplayGTK.h>
79 #include <visp/vpMath.h>
80 #include <visp/vpHomogeneousMatrix.h>
81 #include <visp/vpFeaturePoint.h>
82 #include <visp/vpPoint.h>
83 #include <visp/vpServo.h>
84 #include <visp/vpFeatureBuilder.h>
85 #include <visp/vpRobotViper850.h>
86 #include <visp/vpIoTools.h>
87 #include <visp/vpException.h>
88 #include <visp/vpMatrixException.h>
89 #include <visp/vpServoDisplay.h>
90 #include <visp/vpDot2.h>
91 #include <visp/vpPlot.h>
113 vpDisplayX display(I,800,100,
"Current image") ;
114 #elif defined(VISP_HAVE_OPENCV)
116 #elif defined(VISP_HAVE_GTK)
133 for (
unsigned int i=0 ; i < 6 ; i++)
135 Qmin[i] = jointMin[i] + 0.5*rho*(jointMax[i]-jointMin[i]) ;
136 Qmax[i] = jointMax[i] - 0.5*rho*(jointMax[i]-jointMin[i]) ;
138 Qmiddle = (Qmin + Qmax) /2.;
141 for (
unsigned int i=0 ; i < 6 ; i++) {
142 tQmin[i]=Qmin[i]+ 0.5*(rho1)*(Qmax[i]-Qmin[i]) ;
143 tQmax[i]=Qmax[i]- 0.5*(rho1)*(Qmax[i]-Qmin[i]) ;
155 plot.initGraph(0, 10);
157 plot.initGraph(1, 1);
163 plot.initRange(0, 0., 200., -1.2, 1.2);
164 plot.setTitle(0,
"Joint behavior");
171 plot.initRange(1, 0., 200., 0., 1e-4);
172 plot.setTitle(1,
"Cost function");
176 for (
unsigned int i=0; i < 6; i++) {
177 sprintf(legend,
"q%d", i+1);
178 plot.setLegend(0, i, legend);
180 plot.setLegend(0, 6,
"tQmin");
181 plot.setLegend(0, 7,
"tQmax");
182 plot.setLegend(0, 8,
"Qmin");
183 plot.setLegend(0, 9,
"Qmax");
190 plot.setColor(0, 4,
vpColor(0, 128, 0));
192 for (
unsigned int i= 6; i < 10; i++)
196 plot.setLegend(1, 0,
"h_s");
201 std::cout <<
" Give the parameters beta (1) : ";
207 std::cout <<
"Click on a dot..." << std::endl;
234 std::cout << cVe <<std::endl ;
243 std::cout << std::endl ;
255 std::cout <<
"\nHit CTRL-C to stop the loop...\n" << std::flush;
297 for (
unsigned int i=0 ; i < 6 ; i++)
300 if (q[i] > tQmax[i]) S = q[i] - tQmax[i] ;
301 if (q[i] < tQmin[i]) S = q[i] - tQmin[i] ;
302 double D = (Qmax[i]-Qmin[i]) ;
309 std::cout <<
"Cost function h_s: " << h_s << std::endl;
315 v = prim_task + sec_task;
327 for (
unsigned int i=0 ; i < 6 ; i++) {
328 data[i] = (q[i] - Qmiddle[i]) ;
329 data[i] /= (Qmax[i] - Qmin[i]) ;
332 unsigned int joint = 2;
333 data[6] = 2*(tQmin[joint]-Qmiddle[joint])/(Qmax[joint] - Qmin[joint]) ;
334 data[7] = 2*(tQmax[joint]-Qmiddle[joint])/(Qmax[joint] - Qmin[joint]) ;
335 data[8] = -1 ; data[9] = 1 ;
336 plot.plot(0, iter, data);
337 plot.plot(1, 0, iter, h_s);
360 vpERROR_TRACE(
"You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
Definition of the vpMatrix class.
static void display(vpServo &s, const vpCameraParameters &cam, vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
Control of Irisa's Viper S850 robot named Viper850.
static const vpColor black
Class to define colors available for display functionnalities.
Define the X11 console to display images.
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
create a new ste of two visual features
void setLambda(double _lambda)
set the gain lambda
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void get_eJe(vpMatrix &eJe)
vpColVector secondaryTask(vpColVector &de2dt)
Add a secondary task.
vpColVector getJointMin()
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)
void set_cVe(vpVelocityTwistMatrix &_cVe)
static void flush(const vpImage< unsigned char > &I)
static const vpColor orange
vpImagePoint getCog() const
void kill()
destruction (memory deallocation if required)
Initialize the velocity controller.
static const vpColor cyan
vpColVector computeControlLaw()
compute the desired control law
static double sqr(double x)
static void display(const vpImage< unsigned char > &I)
void set_eJe(vpMatrix &_eJe)
The vpDisplayOpenCV allows to display image using the opencv library.
virtual void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)=0
Generic class defining intrinsic camera parameters.
The vpDisplayGTK allows to display image using the GTK+ library version 1.2.
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Set the type of the interaction matrix (current, mean, desired, user).
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void buildFrom(const double x, const double y, const double Z)
vpColVector getJointMax()
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 get_cVe(vpVelocityTwistMatrix &cVe)
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Class for firewire ieee1394 video devices using libdc1394-2.x api.
void set_Z(const double Z)
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 descbribed in and .
void setServo(vpServoType _servo_type)
Choice of the visual servoing control law.
static const vpColor blue