Visual Servoing Platform  version 3.4.0
servoSimuPoint2DCamVelocity3.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 
54 #include <stdio.h>
55 #include <stdlib.h>
56 
57 #include <visp3/core/vpHomogeneousMatrix.h>
58 #include <visp3/core/vpMath.h>
59 #include <visp3/io/vpParseArgv.h>
60 #include <visp3/robot/vpSimulatorCamera.h>
61 #include <visp3/visual_features/vpFeatureBuilder.h>
62 #include <visp3/visual_features/vpFeaturePoint.h>
63 #include <visp3/vs/vpServo.h>
64 
65 // List of allowed command line options
66 #define GETOPTARGS "h"
67 
68 void usage(const char *name, const char *badparam);
69 bool getOptions(int argc, const char **argv);
70 
79 void usage(const char *name, const char *badparam)
80 {
81  fprintf(stdout, "\n\
82 Simulation of a 2D visual servoing on a point:\n\
83 - eye-in-hand control law,\n\
84 - articular velocity are computed,\n\
85 - without display,\n\
86 - only the X coordinate of the point is selected.\n\
87  \n\
88 SYNOPSIS\n\
89  %s [-h]\n", name);
90 
91  fprintf(stdout, "\n\
92 OPTIONS: Default\n\
93  \n\
94  -h\n\
95  Print the help.\n");
96 
97  if (badparam)
98  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
99 }
100 
111 bool getOptions(int argc, const char **argv)
112 {
113  const char *optarg_;
114  int c;
115  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
116 
117  switch (c) {
118  case 'h':
119  usage(argv[0], NULL);
120  return false;
121 
122  default:
123  usage(argv[0], optarg_);
124  return false;
125  }
126  }
127 
128  if ((c == 1) || (c == -1)) {
129  // standalone param or error
130  usage(argv[0], NULL);
131  std::cerr << "ERROR: " << std::endl;
132  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
133  return false;
134  }
135 
136  return true;
137 }
138 
139 int main(int argc, const char **argv)
140 {
141 #if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
142  try {
143  // Read the command line options
144  if (getOptions(argc, argv) == false) {
145  exit(-1);
146  }
147 
148  vpServo task;
149  vpSimulatorCamera robot;
150 
151  std::cout << std::endl;
152  std::cout << "-------------------------------------------------------" << std::endl;
153  std::cout << " Test program for vpServo " << std::endl;
154  std::cout << " Eye-in-hand task control, articular velocity are computed" << std::endl;
155  std::cout << " Simulation " << std::endl;
156  std::cout << " task : servo a point " << std::endl;
157  std::cout << "-------------------------------------------------------" << std::endl;
158  std::cout << std::endl;
159 
160  // sets the initial camera location
162  cMo[0][3] = 0.1;
163  cMo[1][3] = 0.2;
164  cMo[2][3] = 2;
165  // Compute the position of the object in the world frame
166  vpHomogeneousMatrix wMc, wMo;
167  robot.getPosition(wMc);
168  wMo = wMc * cMo;
169 
170  // sets the point coordinates in the world frame
171  vpPoint point(0, 0, 0);
172 
173  // computes the point coordinates in the camera frame and its 2D
174  // coordinates
175  point.track(cMo);
176 
177  // sets the current position of the visual feature
178  vpFeaturePoint p;
179  vpFeatureBuilder::create(p, point); // retrieve x,y and Z of the vpPoint structure
180 
181  // sets the desired position of the visual feature
182  vpFeaturePoint pd;
183  pd.buildFrom(0, 0, 1); // buildFrom(x,y,Z) ;
184 
185  // define the task
186  // - we want an eye-in-hand control law
187  // - articular velocity are computed
189 
190  // Set the position of the end-effector frame in the camera frame as identity
192  vpVelocityTwistMatrix cVe(cMe);
193  task.set_cVe(cVe);
194 
195  // Set the Jacobian (expressed in the end-effector frame)
196  vpMatrix eJe;
197  robot.get_eJe(eJe);
198  task.set_eJe(eJe);
199 
200  // we want to see a point on a point
201  task.addFeature(p, pd, vpFeaturePoint::selectX());
202 
203  // set the gain
204  task.setLambda(1);
205 
206  // Display task information
207  task.print();
208 
209  unsigned int iter = 0;
210  // loop
211  while (iter++ < 100) {
212  std::cout << "---------------------------------------------" << iter << std::endl;
213  vpColVector v;
214 
215  // Set the Jacobian (expressed in the end-effector frame)
216  // since q is modified eJe is modified
217  robot.get_eJe(eJe);
218  task.set_eJe(eJe);
219 
220  // get the robot position
221  robot.getPosition(wMc);
222  // Compute the position of the object frame in the camera frame
223  cMo = wMc.inverse() * wMo;
224 
225  // new point position
226  point.track(cMo);
227  vpFeatureBuilder::create(p, point); // retrieve x,y and Z of the vpPoint structure
228 
229  // compute the control law
230  v = task.computeControlLaw();
231 
232  // send the camera velocity to the controller
234 
235  std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
236  }
237 
238  // Display task information
239  task.print();
240  return EXIT_SUCCESS;
241  } catch (const vpException &e) {
242  std::cout << "Catch a ViSP exception: " << e << std::endl;
243  return EXIT_FAILURE;
244  }
245 #else
246  (void)argc;
247  (void)argv;
248  std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
249  return EXIT_SUCCESS;
250 #endif
251 }
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)
static unsigned int selectX()
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
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)
void get_eJe(vpMatrix &eJe)
@ CAMERA_FRAME
Definition: vpRobot.h:82
@ EYEINHAND_L_cVe_eJe
Definition: vpServo.h:159
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:448
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 set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:506
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.