ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
servoMomentPoints.cpp
1 /****************************************************************************
2  *
3  * $Id: servoMomentPoints.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Example of visual servoing with moments using discrete points as object
36  * container
37  *
38  * Authors:
39  * Filip Novotny
40  *
41  *****************************************************************************/
42 
48 #include <visp/vpDebug.h>
49 #include <visp/vpConfig.h>
50 #include <iostream>
51 #include <visp/vpHomogeneousMatrix.h>
52 #include <visp/vpMomentObject.h>
53 #include <visp/vpMomentDatabase.h>
54 #include <visp/vpMomentCommon.h>
55 #include <visp/vpFeatureMomentCommon.h>
56 #include <visp/vpDisplayX.h>
57 #include <visp/vpDisplayGTK.h>
58 #include <visp/vpDisplayGDI.h>
59 #include <visp/vpCameraParameters.h>
60 #include <visp/vpIoTools.h>
61 #include <visp/vpMath.h>
62 #include <visp/vpHomogeneousMatrix.h>
63 #include <visp/vpServo.h>
64 #include <visp/vpDebug.h>
65 #include <visp/vpFeatureBuilder.h>
66 #include <visp/vpFeaturePoint.h>
67 #include <visp/vpSimulatorAfma6.h>
68 #include <visp/vpPlane.h>
69 
70 //setup robot parameters
71 void paramRobot();
72 
73 //update moment objects and interface
74 void refreshScene(vpMomentObject &obj);
75 //initialize scene in the interface
76 void initScene();
77 //initialize the moment features
78 void initFeatures();
79 
80 void init(vpHomogeneousMatrix& cMo, vpHomogeneousMatrix& cdMo);
81 void execute(unsigned int nbIter); //launch the simulation
82 void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type);
83 double error();
84 void _planeToABC(vpPlane& pl, double& A,double& B, double& C);
85 void paramRobot();
86 
87 #if !defined(WIN32) && !defined(VISP_HAVE_PTHREAD)
88 // Robot simulator used in this example is not available
89 int main()
90 {
91  std::cout << "Can't run this example since vpSimulatorAfma6 capability is not available." << std::endl;
92  std::cout << "You should install pthread third-party library." << std::endl;
93 }
94 // No display available
95 #elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && !defined(VISP_HAVE_GTK)
96 int main()
97 {
98  std::cout << "Can't run this example since no display capability is available." << std::endl;
99  std::cout << "You should install one of the following third-party library: X11, OpenCV, GDI, GTK." << std::endl;
100 }
101 #else
102 int main(){
103  //intial pose
104  vpHomogeneousMatrix cMo(0.05,0.1,1.5,vpMath::rad(30),vpMath::rad(20),-vpMath::rad(15));
105  //Desired pose
107 
108  //init and run the simulation
109  init(cMo,cdMo);
110  execute(1500);
111  return 0;
112 }
113 
114 //init the right display
115 #if defined VISP_HAVE_X11
116 vpDisplayX displayInt;
117 #elif defined VISP_HAVE_OPENCV
118 vpDisplayOpenCV displayInt;
119 #elif defined VISP_HAVE_GDI
120 vpDisplayGDI displayInt;
121 #elif defined VISP_HAVE_D3D9
122 vpDisplayD3D displayInt;
123 #elif defined VISP_HAVE_GTK
124 vpDisplayGTK displayInt;
125 #endif
126 
127 //start and destination positioning matrices
130 
131 vpSimulatorAfma6 robot(false);//robot used in this simulation
132 vpImage<vpRGBa> Iint(480,640, 255);//internal image used for interface display
133 vpServo::vpServoIteractionMatrixType interaction_type; //current or desired
134 vpServo task; //servoing task
135 vpCameraParameters cam;//robot camera parameters
136 double _error; //current error
137 vpImageSimulator imsim;//image simulator used to simulate the perspective-projection camera
138 
139 //moment sets and their corresponding features
140 vpMomentCommon *moments;
141 vpMomentCommon *momentsDes;
142 vpFeatureMomentCommon *featureMoments;
143 vpFeatureMomentCommon *featureMomentsDes;
144 
145 //source and destination objects for moment manipulation
146 vpMomentObject src(6);
147 vpMomentObject dst(6);
148 
149 using namespace std;
150 
151 
152 void initScene(){
153  vector<vpPoint> src_pts;
154  vector<vpPoint> dst_pts;
155 
156  double x[8] = { 1,3, 4,-1 ,-3,-2,-1,1};
157  double y[8] = { 0,1, 4, 4, -2,-2, 1,0};
158  int nbpoints = 8;
159 
160  for (int i = 0 ; i < nbpoints ; i++){
161  vpPoint p;
162  p.setWorldCoordinates(x[i]/20,y[i]/20,0.0);
163  p.track(cMo) ;
164  src_pts.push_back(p);
165  }
166 
168  src.fromVector(src_pts);
169  for (int i = 0 ; i < nbpoints ; i++){
170  vpPoint p;
171  p.setWorldCoordinates(x[i]/20,y[i]/20,0.0);
172  p.track(cdMo) ;
173  dst_pts.push_back(p);
174  }
176  dst.fromVector(dst_pts);
177 
178 }
179 
180 void initFeatures(){
181  //A,B,C parameters of source and destination plane
182  double A; double B; double C;
183  double Ad; double Bd; double Cd;
184  //init main object: using moments up to order 6
185 
186  //Initializing values from regular plane (with ax+by+cz=d convention)
187  vpPlane pl;
188  pl.setABCD(0,0,1.0,0);
189  pl.changeFrame(cMo);
190  _planeToABC(pl,A,B,C);
191 
192  pl.setABCD(0,0,1.0,0);
193  pl.changeFrame(cdMo);
194  _planeToABC(pl,Ad,Bd,Cd);
195 
196  //extracting initial position (actually we only care about Zdst)
198  cdMo.extract(vec);
199 
201  //don't need to be specific, vpMomentCommon automatically loads Xg,Yg,An,Ci,Cj,Alpha moments
204  //same thing with common features
205  featureMoments = new vpFeatureMomentCommon(*moments);
206  featureMomentsDes = new vpFeatureMomentCommon(*momentsDes);
207 
208  moments->updateAll(src);
209  momentsDes->updateAll(dst);
210 
211  featureMoments->updateAll(A,B,C);
212  featureMomentsDes->updateAll(Ad,Bd,Cd);
213 
214  //setup the interaction type
215  task.setInteractionMatrixType(interaction_type) ;
217  task.addFeature(featureMoments->getFeatureGravityNormalized(),featureMomentsDes->getFeatureGravityNormalized());
218  task.addFeature(featureMoments->getFeatureAn(),featureMomentsDes->getFeatureAn());
219  task.addFeature(featureMoments->getFeatureCInvariant(),featureMomentsDes->getFeatureCInvariant(),(1 << 3) | (1 << 5));
220  task.addFeature(featureMoments->getFeatureAlpha(),featureMomentsDes->getFeatureAlpha());
221 
222  task.setLambda(1.) ;
223 }
224 
225 
226 void refreshScene(vpMomentObject &obj){
227  //double x[8] = { 0.05,0.15, 0.2,-0.05 ,-0.15,-0.1,-0.05,0.05};
228  //double y[8] = { 0,0.05, 0.2, 0.2, -0.1,-0.1, 0.05,0};
229  double x[8] = { 1,3, 4,-1 ,-3,-2,-1,1};
230  double y[8] = { 0,1, 4, 4, -2,-2, 1,0};
231  int nbpoints = 8;
232  vector<vpPoint> cur_pts;
233 
234  for (int i = 0 ; i < nbpoints ; i++){
235  vpPoint p;
236  p.setWorldCoordinates(x[i]/20,y[i]/20,0.0);
237  p.track(cMo) ;
238  cur_pts.push_back(p);
239  }
240  obj.fromVector(cur_pts);
241 }
242 
243 void init(vpHomogeneousMatrix& _cMo, vpHomogeneousMatrix& _cdMo)
244 
245 {
246  cMo = _cMo; //init source matrix
247  cdMo = _cdMo; //init destination matrix
248  interaction_type = vpServo::CURRENT; //use interaction matrix for current position
249 
250  displayInt.init(Iint,700,0, "Visual servoing with moments") ;
251 
252  paramRobot(); //set up robot parameters
253 
255  initScene(); //initialize graphical scene (for interface)
256  initFeatures();//initialize moment features
257 }
258 
259 void execute(unsigned int nbIter){
260  //init main object: using moments up to order 6
261  vpMomentObject obj(6);
262  //setting object type (disrete, continuous[form polygon])
264 
265  vpTRACE("Display task information " ) ;
266  task.print() ;
267 
268  vpDisplay::display(Iint);
269  robot.getInternalView(Iint);
270  vpDisplay::flush(Iint);
271  unsigned int iter=0;
272 
274  while(iter++<nbIter ){
275  vpColVector v ;
276  //get the cMo
277  cMo = robot.get_cMo();
278  //setup the plane in A,B,C style
279  vpPlane pl;
280  double A,B,C;
281  pl.setABCD(0,0,1.0,0);
282  pl.changeFrame(cMo);
283  _planeToABC(pl,A,B,C);
284 
285  //track points, draw points and add refresh our object
286  refreshScene(obj);
287  //this is the most important thing to do: update our moments
288  moments->updateAll(obj);
289  //and update our features. Do it in that order. Features need to use the information computed by moments
290  featureMoments->updateAll(A,B,C);
291 
292  vpDisplay::display(Iint) ;
293  robot.getInternalView(Iint);
294  vpDisplay::flush(Iint);
295 
296  if (iter == 1)
297  vpDisplay::getClick(Iint) ;
298  v = task.computeControlLaw() ;
299 
300  //pilot robot using position control. The displacement is t*v with t=10ms step
301  //robot.setPosition(vpRobot::CAMERA_FRAME,0.01*v);
303 
304 
305  _error = ( task.getError() ).sumSquare();
306  }
307 
308  task.kill();
309 
310  vpTRACE("\n\nClick in the internal view window to end...");
311  vpDisplay::getClick(Iint) ;
312 
313  delete moments;
314  delete momentsDes;
315  delete featureMoments;
316  delete featureMomentsDes;
317 }
318 
319 void removeJointLimits(vpSimulatorAfma6& robot){
320  vpColVector limMin(6);
321  vpColVector limMax(6);
322  limMin[0] = vpMath::rad(-3600);
323  limMin[1] = vpMath::rad(-3600);
324  limMin[2] = vpMath::rad(-3600);
325  limMin[3] = vpMath::rad(-3600);
326  limMin[4] = vpMath::rad(-3600);
327  limMin[5] = vpMath::rad(-3600);
328 
329  limMax[0] = vpMath::rad(3600);
330  limMax[1] = vpMath::rad(3600);
331  limMax[2] = vpMath::rad(3600);
332  limMax[3] = vpMath::rad(3600);
333  limMax[4] = vpMath::rad(3600);
334  limMax[5] = vpMath::rad(3600);
335 
336  robot.setJointLimit(limMin,limMax);
337  robot.setMaxRotationVelocity(99999);
338  robot.setMaxTranslationVelocity(999999);
339 }
340 
341 void _planeToABC(vpPlane& pl, double& A,double& B, double& C){
342  if(fabs(pl.getD())<std::numeric_limits<double>::epsilon()){
343  std::cout << "Invalid position:" << std::endl;
344  std::cout << cMo << std::endl;
345  std::cout << "Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
346  throw vpException(vpException::divideByZeroError,"invalid position!");
347  }
348  A=-pl.getA()/pl.getD();
349  B=-pl.getB()/pl.getD();
350  C=-pl.getC()/pl.getD();
351 }
352 
353 void paramRobot(){
354  /*Initialise the robot and especially the camera*/
356  robot.setCurrentViewColor(vpColor(150,150,150));
357  robot.setDesiredViewColor(vpColor(200,200,200));
359  removeJointLimits(robot);
361  robot.setConstantSamplingTimeMode(true);
362  /*Initialise the position of the object relative to the pose of the robot's camera*/
364 
365  /*Set the desired position (for the displaypart)*/
366  robot.setDesiredCameraPosition(cdMo);
367  robot.getCameraParameters(cam,Iint);
368 }
369 
370 void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type){interaction_type=type;}
371 double error(){return _error;}
372 
373 #endif
The object displayed at the desired position is the same than the scene object defined in vpSceneObje...
void setDesiredCameraPosition(const vpHomogeneousMatrix cdMo)
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
Perspective projection without distortion model.
void setMaxTranslationVelocity(const double maxVt)
Definition: vpRobot.cpp:191
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpTRACE
Definition: vpDebug.h:401
A plate with 8 points at coordinates (0.05,0,0), (0.15,0.05,0), (0.2,0.2,0), (-0.05,0.2,0), (-0.15,-0.1,0), (-0.1,-0.1,0), (-0.05,0.05,0) and (0.5,0,0). ach point is represented by a circle with 2cm radius.
static double getAlpha(vpMomentObject &objec)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:133
Class to define colors available for display functionnalities.
Definition: vpColor.h:125
Define the X11 console to display images.
Definition: vpDisplayX.h:152
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
create a new ste of two visual features
Definition: vpServo.cpp:444
This class allows to access common vpFeatureMoments in a pre-filled database.
void setLambda(double _lambda)
set the gain lambda
Definition: vpServo.h:253
error that can be emited by ViSP classes.
Definition: vpException.h:75
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
Class for generic objects.
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const char *title=NULL)
void track(const vpHomogeneousMatrix &cMo)
void setType(vpObjectType type)
void setABCD(const double A, const double B, const double C, const double D)
Definition: vpPlane.h:99
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:1991
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Class that defines what is a point.
Definition: vpPoint.h:65
Display for windows using Direct3D.
Definition: vpDisplayD3D.h:109
void kill()
destruction (memory deallocation if required)
Definition: vpServo.cpp:177
Initialize the velocity controller.
Definition: vpRobot.h:70
vpColVector getError() const
Definition: vpServo.h:301
vpColVector computeControlLaw()
compute the desired control law
Definition: vpServo.cpp:883
vpFeatureMomentAlpha & getFeatureAlpha()
void updateAll(double A, double B, double C)
void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo)
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:352
static std::vector< double > getMu3(vpMomentObject &object)
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:203
The vpDisplayOpenCV allows to display image using the opencv library.
Generic class defining intrinsic camera parameters.
Class which enables to project an image in the 3D space and get the view of a virtual camera...
Simulator of Irisa's gantry robot named Afma6.
The vpDisplayGTK allows to display image using the GTK+ library version 1.2.
Definition: vpDisplayGTK.h:145
void extract(vpRotationMatrix &R) const
void init(vpAfma6::vpAfma6ToolType tool, vpCameraParameters::vpCameraParametersProjType projModel=vpCameraParameters::perspectiveProjWithoutDistortion)
vpServoIteractionMatrixType
Definition: vpServo.h:169
void setCurrentViewColor(const vpColor col)
void fromVector(std::vector< vpPoint > &points)
static double getSurface(vpMomentObject &object)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Set the type of the interaction matrix (current, mean, desired, user).
Definition: vpServo.cpp:509
vpFeatureMomentCInvariant & getFeatureCInvariant()
static double rad(double deg)
Definition: vpMath.h:100
void updateAll(vpMomentObject &object)
void setMaxRotationVelocity(const double maxVr)
Definition: vpRobot.cpp:215
This class initializes and allows access to commonly used moments.
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
void setConstantSamplingTimeMode(const bool _constantSamplingTimeMode)
vpFeatureMomentGravityCenterNormalized & getFeatureGravityNormalized()
double getB() const
Definition: vpPlane.h:114
void setDesiredViewColor(const vpColor col)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:258
double getA() const
Definition: vpPlane.h:112
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
double getC() const
Definition: vpPlane.h:116
virtual bool getClick(bool blocking=true)=0
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:67
Class required to compute the visual servoing control law descbribed in and .
Definition: vpServo.h:153
vpFeatureMomentAreaNormalized & getFeatureAn()
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
Class that consider the case of a translation vector.
void setServo(vpServoType _servo_type)
Choice of the visual servoing control law.
Definition: vpServo.cpp:214
double getD() const
Definition: vpPlane.h:118
void setWorldCoordinates(const double ox, const double oy, const double oz)
Set the point world coordinates. We mean here the coordinates of the point in the object frame...
Definition: vpPoint.cpp:74