Visual Servoing Platform  version 3.5.1 under development (2022-12-02)
servoSimu3D_cMcd_CamVelocityWithoutVpServo.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 3D visual servoing.
33  *
34  * Authors:
35  * Eric Marchand
36  * Fabien Spindler
37  *
38  *****************************************************************************/
82 #include <stdio.h>
83 #include <stdlib.h>
84 
85 #include <visp3/core/vpHomogeneousMatrix.h>
86 #include <visp3/core/vpIoTools.h>
87 #include <visp3/core/vpMath.h>
88 #include <visp3/io/vpParseArgv.h>
89 #include <visp3/robot/vpSimulatorCamera.h>
90 #include <visp3/visual_features/vpFeatureThetaU.h>
91 #include <visp3/visual_features/vpFeatureTranslation.h>
92 #include <visp3/vs/vpServo.h>
93 
94 // List of allowed command line options
95 #define GETOPTARGS "h"
96 
97 void usage(const char *name, const char *badparam);
98 bool getOptions(int argc, const char **argv);
99 
108 void usage(const char *name, const char *badparam)
109 {
110  fprintf(stdout, "\n\
111 Simulation of a 3D visual servoing:\n\
112 - eye-in-hand control law,\n\
113 - velocity computed in the camera frame,\n\
114 - without display.\n\
115 \n\
116 SYNOPSIS\n\
117  %s [-h]\n",
118  name);
119 
120  fprintf(stdout, "\n\
121 OPTIONS: Default\n\
122 \n\
123  -h\n\
124  Print the help.\n");
125 
126  if (badparam)
127  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
128 }
129 
139 bool getOptions(int argc, const char **argv)
140 {
141  const char *optarg_;
142  int c;
143  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
144 
145  switch (c) {
146  case 'h':
147  usage(argv[0], NULL);
148  return false;
149 
150  default:
151  usage(argv[0], optarg_);
152  return false;
153  }
154  }
155 
156  if ((c == 1) || (c == -1)) {
157  // standalone param or error
158  usage(argv[0], NULL);
159  std::cerr << "ERROR: " << std::endl;
160  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
161  return false;
162  }
163 
164  return true;
165 }
166 
167 int main(int argc, const char **argv)
168 {
169  try {
170  // Read the command line options
171  if (getOptions(argc, argv) == false) {
172  exit(-1);
173  }
174 
175  // Log file creation in /tmp/$USERNAME/log.dat
176  // This file contains by line:
177  // - the 6 computed camera velocities (m/s, rad/s) to achieve the task
178  // - the 6 values of s - s*
179  std::string username;
180  // Get the user login name
181  vpIoTools::getUserName(username);
182 
183  // Create a log filename to save velocities...
184  std::string logdirname;
185 #if defined(_WIN32)
186  logdirname = "C:/temp/" + username;
187 #else
188  logdirname = "/tmp/" + username;
189 #endif
190  // Test if the output path exist. If no try to create it
191  if (vpIoTools::checkDirectory(logdirname) == false) {
192  try {
193  // Create the dirname
194  vpIoTools::makeDirectory(logdirname);
195  } catch (...) {
196  std::cerr << std::endl << "ERROR:" << std::endl;
197  std::cerr << " Cannot create " << logdirname << std::endl;
198  exit(-1);
199  }
200  }
201  std::string logfilename;
202  logfilename = logdirname + "/log.dat";
203 
204  // Open the log file name
205  std::ofstream flog(logfilename.c_str());
206 
207  vpSimulatorCamera robot;
208 
209  std::cout << std::endl;
210  std::cout << "-------------------------------------------------------" << std::endl;
211  std::cout << " Test program for vpServo " << std::endl;
212  std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl;
213  std::cout << " Simulation " << std::endl;
214  std::cout << " task : 3D visual servoing " << std::endl;
215  std::cout << "-------------------------------------------------------" << std::endl;
216  std::cout << std::endl;
217 
218  // Sets the initial camera location
219  vpPoseVector c_r_o( // Translation tx,ty,tz
220  0.1, 0.2, 2,
221  // ThetaU rotation
222  vpMath::rad(20), vpMath::rad(10), vpMath::rad(50));
223 
224  // From the camera pose build the corresponding homogeneous matrix
225  vpHomogeneousMatrix cMo(c_r_o);
226 
227  // Set the robot initial position
228  vpHomogeneousMatrix wMc, wMo;
229  robot.getPosition(wMc);
230  wMo = wMc * cMo; // Compute the position of the object in the world frame
231 
232  // Sets the desired camera location
233  vpPoseVector cd_r_o( // Translation tx,ty,tz
234  0, 0, 1,
235  // ThetaU rotation
236  vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
237 
238  // From the camera desired pose build the corresponding homogeneous matrix
239  vpHomogeneousMatrix cdMo(cd_r_o);
240 
241  vpHomogeneousMatrix cMcd; // Transformation between current and desired camera frame
242  vpRotationMatrix cRcd; // Rotation between current and desired camera frame
243 
244  // Set the constant gain of the servo
245  double lambda = 1;
246 
247  unsigned int iter = 0;
248  // Start the visual servoing loop. We stop the servo after 200 iterations
249  while (iter++ < 200) {
250  std::cout << "------------------------------------" << iter << std::endl;
251 
252  // get the robot position
253  robot.getPosition(wMc);
254  // Compute the position of the object frame in the camera frame
255  cMo = wMc.inverse() * wMo;
256 
257  // new displacement to achieve
258  cMcd = cMo * cdMo.inverse();
259 
260  // Extract the translation vector ctc* which is the current
261  // translational visual feature.
262  vpTranslationVector ctcd;
263  cMcd.extract(ctcd);
264  // Compute the current theta U visual feature
265  vpThetaUVector tu_cRcd(cMcd);
266 
267  // Create the identity matrix
268  vpMatrix I(3, 3);
269  I.eye();
270 
271  // Compute the camera translational velocity
272  vpColVector v(3);
273  v = lambda * (I - vpColVector::skew(tu_cRcd)) * ctcd;
274  // Compute the camera rotational velocity
275  vpColVector w(3);
276  w = lambda * tu_cRcd;
277 
278  // Update the complete camera velocity vector
279  vpColVector velocity(6);
280  for (unsigned int i = 0; i < 3; i++) {
281  velocity[i] = v[i]; // Translational velocity
282  velocity[i + 3] = w[i]; // Rotational velocity
283  }
284 
285  // Send the camera velocity to the controller
286  robot.setVelocity(vpRobot::CAMERA_FRAME, velocity);
287 
288  // Retrieve the error (s-s*)
289  std::cout << "|| s - s* || = " << ctcd.t() << " " << tu_cRcd.t() << std::endl;
290 
291  // Save log
292  flog << velocity.t() << " " << ctcd.t() << " " << tu_cRcd.t() << std::endl;
293  }
294 
295  // Close the log file
296  flog.close();
297  return EXIT_SUCCESS;
298  } catch (const vpException &e) {
299  std::cout << "Catch a ViSP exception: " << e << std::endl;
300  return EXIT_FAILURE;
301  }
302 }
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
static vpMatrix skew(const vpColVector &v)
vpRowVector t() const
error that can be emited by ViSP classes.
Definition: vpException.h:72
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
void extract(vpRotationMatrix &R) const
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:431
static std::string getUserName()
Definition: vpIoTools.cpp:327
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:581
static double rad(double deg)
Definition: vpMath.h:117
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
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ CAMERA_FRAME
Definition: vpRobot.h:83
Implementation of a rotation matrix and operations on such kind of matrices.
vpColVector t() const
Class that defines the simplest robot: a free flying camera.
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.
vpRowVector t() const