Visual Servoing Platform  version 3.5.1 under development (2022-12-02)
servoSimuFourPoints2DCamVelocity.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 using 4 points as visual feature.
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/vpConfig.h>
58 #include <visp3/core/vpHomogeneousMatrix.h>
59 #include <visp3/core/vpMath.h>
60 #include <visp3/io/vpParseArgv.h>
61 #include <visp3/robot/vpSimulatorCamera.h>
62 #include <visp3/visual_features/vpFeatureBuilder.h>
63 #include <visp3/visual_features/vpFeaturePoint.h>
64 #include <visp3/vs/vpServo.h>
65 
66 // List of allowed command line options
67 #define GETOPTARGS "h"
68 
69 void usage(const char *name, const char *badparam);
70 bool getOptions(int argc, const char **argv);
71 
80 void usage(const char *name, const char *badparam)
81 {
82  fprintf(stdout, "\n\
83 Simulation of a 2D visual servoing:\n\
84 - servo on 4 points,\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(stderr, "ERROR: \n");
101  fprintf(stderr, "\nBad parameter [%s]\n", badparam);
102  }
103 }
104 
115 bool getOptions(int argc, const char **argv)
116 {
117  const char *optarg_;
118  int c;
119  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
120 
121  switch (c) {
122  case 'h':
123  usage(argv[0], NULL);
124  return false;
125 
126  default:
127  usage(argv[0], optarg_);
128  return false;
129  }
130  }
131 
132  if ((c == 1) || (c == -1)) {
133  // standalone param or error
134  usage(argv[0], NULL);
135  std::cerr << "ERROR: " << std::endl;
136  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
137  return false;
138  }
139 
140  return true;
141 }
142 
143 int main(int argc, const char **argv)
144 {
145 #if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
146  try {
147  // Read the command line options
148  if (getOptions(argc, argv) == false) {
149  exit(-1);
150  }
151 
152  vpServo task;
153  vpSimulatorCamera robot;
154 
155  std::cout << std::endl;
156  std::cout << "-------------------------------------------------------" << std::endl;
157  std::cout << " Test program for vpServo " << std::endl;
158  std::cout << " Eye-in-hand task control, articular velocity are computed" << std::endl;
159  std::cout << " Simulation " << std::endl;
160  std::cout << " task : servo 4 points " << std::endl;
161  std::cout << "-------------------------------------------------------" << std::endl;
162  std::cout << std::endl;
163 
164  // sets the initial camera location with respect to the object
166  cMo[0][3] = 0.1;
167  cMo[1][3] = 0.2;
168  cMo[2][3] = 2;
169 
170  // Compute the position of the object in the world frame
171  vpHomogeneousMatrix wMc, wMo;
172  robot.getPosition(wMc);
173  wMo = wMc * cMo;
174 
175  // sets the point coordinates in the object frame
176  vpPoint point[4];
177  point[0].setWorldCoordinates(-1, -1, 0);
178  point[1].setWorldCoordinates(1, -1, 0);
179  point[2].setWorldCoordinates(1, 1, 0);
180  point[3].setWorldCoordinates(-1, 1, 0);
181 
182  // computes the point coordinates in the camera frame and its 2D
183  // coordinates
184  for (unsigned int i = 0; i < 4; i++)
185  point[i].track(cMo);
186 
187  // sets the desired position of the point
188  vpFeaturePoint p[4];
189  for (unsigned int i = 0; i < 4; i++)
190  vpFeatureBuilder::create(p[i], point[i]); // retrieve x,y and Z of the vpPoint structure
191 
192  // sets the desired position of the point
193  vpFeaturePoint pd[4];
194 
195  pd[0].buildFrom(-0.1, -0.1, 1);
196  pd[1].buildFrom(0.1, -0.1, 1);
197  pd[2].buildFrom(0.1, 0.1, 1);
198  pd[3].buildFrom(-0.1, 0.1, 1);
199 
200  // define the task
201  // - we want an eye-in-hand control law
202  // - articular velocity are computed
205 
206  // Set the position of the end-effector frame in the camera frame as identity
208  vpVelocityTwistMatrix cVe(cMe);
209  task.set_cVe(cVe);
210 
211  // Set the Jacobian (expressed in the end-effector frame)
212  vpMatrix eJe;
213  robot.get_eJe(eJe);
214  task.set_eJe(eJe);
215 
216  // we want to see a point on a point
217  for (unsigned int i = 0; i < 4; i++)
218  task.addFeature(p[i], pd[i]);
219 
220  // set the gain
221  task.setLambda(1);
222 
223  // Display task information
224  task.print();
225 
226  unsigned int iter = 0;
227  // loop
228  while (iter++ < 1500) {
229  std::cout << "---------------------------------------------" << iter << std::endl;
230  vpColVector v;
231 
232  // Set the Jacobian (expressed in the end-effector frame)
233  // since q is modified eJe is modified
234  robot.get_eJe(eJe);
235  task.set_eJe(eJe);
236 
237  // get the robot position
238  robot.getPosition(wMc);
239  // Compute the position of the object frame in the camera frame
240  cMo = wMc.inverse() * wMo;
241 
242  // update new point position and corresponding features
243  for (unsigned int i = 0; i < 4; i++) {
244  point[i].track(cMo);
245  // retrieve x,y and Z of the vpPoint structure
246  vpFeatureBuilder::create(p[i], point[i]);
247  }
248  // since vpServo::MEAN interaction matrix is used, we need also to
249  // update the desired features at each iteration
250  pd[0].buildFrom(-0.1, -0.1, 1);
251  pd[1].buildFrom(0.1, -0.1, 1);
252  pd[2].buildFrom(0.1, 0.1, 1);
253  pd[3].buildFrom(-0.1, 0.1, 1);
254 
255  // compute the control law ") ;
256  v = task.computeControlLaw();
257 
258  // send the camera velocity to the controller ") ;
260 
261  std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
262  }
263 
264  // Display task information
265  task.print();
266  return EXIT_SUCCESS;
267  } catch (const vpException &e) {
268  std::cout << "Catch a ViSP exception: " << e << std::endl;
269  return EXIT_FAILURE;
270  }
271 #else
272  (void)argc;
273  (void)argv;
274  std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
275  return EXIT_SUCCESS;
276 #endif
277 }
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)
void track(const vpHomogeneousMatrix &cMo)
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 setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:113
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.