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>
78 #include <visp/vpMath.h>
79 #include <visp/vpHomogeneousMatrix.h>
80 #include <visp/vpFeaturePoint.h>
81 #include <visp/vpPoint.h>
82 #include <visp/vpServo.h>
83 #include <visp/vpFeatureBuilder.h>
84 #include <visp/vpRobotViper850.h>
85 #include <visp/vpIoTools.h>
86 #include <visp/vpException.h>
87 #include <visp/vpMatrixException.h>
88 #include <visp/vpServoDisplay.h>
89 #include <visp/vpDot2.h>
90 #include <visp/vpPlot.h>
111 vpDisplayX display(I, 800, 100,
"Camera view") ;
126 for (
unsigned int i=0 ; i < 6 ; i++)
128 Qmin[i] = jointMin[i] + 0.5*rho*(jointMax[i]-jointMin[i]) ;
129 Qmax[i] = jointMax[i] - 0.5*rho*(jointMax[i]-jointMin[i]) ;
131 Qmiddle = (Qmin + Qmax) /2.;
134 for (
unsigned int i=0 ; i < 6 ; i++) {
135 tQmin[i]=Qmin[i]+ 0.5*(rho1)*(Qmax[i]-Qmin[i]) ;
136 tQmax[i]=Qmax[i]- 0.5*(rho1)*(Qmax[i]-Qmin[i]) ;
148 plot.initGraph(0, 10);
150 plot.initGraph(1, 1);
156 plot.initRange(0, 0., 200., -1.2, 1.2);
157 plot.setTitle(0,
"Joint behavior");
164 plot.initRange(1, 0., 200., 0., 1e-4);
165 plot.setTitle(1,
"Cost function");
169 for (
unsigned int i=0; i < 6; i++) {
170 sprintf(legend,
"q%d", i+1);
171 plot.setLegend(0, i, legend);
173 plot.setLegend(0, 6,
"tQmin");
174 plot.setLegend(0, 7,
"tQmax");
175 plot.setLegend(0, 8,
"Qmin");
176 plot.setLegend(0, 9,
"Qmax");
183 plot.setColor(0, 4,
vpColor(0, 128, 0));
185 for (
unsigned int i= 6; i < 10; i++)
189 plot.setLegend(1, 0,
"h_s");
194 std::cout <<
" Give the parameters beta (1) : ";
200 std::cout <<
"Click on a dot..." << std::endl;
227 std::cout << cVe <<std::endl ;
236 std::cout << std::endl ;
248 std::cout <<
"\nHit CTRL-C to stop the loop...\n" << std::flush;
290 for (
unsigned int i=0 ; i < 6 ; i++)
293 if (q[i] > tQmax[i]) S = q[i] - tQmax[i] ;
294 if (q[i] < tQmin[i]) S = q[i] - tQmin[i] ;
295 double D = (Qmax[i]-Qmin[i]) ;
302 std::cout <<
"Cost function h_s: " << h_s << std::endl;
308 v = prim_task + sec_task;
320 for (
unsigned int i=0 ; i < 6 ; i++) {
321 data[i] = (q[i] - Qmiddle[i]) ;
322 data[i] /= (Qmax[i] - Qmin[i]) ;
325 unsigned int joint = 2;
326 data[6] = 2*(tQmin[joint]-Qmiddle[joint])/(Qmax[joint] - Qmin[joint]) ;
327 data[7] = 2*(tQmax[joint]-Qmiddle[joint])/(Qmax[joint] - Qmin[joint]) ;
328 data[8] = -1 ; data[9] = 1 ;
329 plot.plot(0, iter, data);
330 plot.plot(1, 0, iter, h_s);
353 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)
virtual void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)=0
Generic class defining intrinsic camera parameters.
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.
void setServo(vpServoType _servo_type)
Choice of the visual servoing control law.
static const vpColor blue