Visual Servoing Platform  version 3.5.0
servoSimuPoint2DCamVelocity1.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 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  * Simulation of a 2D visual servoing on a point.
33  *
34  * Authors:
35  * Eric Marchand
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 
48 #include <stdio.h>
49 #include <stdlib.h>
50 
51 #include <visp3/core/vpHomogeneousMatrix.h>
52 #include <visp3/core/vpMath.h>
53 #include <visp3/io/vpParseArgv.h>
54 #include <visp3/robot/vpSimulatorCamera.h>
55 #include <visp3/visual_features/vpFeatureBuilder.h>
56 #include <visp3/visual_features/vpFeaturePoint.h>
57 #include <visp3/vs/vpServo.h>
58 
59 // List of allowed command line options
60 #define GETOPTARGS "h"
61 
62 void usage(const char *name, const char *badparam);
63 bool getOptions(int argc, const char **argv);
64 
73 void usage(const char *name, const char *badparam)
74 {
75  fprintf(stdout, "\n\
76 Simulation of a 2D visual servoing on a point:\n\
77 - eye-in-hand control law,\n\
78 - velocity computed in the camera frame,\n\
79 - without display.\n\
80  \n\
81 SYNOPSIS\n\
82  %s [-h]\n", name);
83 
84  fprintf(stdout, "\n\
85 OPTIONS: Default\n\
86  \n\
87  -h\n\
88  Print the help.\n");
89 
90  if (badparam)
91  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
92 }
93 
104 bool getOptions(int argc, const char **argv)
105 {
106  const char *optarg_;
107  int c;
108  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
109 
110  switch (c) {
111  case 'h':
112  usage(argv[0], NULL);
113  return false;
114 
115  default:
116  usage(argv[0], optarg_);
117  return false;
118  }
119  }
120 
121  if ((c == 1) || (c == -1)) {
122  // standalone param or error
123  usage(argv[0], NULL);
124  std::cerr << "ERROR: " << std::endl;
125  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
126  return false;
127  }
128 
129  return true;
130 }
131 
132 int main(int argc, const char **argv)
133 {
134 #if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
135  try {
136  // Read the command line options
137  if (getOptions(argc, argv) == false) {
138  exit(-1);
139  }
140 
141  vpServo task;
142  vpSimulatorCamera robot;
143 
144  // sets the initial camera location
146  cMo[0][3] = 0.1;
147  cMo[1][3] = 0.2;
148  cMo[2][3] = 2;
149 
150  // Compute the position of the object in the world frame
151  vpHomogeneousMatrix wMc, wMo;
152  robot.getPosition(wMc);
153  wMo = wMc * cMo;
154 
155  // sets the point coordinates in the world frame
156  vpPoint point(0, 0, 0);
157 
158  // computes the point coordinates in the camera frame and its 2D
159  // coordinates
160  point.track(cMo);
161 
162  // sets the current position of the visual feature
163  vpFeaturePoint p;
164  vpFeatureBuilder::create(p, point); // retrieve x,y and Z of the vpPoint structure
165 
166  // sets the desired position of the visual feature
167  vpFeaturePoint pd;
168  pd.buildFrom(0, 0, 1); // buildFrom(x,y,Z) ;
169 
170  // define the task
171  // - we want an eye-in-hand control law
172  // - robot is controlled in the camera frame
174 
175  // we want to see a point on a point
176  std::cout << std::endl;
177  task.addFeature(p, pd);
178 
179  // set the gain
180  task.setLambda(1);
181 
182  // Display task information
183  task.print();
184 
185  unsigned int iter = 0;
186  // loop
187  while (iter++ < 100) {
188  std::cout << "---------------------------------------------" << iter << std::endl;
189  vpColVector v;
190 
191  // get the robot position
192  robot.getPosition(wMc);
193  // Compute the position of the object frame in the camera frame
194  cMo = wMc.inverse() * wMo;
195 
196  // new point position
197  point.track(cMo);
198  // retrieve x,y and Z of the vpPoint structure
199  vpFeatureBuilder::create(p, point);
200 
201  // compute the control law
202  v = task.computeControlLaw();
203 
204  // send the camera velocity to the controller
206 
207  std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
208  }
209 
210  // Display task information
211  task.print();
212  return EXIT_SUCCESS;
213  } catch (const vpException &e) {
214  std::cout << "Catch a ViSP exception: " << e << std::endl;
215  return EXIT_FAILURE;
216  }
217 #else
218  (void)argc;
219  (void)argv;
220  std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
221  return EXIT_SUCCESS;
222 #endif
223 }
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
error that can be emited by ViSP classes.
Definition: vpException.h:72
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void buildFrom(double x, double y, double Z)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ CAMERA_FRAME
Definition: vpRobot.h:82
@ EYEINHAND_CAMERA
Definition: vpServo.h:155
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:306
void setLambda(double c)
Definition: vpServo.h:404
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:218
vpColVector getError() const
Definition: vpServo.h:278
vpColVector computeControlLaw()
Definition: vpServo.cpp:929
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:490
Class that defines the simplest robot: a free flying camera.