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