88 #include <visp/vpMath.h>
89 #include <visp/vpFeatureThetaU.h>
90 #include <visp/vpFeatureTranslation.h>
91 #include <visp/vpHomogeneousMatrix.h>
92 #include <visp/vpIoTools.h>
93 #include <visp/vpParseArgv.h>
94 #include <visp/vpServo.h>
95 #include <visp/vpSimulatorCamera.h>
98 #define GETOPTARGS "h"
100 void usage(
const char *name,
const char *badparam);
101 bool getOptions(
int argc,
const char **argv);
111 void usage(
const char *name,
const char *badparam)
114 Simulation of a 3D visual servoing:\n\
115 - eye-in-hand control law,\n\
116 - velocity computed in the camera frame,\n\
117 - without display.\n\
129 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
141 bool getOptions(
int argc,
const char **argv)
148 case 'h': usage(argv[0], NULL);
return false;
break;
151 usage(argv[0], optarg_);
156 if ((c == 1) || (c == -1)) {
158 usage(argv[0], NULL);
159 std::cerr <<
"ERROR: " << std::endl;
160 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
168 main(
int argc,
const char ** argv)
172 if (getOptions(argc, argv) ==
false) {
180 std::string username;
185 std::string logdirname;
187 logdirname =
"C:/temp/" + username;
189 logdirname =
"/tmp/" + username;
198 std::cerr << std::endl
199 <<
"ERROR:" << std::endl;
200 std::cerr <<
" Cannot create " << logdirname << std::endl;
204 std::string logfilename;
205 logfilename = logdirname +
"/log.dat";
208 std::ofstream flog(logfilename.c_str());
212 std::cout << std::endl ;
213 std::cout <<
"-------------------------------------------------------" << std::endl ;
214 std::cout <<
" Test program for vpServo " <<std::endl ;
215 std::cout <<
" Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
216 std::cout <<
" Simulation " << std::endl ;
217 std::cout <<
" task : 3D visual servoing " << std::endl ;
218 std::cout <<
"-------------------------------------------------------" << std::endl ;
219 std::cout << std::endl ;
232 robot.getPosition(wMc) ;
250 unsigned int iter=0 ;
252 while(iter++ < 200) {
253 std::cout <<
"------------------------------------" << iter <<std::endl ;
256 robot.getPosition(wMc) ;
279 w = lambda * tu_cRcd;
283 for (
unsigned int i=0; i<3; i++) {
285 velocity[i+3] = w[i];
292 std::cout <<
"|| s - s* || = " << ctcd.
t() <<
" " << tu_cRcd.
t() << std::endl;
295 flog << velocity.
t() <<
" " << ctcd.
t() <<
" " << tu_cRcd.
t() << std::endl;
303 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
Definition of the vpMatrix class.
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
Class that defines the simplest robot: a free flying camera.
error that can be emited by ViSP classes.
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
The vpRotationMatrix considers the particular case of a rotation matrix.
vpColVector t() const
Transpose the vector.
vpRowVector t() const
Transpose of a vector.
void extract(vpRotationMatrix &R) const
static double rad(double deg)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
The pose is a complete representation of every rigid motion in the euclidian space.
vpHomogeneousMatrix inverse() const
static vpMatrix skew(const vpColVector &v)
Class that consider the case of a translation vector.
Class that consider the case of the parameterization for the rotation.