Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
servoSimuPoint2DCamVelocity1.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
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Simulation of a 2D visual servoing on a point.
32  *
33  * Authors:
34  * Eric Marchand
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
47 #include <stdlib.h>
48 #include <stdio.h>
49 
50 #include <visp3/visual_features/vpFeatureBuilder.h>
51 #include <visp3/visual_features/vpFeaturePoint.h>
52 #include <visp3/core/vpHomogeneousMatrix.h>
53 #include <visp3/core/vpMath.h>
54 #include <visp3/io/vpParseArgv.h>
55 #include <visp3/vs/vpServo.h>
56 #include <visp3/robot/vpSimulatorCamera.h>
57 
58 // List of allowed command line options
59 #define GETOPTARGS "h"
60 
61 void usage(const char *name, const char *badparam);
62 bool getOptions(int argc, const char **argv);
63 
72 void usage(const char *name, const char *badparam)
73 {
74  fprintf(stdout, "\n\
75 Simulation of a 2D visual servoing on a point:\n\
76 - eye-in-hand control law,\n\
77 - velocity computed in the camera frame,\n\
78 - without display.\n\
79  \n\
80 SYNOPSIS\n\
81  %s [-h]\n", name);
82 
83  fprintf(stdout, "\n\
84 OPTIONS: Default\n\
85  \n\
86  -h\n\
87  Print the help.\n");
88 
89  if (badparam)
90  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
91 }
92 
103 bool getOptions(int argc, const char **argv)
104 {
105  const char *optarg_;
106  int c;
107  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
108 
109  switch (c) {
110  case 'h': usage(argv[0], NULL); return false; break;
111 
112  default:
113  usage(argv[0], optarg_);
114  return false; break;
115  }
116  }
117 
118  if ((c == 1) || (c == -1)) {
119  // standalone param or error
120  usage(argv[0], NULL);
121  std::cerr << "ERROR: " << std::endl;
122  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
123  return false;
124  }
125 
126  return true;
127 }
128 
129 int
130 main(int argc, const char ** argv)
131 {
132  try {
133  // Read the command line options
134  if (getOptions(argc, argv) == false) {
135  exit (-1);
136  }
137 
138  vpServo task ;
139  vpSimulatorCamera robot ;
140 
141  // sets the initial camera location
143  cMo[0][3] = 0.1;
144  cMo[1][3] = 0.2;
145  cMo[2][3] = 2;
146 
147  // Compute the position of the object in the world frame
148  vpHomogeneousMatrix wMc, wMo;
149  robot.getPosition(wMc);
150  wMo = wMc * cMo;
151 
152  // sets the point coordinates in the world frame
153  vpPoint point(0, 0, 0);
154 
155  // computes the point coordinates in the camera frame and its 2D coordinates
156  point.track(cMo);
157 
158  // sets the current position of the visual feature
159  vpFeaturePoint p ;
160  vpFeatureBuilder::create(p,point); //retrieve x,y and Z of the vpPoint structure
161 
162  // sets the desired position of the visual feature
163  vpFeaturePoint pd;
164  pd.buildFrom(0,0,1); // buildFrom(x,y,Z) ;
165 
166  // define the task
167  // - we want an eye-in-hand control law
168  // - robot is controlled in the camera frame
170 
171  // we want to see a point on a point
172  std::cout << std::endl ;
173  task.addFeature(p,pd) ;
174 
175  // set the gain
176  task.setLambda(1) ;
177 
178  // Display task information
179  task.print() ;
180 
181  unsigned int iter=0 ;
182  // loop
183  while(iter++<100)
184  {
185  std::cout << "---------------------------------------------" << iter <<std::endl ;
186  vpColVector v ;
187 
188  // get the robot position
189  robot.getPosition(wMc) ;
190  // Compute the position of the camera wrt the object frame
191  cMo = wMc.inverse() * wMo;
192 
193  // new point position
194  point.track(cMo) ;
195  //retrieve x,y and Z of the vpPoint structure
196  vpFeatureBuilder::create(p,point);
197 
198  // compute the control law
199  v = task.computeControlLaw() ;
200 
201  // send the camera velocity to the controller
203 
204  std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() <<std::endl ;
205  }
206 
207  // Display task information
208  task.print() ;
209  task.kill();
210  return 0;
211  }
212  catch(vpException &e) {
213  std::cout << "Catch a ViSP exception: " << e << std::endl;
214  return 1;
215  }
216 }
217 
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines the simplest robot: a free flying camera.
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:512
error that can be emited by ViSP classes.
Definition: vpException.h:73
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:76
Class that defines what is a point.
Definition: vpPoint.h:59
void kill()
Definition: vpServo.cpp:191
vpColVector getError() const
Definition: vpServo.h:271
vpColVector computeControlLaw()
Definition: vpServo.cpp:954
void setLambda(double c)
Definition: vpServo.h:391
vpHomogeneousMatrix getPosition() const
void buildFrom(const double x, const double y, const double Z)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
vpHomogeneousMatrix inverse() const
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:314
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:222