ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
servoMomentPolygon.cpp
1 /****************************************************************************
2  *
3  * $Id: servoMomentPolygon.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 a polygon as object container
36  *
37  * Authors:
38  * Filip Novotny
39  *
40  *****************************************************************************/
41 
47 #include <visp/vpDebug.h>
48 #include <visp/vpConfig.h>
49 #include <iostream>
50 #include <visp/vpHomogeneousMatrix.h>
51 #include <visp/vpMomentObject.h>
52 #include <visp/vpMomentDatabase.h>
53 #include <visp/vpMomentCommon.h>
54 #include <visp/vpFeatureMomentCommon.h>
55 #include <visp/vpDisplayX.h>
56 #include <visp/vpDisplayGTK.h>
57 #include <visp/vpDisplayGDI.h>
58 #include <visp/vpCameraParameters.h>
59 #include <visp/vpIoTools.h>
60 #include <visp/vpMath.h>
61 #include <visp/vpHomogeneousMatrix.h>
62 #include <visp/vpServo.h>
63 #include <visp/vpDebug.h>
64 #include <visp/vpFeatureBuilder.h>
65 #include <visp/vpFeaturePoint.h>
66 #include <visp/vpSimulatorAfma6.h>
67 #include <visp/vpPlane.h>
68 
69 //setup robot parameters
70 void paramRobot();
71 
72 //update moment objects and interface
73 void refreshScene(vpMomentObject &obj);
74 //initialize scene in the interface
75 void initScene();
76 //initialize the moment features
77 void initFeatures();
78 
79 void init(vpHomogeneousMatrix& cMo, vpHomogeneousMatrix& cdMo);
80 void execute(unsigned int nbIter); //launch the simulation
81 void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type);
82 double error();
83 void _planeToABC(vpPlane& pl, double& A,double& B, double& C);
84 void paramRobot();
85 
86 #if !defined(WIN32) && !defined(VISP_HAVE_PTHREAD)
87 // Robot simulator used in this example is not available
88 int main()
89 {
90  std::cout << "Can't run this example since vpSimulatorAfma6 capability is not available." << std::endl;
91  std::cout << "You should install pthread third-party library." << std::endl;
92 }
93 // No display available
94 #elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && !defined(VISP_HAVE_GTK)
95 int main()
96 {
97  std::cout << "Can't run this example since no display capability is available." << std::endl;
98  std::cout << "You should install one of the following third-party library: X11, OpenCV, GDI, GTK." << std::endl;
99 }
100 #else
101 int main(){
102  //intial pose
103  vpHomogeneousMatrix cMo(-0.1,-0.1,1.5,-vpMath::rad(20),-vpMath::rad(20),-vpMath::rad(30));
104  //Desired pose
106 
107  //init and run the simulation
108  init(cMo,cdMo);
109  execute(1500);
110  return 0;
111 }
112 
113 //init the right display
114 #if defined VISP_HAVE_X11
115 vpDisplayX displayInt;
116 #elif defined VISP_HAVE_OPENCV
117 vpDisplayOpenCV displayInt;
118 #elif defined VISP_HAVE_GDI
119 vpDisplayGDI displayInt;
120 #elif defined VISP_HAVE_D3D9
121 vpDisplayD3D displayInt;
122 #elif defined VISP_HAVE_GTK
123 vpDisplayGTK displayInt;
124 #endif
125 
126 //start and destination positioning matrices
129 
130 vpSimulatorAfma6 robot(false);//robot used in this simulation
131 vpImage<vpRGBa> Iint(480,640, 255);//internal image used for interface display
132 vpServo task; //servoing task
133 vpCameraParameters cam;//robot camera parameters
134 double _error; //current error
135 vpServo::vpServoIteractionMatrixType interaction_type; //current or desired
136 vpImageSimulator imsim;//image simulator used to simulate the perspective-projection camera
137 
138 //source and destination objects for moment manipulation
139 vpMomentObject src(6);
140 vpMomentObject dst(6);
141 //moment sets and their corresponding features
142 vpMomentCommon *moments;
143 vpMomentCommon *momentsDes;
144 vpFeatureMomentCommon *featureMoments;
145 vpFeatureMomentCommon *featureMomentsDes;
146 
147 using namespace std;
148 
149 
150 void initScene(){
151  vector<vpPoint> src_pts;
152  vector<vpPoint> dst_pts;
153 
154  double x[5] = { 0.2, 0.2,-0.2,-0.2, 0.2 };
155  double y[5] = {-0.1, 0.1, 0.1,-0.1,-0.1 };
156  int nbpoints = 4;
157 
158  for (int i = 0 ; i < nbpoints ; i++){
159  vpPoint p;
160  p.setWorldCoordinates(x[i],y[i],0.0);
161  p.track(cMo) ;
162  src_pts.push_back(p);
163  }
164 
166  src.fromVector(src_pts);
167  for (int i = 0 ; i < nbpoints ; i++){
168  vpPoint p;
169  p.setWorldCoordinates(x[i],y[i],0.0);
170  p.track(cdMo) ;
171  dst_pts.push_back(p);
172  }
174  dst.fromVector(dst_pts);
175 
176 }
177 
178 void refreshScene(vpMomentObject &obj){
179  double x[5] = { 0.2, 0.2,-0.2,-0.2, 0.2 };
180  double y[5] = {-0.1, 0.1, 0.1,-0.1,-0.1 };
181  int nbpoints = 5;
182  vector<vpPoint> cur_pts;
183 
184  for (int i = 0 ; i < nbpoints ; i++){
185  vpPoint p;
186  p.setWorldCoordinates(x[i],y[i],0.0);
187  p.track(cMo) ;
188  cur_pts.push_back(p);
189  }
190  obj.fromVector(cur_pts);
191 }
192 
193 void init(vpHomogeneousMatrix& _cMo, vpHomogeneousMatrix& _cdMo)
194 {
195  cMo = _cMo;
196  cdMo = _cdMo;
197  interaction_type = vpServo::CURRENT;
198  displayInt.init(Iint,700,0, "Visual servoing with moments") ;
199 
200  paramRobot(); //set up robot parameters
201 
203  initScene(); //initialize graphical scene (for interface)
204  initFeatures();//initialize moment features
205 }
206 
207 void initFeatures(){
208  //A,B,C parameters of source and destination plane
209  double A; double B; double C;
210  double Ad; double Bd; double Cd;
211  //init main object: using moments up to order 6
212 
213  //Initializing values from regular plane (with ax+by+cz=d convention)
214  vpPlane pl;
215  pl.setABCD(0,0,1.0,0);
216  pl.changeFrame(cMo);
217  _planeToABC(pl,A,B,C);
218 
219  pl.setABCD(0,0,1.0,0);
220  pl.changeFrame(cdMo);
221  _planeToABC(pl,Ad,Bd,Cd);
222 
223  //extracting initial position (actually we only care about Zdst)
225  cdMo.extract(vec);
226 
228  //don't need to be specific, vpMomentCommon automatically loads Xg,Yg,An,Ci,Cj,Alpha moments
231  //same thing with common features
232  featureMoments = new vpFeatureMomentCommon(*moments);
233  featureMomentsDes = new vpFeatureMomentCommon(*momentsDes);
234 
235  moments->updateAll(src);
236  momentsDes->updateAll(dst);
237 
238  featureMoments->updateAll(A,B,C);
239  featureMomentsDes->updateAll(Ad,Bd,Cd);
240 
241  //setup the interaction type
242  task.setInteractionMatrixType(interaction_type) ;
244  task.addFeature(featureMoments->getFeatureGravityNormalized(),featureMomentsDes->getFeatureGravityNormalized());
245  task.addFeature(featureMoments->getFeatureAn(),featureMomentsDes->getFeatureAn());
246  //the moments are different in case of a symmetric object
247  task.addFeature(featureMoments->getFeatureCInvariant(),featureMomentsDes->getFeatureCInvariant(),(1 << 10) | (1 << 11));
248  task.addFeature(featureMoments->getFeatureAlpha(),featureMomentsDes->getFeatureAlpha());
249 
250  task.setLambda(0.4) ;
251 }
252 
253 void execute(unsigned int nbIter){
254  //init main object: using moments up to order 5
255  vpMomentObject obj(6);
256  //setting object type (disrete, continuous[form polygon])
258 
259  vpTRACE("Display task information " ) ;
260  task.print() ;
261 
262  vpDisplay::display(Iint);
263  robot.getInternalView(Iint);
264  vpDisplay::flush(Iint);
265  unsigned int iter=0;
266  double t=0;
268  while(iter++<nbIter ){
269  vpColVector v ;
270  t = vpTime::measureTimeMs();
271  //get the cMo
272  cMo = robot.get_cMo();
273  //setup the plane in A,B,C style
274  vpPlane pl;
275  double A,B,C;
276  pl.setABCD(0,0,1.0,0);
277  pl.changeFrame(cMo);
278  _planeToABC(pl,A,B,C);
279 
280  //track points, draw points and add refresh our object
281  refreshScene(obj);
282  //this is the most important thing to do: update our moments
283  moments->updateAll(obj);
284  //and update our features. Do it in that order. Features need to use the information computed by moments
285  featureMoments->updateAll(A,B,C);
286 
287  vpDisplay::display(Iint) ;
288  robot.getInternalView(Iint);
289  vpDisplay::flush(Iint);
290 
291  if (iter == 1)
292  vpDisplay::getClick(Iint) ;
293  v = task.computeControlLaw() ;
294 
295  //pilot robot using position control. The displacement is t*v with t=10ms step
296  robot.setPosition(vpRobot::CAMERA_FRAME,0.01*v);
297 
298  vpTime::wait(t,10);
299  _error = ( task.getError() ).sumSquare();
300  }
301 
302  task.kill();
303 
304  vpTRACE("\n\nClick in the internal view window to end...");
305  vpDisplay::getClick(Iint) ;
306 
307  delete moments;
308  delete momentsDes;
309  delete featureMoments;
310  delete featureMomentsDes;
311 }
312 
313 void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type){interaction_type=type;}
314 double error(){return _error;}
315 
316 void removeJointLimits(vpSimulatorAfma6& robot){
317  vpColVector limMin(6);
318  vpColVector limMax(6);
319  limMin[0] = vpMath::rad(-3600);
320  limMin[1] = vpMath::rad(-3600);
321  limMin[2] = vpMath::rad(-3600);
322  limMin[3] = vpMath::rad(-3600);
323  limMin[4] = vpMath::rad(-3600);
324  limMin[5] = vpMath::rad(-3600);
325 
326  limMax[0] = vpMath::rad(3600);
327  limMax[1] = vpMath::rad(3600);
328  limMax[2] = vpMath::rad(3600);
329  limMax[3] = vpMath::rad(3600);
330  limMax[4] = vpMath::rad(3600);
331  limMax[5] = vpMath::rad(3600);
332 
333  robot.setJointLimit(limMin,limMax);
334 }
335 
336 void _planeToABC(vpPlane& pl, double& A,double& B, double& C){
337  if(fabs(pl.getD())<std::numeric_limits<double>::epsilon()){
338  std::cout << "Invalid position:" << std::endl;
339  std::cout << cMo << std::endl;
340  std::cout << "Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
341  throw vpException(vpException::divideByZeroError,"invalid position!");
342  }
343  A=-pl.getA()/pl.getD();
344  B=-pl.getB()/pl.getD();
345  C=-pl.getC()/pl.getD();
346 }
347 
348 void paramRobot(){
349  /*Initialise the robot and especially the camera*/
351  robot.setCurrentViewColor(vpColor(150,150,150));
352  robot.setDesiredViewColor(vpColor(200,200,200));
354  removeJointLimits(robot);
356  /*Initialise the position of the object relative to the pose of the robot's camera*/
358 
359  /*Set the desired position (for the displaypart)*/
360  robot.setDesiredCameraPosition(cdMo);
361  robot.getCameraParameters(cam,Iint);
362 }
363 
364 #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.
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
void setPosition(const vpHomogeneousMatrix &cMw)
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
Initialize the position controller.
Definition: vpRobot.h:71
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)
static double measureTimeMs()
Definition: vpTime.cpp:86
static int wait(double t0, double t)
Definition: vpTime.cpp:149
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
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
A 40cm by 40cm plate with 4 points at coordinates (-0.07,-0.05,0), (0.07,0.05,0), (0...
void updateAll(vpMomentObject &object)
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
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()
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