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