Visual Servoing Platform  version 3.1.0
servoMomentPoints.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 discrete points as object
33  * container
34  *
35  * Authors:
36  * Filip Novotny
37  *
38  *****************************************************************************/
39 
45 #include <iostream>
46 #include <visp3/core/vpCameraParameters.h>
47 #include <visp3/core/vpConfig.h>
48 #include <visp3/core/vpDebug.h>
49 #include <visp3/core/vpHomogeneousMatrix.h>
50 #include <visp3/core/vpIoTools.h>
51 #include <visp3/core/vpMath.h>
52 #include <visp3/core/vpMomentCommon.h>
53 #include <visp3/core/vpMomentDatabase.h>
54 #include <visp3/core/vpMomentObject.h>
55 #include <visp3/core/vpPlane.h>
56 #include <visp3/gui/vpDisplayGDI.h>
57 #include <visp3/gui/vpDisplayGTK.h>
58 #include <visp3/gui/vpDisplayX.h>
59 #include <visp3/robot/vpSimulatorAfma6.h>
60 #include <visp3/visual_features/vpFeatureBuilder.h>
61 #include <visp3/visual_features/vpFeatureMomentCommon.h>
62 #include <visp3/visual_features/vpFeaturePoint.h>
63 #include <visp3/vs/vpServo.h>
64 
65 #if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD)
66 // Robot simulator used in this example is not available
67 int main()
68 {
69  std::cout << "Can't run this example since vpSimulatorAfma6 capability is "
70  "not available."
71  << std::endl;
72  std::cout << "You should install pthread third-party library." << std::endl;
73 }
74 // No display available
75 #elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && \
76  !defined(VISP_HAVE_GTK)
77 int main()
78 {
79  std::cout << "Can't run this example since no display capability is available." << std::endl;
80  std::cout << "You should install one of the following third-party library: "
81  "X11, OpenCV, GDI, GTK."
82  << std::endl;
83 }
84 #else
85 
86 // setup robot parameters
87 void paramRobot();
88 
89 // update moment objects and interface
90 void refreshScene(vpMomentObject &obj);
91 // initialize scene in the interface
92 void initScene();
93 // initialize the moment features
94 void initFeatures();
95 
96 void init(vpHomogeneousMatrix &cMo, vpHomogeneousMatrix &cdMo);
97 void execute(unsigned int nbIter); // launch the simulation
98 void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type);
99 double error();
100 void planeToABC(vpPlane &pl, double &A, double &B, double &C);
101 void paramRobot();
102 void removeJointLimits(vpSimulatorAfma6 &robot);
103 
104 int main()
105 {
106  try { // intial pose
107  vpHomogeneousMatrix cMo(0.05, 0.1, 1.5, vpMath::rad(30), vpMath::rad(20), -vpMath::rad(15));
108  // Desired pose
110 
111  // init and run the simulation
112  init(cMo, cdMo);
113  execute(1500);
114  return 0;
115  } catch (vpException &e) {
116  std::cout << "Catch an exception: " << e << std::endl;
117  return 1;
118  }
119 }
120 
121 // init the right display
122 #if defined VISP_HAVE_X11
123 vpDisplayX displayInt;
124 #elif defined VISP_HAVE_OPENCV
125 vpDisplayOpenCV displayInt;
126 #elif defined VISP_HAVE_GDI
127 vpDisplayGDI displayInt;
128 #elif defined VISP_HAVE_D3D9
129 vpDisplayD3D displayInt;
130 #elif defined VISP_HAVE_GTK
131 vpDisplayGTK displayInt;
132 #endif
133 
134 // start and destination positioning matrices
137 
138 vpSimulatorAfma6 robot(false); // robot used in this simulation
139 vpImage<vpRGBa> Iint(480, 640,
140  255); // internal image used for interface display
141 vpServo::vpServoIteractionMatrixType interaction_type; // current or desired
142 vpServo task; // servoing task
143 vpCameraParameters cam; // robot camera parameters
144 double _error; // current error
145 vpImageSimulator imsim; // image simulator used to simulate the
146  // perspective-projection camera
147 
148 // moment sets and their corresponding features
149 vpMomentCommon *moments;
150 vpMomentCommon *momentsDes;
151 vpFeatureMomentCommon *featureMoments;
152 vpFeatureMomentCommon *featureMomentsDes;
153 
154 // source and destination objects for moment manipulation
155 vpMomentObject src(6);
156 vpMomentObject dst(6);
157 
158 void initScene()
159 {
160  std::vector<vpPoint> src_pts;
161  std::vector<vpPoint> dst_pts;
162 
163  double x[8] = {1, 3, 4, -1, -3, -2, -1, 1};
164  double y[8] = {0, 1, 4, 4, -2, -2, 1, 0};
165  int nbpoints = 8;
166 
167  for (int i = 0; i < nbpoints; i++) {
168  vpPoint p(x[i] / 20, y[i] / 20, 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] / 20, y[i] / 20, 0.0);
177  p.track(cdMo);
178  dst_pts.push_back(p);
179  }
181  dst.fromVector(dst_pts);
182 }
183 
184 void initFeatures()
185 {
186  // A,B,C parameters of source and destination plane
187  double A;
188  double B;
189  double C;
190  double Ad;
191  double Bd;
192  double Cd;
193  // init main object: using moments up to order 6
194 
195  // Initializing values from regular plane (with ax+by+cz=d convention)
196  vpPlane pl;
197  pl.setABCD(0, 0, 1.0, 0);
198  pl.changeFrame(cMo);
199  planeToABC(pl, A, B, C);
200 
201  pl.setABCD(0, 0, 1.0, 0);
202  pl.changeFrame(cdMo);
203  planeToABC(pl, Ad, Bd, Cd);
204 
205  // extracting initial position (actually we only care about Zdst)
207  cdMo.extract(vec);
208 
211  // don't need to be specific, vpMomentCommon automatically loads
212  // Xg,Yg,An,Ci,Cj,Alpha moments
214  vpMomentCommon::getAlpha(dst), vec[2]);
216  vpMomentCommon::getAlpha(dst), vec[2]);
217  // same thing with common features
218  featureMoments = new vpFeatureMomentCommon(*moments);
219  featureMomentsDes = new vpFeatureMomentCommon(*momentsDes);
220 
221  moments->updateAll(src);
222  momentsDes->updateAll(dst);
223 
224  featureMoments->updateAll(A, B, C);
225  featureMomentsDes->updateAll(Ad, Bd, Cd);
226 
227  // setup the interaction type
228  task.setInteractionMatrixType(interaction_type);
231  task.addFeature(featureMoments->getFeatureGravityNormalized(), featureMomentsDes->getFeatureGravityNormalized());
232  task.addFeature(featureMoments->getFeatureAn(), featureMomentsDes->getFeatureAn());
233  task.addFeature(featureMoments->getFeatureCInvariant(), featureMomentsDes->getFeatureCInvariant(),
234  (1 << 3) | (1 << 5));
235  task.addFeature(featureMoments->getFeatureAlpha(), featureMomentsDes->getFeatureAlpha());
236 
237  task.setLambda(1.);
238 }
239 
240 void refreshScene(vpMomentObject &obj)
241 {
242  // double x[8] = { 0.05,0.15, 0.2,-0.05 ,-0.15,-0.1,-0.05,0.05};
243  // double y[8] = { 0,0.05, 0.2, 0.2, -0.1,-0.1, 0.05,0};
244  double x[8] = {1, 3, 4, -1, -3, -2, -1, 1};
245  double y[8] = {0, 1, 4, 4, -2, -2, 1, 0};
246  int nbpoints = 8;
247  std::vector<vpPoint> cur_pts;
248 
249  for (int i = 0; i < nbpoints; i++) {
250  vpPoint p(x[i] / 20, y[i] / 20, 0.0);
251  p.track(cMo);
252  cur_pts.push_back(p);
253  }
254  obj.fromVector(cur_pts);
255 }
256 
257 void init(vpHomogeneousMatrix &_cMo, vpHomogeneousMatrix &_cdMo)
258 
259 {
260  cMo = _cMo; // init source matrix
261  cdMo = _cdMo; // init destination matrix
262  interaction_type = vpServo::CURRENT; // use interaction matrix for current position
263 
264  displayInt.init(Iint, 700, 0, "Visual servoing with moments");
265 
266  paramRobot(); // set up robot parameters
267 
269  initScene(); // initialize graphical scene (for interface)
270  initFeatures(); // initialize moment features
271 }
272 
273 void execute(unsigned int nbIter)
274 {
275  // init main object: using moments up to order 6
276  vpMomentObject obj(6);
277  // setting object type (disrete, continuous[form polygon])
279 
280  vpTRACE("Display task information ");
281  task.print();
282 
283  vpDisplay::display(Iint);
284  robot.getInternalView(Iint);
285  vpDisplay::flush(Iint);
286  unsigned int iter = 0;
287 
289  while (iter++ < nbIter) {
290  vpColVector v;
291  // get the cMo
292  cMo = robot.get_cMo();
293  // setup the plane in A,B,C style
294  vpPlane pl;
295  double A, B, C;
296  pl.setABCD(0, 0, 1.0, 0);
297  pl.changeFrame(cMo);
298  planeToABC(pl, A, B, C);
299 
300  // track points, draw points and add refresh our object
301  refreshScene(obj);
302  // this is the most important thing to do: update our moments
303  moments->updateAll(obj);
304  // and update our features. Do it in that order. Features need to use the
305  // information computed by moments
306  featureMoments->updateAll(A, B, C);
307 
308  vpDisplay::display(Iint);
309  robot.getInternalView(Iint);
310  vpDisplay::flush(Iint);
311 
312  if (iter == 1)
313  vpDisplay::getClick(Iint);
314  v = task.computeControlLaw();
315 
316  // pilot robot using position control. The displacement is t*v with t=10ms
317  // step robot.setPosition(vpRobot::CAMERA_FRAME,0.01*v);
319 
320  _error = (task.getError()).sumSquare();
321  }
322 
323  task.kill();
324 
325  vpTRACE("\n\nClick in the internal view window to end...");
326  vpDisplay::getClick(Iint);
327 
328  delete moments;
329  delete momentsDes;
330  delete featureMoments;
331  delete featureMomentsDes;
332 }
333 
334 void removeJointLimits(vpSimulatorAfma6 &robot_)
335 {
336  vpColVector limMin(6);
337  vpColVector limMax(6);
338  limMin[0] = vpMath::rad(-3600);
339  limMin[1] = vpMath::rad(-3600);
340  limMin[2] = vpMath::rad(-3600);
341  limMin[3] = vpMath::rad(-3600);
342  limMin[4] = vpMath::rad(-3600);
343  limMin[5] = vpMath::rad(-3600);
344 
345  limMax[0] = vpMath::rad(3600);
346  limMax[1] = vpMath::rad(3600);
347  limMax[2] = vpMath::rad(3600);
348  limMax[3] = vpMath::rad(3600);
349  limMax[4] = vpMath::rad(3600);
350  limMax[5] = vpMath::rad(3600);
351 
352  robot_.setJointLimit(limMin, limMax);
353  robot_.setMaxRotationVelocity(99999);
354  robot_.setMaxTranslationVelocity(999999);
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  robot.setConstantSamplingTimeMode(true);
380  /*Initialise the position of the object relative to the pose of the robot's
381  * camera*/
382  robot.initialiseObjectRelativeToCamera(cMo);
383 
384  /*Set the desired position (for the displaypart)*/
385  robot.setDesiredCameraPosition(cdMo);
386  robot.getCameraParameters(cam, Iint);
387 }
388 
389 void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type) { interaction_type = type; }
390 double error() { return _error; }
391 
392 #endif
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void setMaxTranslationVelocity(const double maxVt)
Definition: vpRobot.cpp:239
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.
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)
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
Initialize the velocity controller.
Definition: vpRobot.h:67
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)
void setMaxRotationVelocity(const double maxVr)
Definition: vpRobot.cpp:260
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.