Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
servoSimu3D_cdMc_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  *****************************************************************************/
58 #include <stdlib.h>
59 #include <stdio.h>
60 
61 #include <visp3/visual_features/vpFeatureThetaU.h>
62 #include <visp3/visual_features/vpFeatureTranslation.h>
63 #include <visp3/core/vpHomogeneousMatrix.h>
64 #include <visp3/core/vpIoTools.h>
65 #include <visp3/core/vpMath.h>
66 #include <visp3/io/vpParseArgv.h>
67 #include <visp3/vs/vpServo.h>
68 #include <visp3/robot/vpSimulatorCamera.h>
69 
70 // List of allowed command line options
71 #define GETOPTARGS "h"
72 
73 void usage(const char *name, const char *badparam);
74 bool getOptions(int argc, const char **argv);
75 
84 void usage(const char *name, const char *badparam)
85 {
86  fprintf(stdout, "\n\
87 Simulation of a 3D visual servoing:\n\
88  - eye-in-hand control law,\n\
89  - velocity computed in the camera frame,\n\
90  - without display.\n\
91  \n\
92 SYNOPSIS\n\
93  %s [-h]\n", name);
94 
95  fprintf(stdout, "\n\
96 OPTIONS: Default\n\
97  \n\
98  -h\n\
99  Print the help.\n");
100 
101  if (badparam)
102  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
103 }
104 
114 bool getOptions(int argc, const char **argv)
115 {
116  const char *optarg_;
117  int c;
118  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
119 
120  switch (c) {
121  case 'h': usage(argv[0], NULL); return false; break;
122 
123  default:
124  usage(argv[0], optarg_);
125  return false; break;
126  }
127  }
128 
129  if ((c == 1) || (c == -1)) {
130  // standalone param or error
131  usage(argv[0], NULL);
132  std::cerr << "ERROR: " << std::endl;
133  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
134  return false;
135  }
136 
137  return true;
138 }
139 
140 int
141 main(int argc, const char ** argv)
142 {
143  try {
144  // Read the command line options
145  if (getOptions(argc, argv) == false) {
146  exit (-1);
147  }
148 
149  // Log file creation in /tmp/$USERNAME/log.dat
150  // This file contains by line:
151  // - the 6 computed camera velocities (m/s, rad/s) to achieve the task
152  // - the 6 values of s - s*
153  std::string username;
154  // Get the user login name
155  vpIoTools::getUserName(username);
156 
157  // Create a log filename to save velocities...
158  std::string logdirname;
159 #if defined(_WIN32)
160  logdirname ="C:/temp/" + username;
161 #else
162  logdirname ="/tmp/" + username;
163 #endif
164  // Test if the output path exist. If no try to create it
165  if (vpIoTools::checkDirectory(logdirname) == false) {
166  try {
167  // Create the dirname
168  vpIoTools::makeDirectory(logdirname);
169  }
170  catch (...) {
171  std::cerr << std::endl
172  << "ERROR:" << std::endl;
173  std::cerr << " Cannot create " << logdirname << std::endl;
174  exit(-1);
175  }
176  }
177  std::string logfilename;
178  logfilename = logdirname + "/log.dat";
179 
180  // Open the log file name
181  std::ofstream flog(logfilename.c_str());
182 
183  vpServo task ;
184  vpSimulatorCamera robot ;
185 
186  std::cout << std::endl ;
187  std::cout << "-------------------------------------------------------" << std::endl ;
188  std::cout << " Test program for vpServo " <<std::endl ;
189  std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
190  std::cout << " Simulation " << std::endl ;
191  std::cout << " task : 3D visual servoing " << std::endl ;
192  std::cout << "-------------------------------------------------------" << std::endl ;
193  std::cout << std::endl ;
194 
195 
196  // Sets the initial camera location
197  vpPoseVector c_r_o(// Translation tx,ty,tz
198  0.1, 0.2, 2,
199  // ThetaU rotation
200  vpMath::rad(20), vpMath::rad(10), vpMath::rad(50) ) ;
201 
202  // From the camera pose build the corresponding homogeneous matrix
203  vpHomogeneousMatrix cMo(c_r_o) ;
204 
205  // Set the robot initial position
206  vpHomogeneousMatrix wMc, wMo;
207  robot.getPosition(wMc) ;
208  wMo = wMc * cMo; // Compute the position of the object in the world frame
209 
210  // Sets the desired camera location
211  vpPoseVector cd_r_o(// Translation tx,ty,tz
212  0, 0, 1,
213  // ThetaU rotation
215  // From the camera desired pose build the corresponding homogeneous matrix
216  vpHomogeneousMatrix cdMo(cd_r_o) ;
217 
218  // Compute the homogeneous transformation from the desired camera position to the initial one
219  vpHomogeneousMatrix cdMc ;
220  cdMc = cdMo*cMo.inverse() ;
221 
222  // Build the current visual features s = (c*tc, thetaU_c*Rc)^T
224  vpFeatureThetaU tu(vpFeatureThetaU::cdRc); // current feature
225  t.buildFrom(cdMc) ;
226  tu.buildFrom(cdMc) ;
227 
228  // Sets the desired rotation (always zero !) since s is the
229  // rotation that the camera has to achieve. Here s* = (0, 0)^T
231  vpFeatureThetaU tud(vpFeatureThetaU::cdRc); // desired feature
232 
233  // Define the task
234  // - we want an eye-in-hand control law
235  // - the robot is controlled in the camera frame
236  task.setServo(vpServo::EYEINHAND_CAMERA) ;
237  // - we use here the interaction matrix computed with the
238  // current features
239  task.setInteractionMatrixType(vpServo::CURRENT);
240 
241  // Add the current and desired visual features
242  task.addFeature(t,td) ; // 3D translation
243  task.addFeature(tu,tud) ; // 3D rotation
244 
245  // - set the constant gain to 1.0
246  task.setLambda(1) ;
247 
248  // Display task information
249  task.print() ;
250 
251  unsigned int iter=0 ;
252  // Start the visual servoing loop. We stop the servo after 200 iterations
253  while(iter++ < 200) {
254  std::cout << "-----------------------------------" << iter <<std::endl ;
255  vpColVector v ;
256 
257  // get the robot position
258  robot.getPosition(wMc) ;
259  // Compute the position of the camera wrt the object frame
260  cMo = wMc.inverse() * wMo;
261 
262  // new displacement to achieve
263  cdMc = cdMo*cMo.inverse() ;
264 
265  // Update the current visual features
266  t.buildFrom(cdMc) ;
267  tu.buildFrom(cdMc) ;
268 
269  // Compute the control law
270  v = task.computeControlLaw() ;
271 
272  // Display task information
273  if (iter==1) task.print() ;
274 
275  // Send the camera velocity to the controller
277 
278  // Retrieve the error
279  std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() <<std::endl ;
280 
281  // Save log
282  flog << v.t() << " " << ( task.getError() ).t() << std::endl;
283  }
284  // Display task information
285  task.print() ;
286 
287  // Kill the task
288  task.kill();
289 
290  // Close the log file
291  flog.close();
292  return 0;
293  }
294  catch(vpException &e) {
295  std::cout << "Catch a ViSP exception: " << e << std::endl;
296  return 1;
297  }
298 }
299 
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...