Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
servoSimu3D_cMcd_CamVelocity.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Simulation of a 3D visual servoing.
32  *
33  * Authors:
34  * Eric Marchand
35  * Fabien Spindler
36  *
37  *****************************************************************************/
57 #include <stdlib.h>
58 #include <stdio.h>
59 
60 #include <visp3/visual_features/vpFeatureThetaU.h>
61 #include <visp3/visual_features/vpFeatureTranslation.h>
62 #include <visp3/core/vpHomogeneousMatrix.h>
63 #include <visp3/core/vpIoTools.h>
64 #include <visp3/core/vpMath.h>
65 #include <visp3/io/vpParseArgv.h>
66 #include <visp3/vs/vpServo.h>
67 #include <visp3/robot/vpSimulatorCamera.h>
68 
69 // List of allowed command line options
70 #define GETOPTARGS "h"
71 
72 void usage(const char *name, const char *badparam);
73 bool getOptions(int argc, const char **argv);
74 
83 void usage(const char *name, const char *badparam)
84 {
85  fprintf(stdout, "\n\
86 Simulation of a 3D visual servoing:\n\
87 - eye-in-hand control law,\n\
88 - velocity computed in the camera frame,\n\
89 - without display.\n\
90  \n\
91 SYNOPSIS\n\
92  %s [-h]\n", name);
93 
94  fprintf(stdout, "\n\
95 OPTIONS: Default\n\
96  \n\
97  -h\n\
98  Print the help.\n");
99 
100  if (badparam)
101  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
102 }
103 
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': usage(argv[0], NULL); return false; break;
121 
122  default:
123  usage(argv[0], optarg_);
124  return false; break;
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
140 main(int argc, const char ** argv)
141 {
142  try {
143  // Read the command line options
144  if (getOptions(argc, argv) == false) {
145  exit (-1);
146  }
147 
148  // Log file creation in /tmp/$USERNAME/log.dat
149  // This file contains by line:
150  // - the 6 computed camera velocities (m/s, rad/s) to achieve the task
151  // - the 6 values of s - s*
152  std::string username;
153  // Get the user login name
154  vpIoTools::getUserName(username);
155 
156  // Create a log filename to save velocities...
157  std::string logdirname;
158 #if defined(_WIN32)
159  logdirname ="C:/temp/" + username;
160 #else
161  logdirname ="/tmp/" + username;
162 #endif
163  // Test if the output path exist. If no try to create it
164  if (vpIoTools::checkDirectory(logdirname) == false) {
165  try {
166  // Create the dirname
167  vpIoTools::makeDirectory(logdirname);
168  }
169  catch (...) {
170  std::cerr << std::endl
171  << "ERROR:" << std::endl;
172  std::cerr << " Cannot create " << logdirname << std::endl;
173  exit(-1);
174  }
175  }
176  std::string logfilename;
177  logfilename = logdirname + "/log.dat";
178 
179  // Open the log file name
180  std::ofstream flog(logfilename.c_str());
181 
182  vpServo task ;
183  vpSimulatorCamera robot ;
184 
185  std::cout << std::endl ;
186  std::cout << "-------------------------------------------------------" << std::endl ;
187  std::cout << " Test program for vpServo " <<std::endl ;
188  std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
189  std::cout << " Simulation " << std::endl ;
190  std::cout << " task : 3D visual servoing " << std::endl ;
191  std::cout << "-------------------------------------------------------" << std::endl ;
192  std::cout << std::endl ;
193 
194  // Sets the initial camera location
195  vpPoseVector c_r_o(// Translation tx,ty,tz
196  0.1, 0.2, 2,
197  // ThetaU rotation
198  vpMath::rad(20), vpMath::rad(10), vpMath::rad(50) ) ;
199 
200  // From the camera pose build the corresponding homogeneous matrix
201  vpHomogeneousMatrix cMo(c_r_o) ;
202 
203  // Set the robot initial position
204  vpHomogeneousMatrix wMc, wMo;
205  robot.getPosition(wMc) ;
206  wMo = wMc * cMo; // Compute the position of the object in the world frame
207 
208  // Sets the desired camera location
209  vpPoseVector cd_r_o(// Translation tx,ty,tz
210  0, 0, 1,
211  // ThetaU rotation
213 
214  // From the camera desired pose build the corresponding homogeneous matrix
215  vpHomogeneousMatrix cdMo(cd_r_o) ;
216 
217  // Compute the transformation from the initial camera position to the desired one
218  vpHomogeneousMatrix cMcd ;
219  cMcd = cMo*cdMo.inverse() ;
220 
221  // Build the 3D translation feature: ctc*
223  t.buildFrom(cMcd) ;
224 
225  // Build the 3D rotation feature: thetaU_cRc*
226  vpFeatureThetaU tu(vpFeatureThetaU::cRcd); // current feature
227  tu.buildFrom(cMcd) ;
228 
229  // Sets the desired rotation (always zero !) since s is the
230  // rotation that the camera has to achieve. Here s* = (0, 0)^T
232  vpFeatureThetaU tud(vpFeatureThetaU::cRcd); // desired feature
233 
234  // Define the task
235  // - we want an eye-in-hand control law
236  // - the robot is controlled in the camera frame
237  task.setServo(vpServo::EYEINHAND_CAMERA) ;
238  // - we use here the interaction matrix computed with the current
239  // features
240  task.setInteractionMatrixType(vpServo::CURRENT);
241 
242  // Add the current and desired visual features
243  task.addFeature(t,td) ; // 3D translation
244  task.addFeature(tu,tud) ; // 3D rotation theta u
245 
246  // - set the constant gain to 1.0
247  task.setLambda(1) ;
248 
249  // Display task information
250  task.print() ;
251 
252  unsigned int iter=0 ;
253  // Start the visual servoing loop. We stop the servo after 200 iterations
254  while(iter++ < 200) {
255  std::cout << "------------------------------------" << iter <<std::endl ;
256  vpColVector v ;
257 
258  // get the robot position
259  robot.getPosition(wMc) ;
260  // Compute the position of the camera wrt the object frame
261  cMo = wMc.inverse() * wMo;
262 
263  // new displacement to achieve
264  cMcd = cMo*cdMo.inverse() ;
265 
266  // Update the current visual features
267  t.buildFrom(cMcd) ;
268  tu.buildFrom(cMcd) ;
269 
270  // Compute the control law
271  v = task.computeControlLaw() ;
272 
273  // Display task information
274  if (iter==1) task.print() ;
275 
276  // Send the camera velocity to the controller
278 
279  // Retrieve the error
280  std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() <<std::endl ;
281 
282  // Save log
283  flog << v.t() << " " << ( task.getError() ).t() << std::endl;
284  }
285  // Display task information
286  task.print() ;
287 
288  // Kill the task
289  task.kill();
290 
291  // Close the log file
292  flog.close();
293  return 0;
294  }
295  catch(vpException &e) {
296  std::cout << "Catch a ViSP exception: " << e << std::endl;
297  return 1;
298  }
299 }
300 
Class that defines the translation visual feature .
static bool checkDirectory(const char *dirname)
Definition: vpIoTools.cpp:358
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.
error that can be emited by ViSP classes.
Definition: vpException.h:73
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:76
static void makeDirectory(const char *dirname)
Definition: vpIoTools.cpp:427
int print(std::ostream &s, unsigned int length, char const *intro=0) const
vpRowVector t() const
static std::string getUserName()
Definition: vpIoTools.cpp:177
vpHomogeneousMatrix getPosition() const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double rad(double deg)
Definition: vpMath.h:104
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:93
vpHomogeneousMatrix inverse() const
int print(std::ostream &s, unsigned int length, char const *intro=0) const
Class that defines a 3D visual feature from a axis/angle parametrization that represent the rotatio...