Visual Servoing Platform  version 3.3.1 under development (2020-05-29)
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  task.kill();
213  return EXIT_SUCCESS;
214  } catch (const vpException &e) {
215  std::cout << "Catch a ViSP exception: " << e << std::endl;
216  return EXIT_FAILURE;
217  }
218 #else
219  (void)argc;
220  (void)argv;
221  std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
222  return EXIT_SUCCESS;
223 #endif
224 }
void buildFrom(double x, double y, double Z)
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, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:496
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpHomogeneousMatrix inverse() const
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
vpHomogeneousMatrix getPosition() 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:81
void kill()
Definition: vpServo.cpp:191
vpColVector computeControlLaw()
Definition: vpServo.cpp:935
void setLambda(double c)
Definition: vpServo.h:406
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:312
vpColVector getError() const
Definition: vpServo.h:282
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:222