83 #include <visp/vpHomogeneousMatrix.h>
84 #include <visp/vpIoTools.h>
85 #include <visp/vpMath.h>
86 #include <visp/vpParseArgv.h>
87 #include <visp/vpSimulatorCamera.h>
88 #include <visp/vpThetaUVector.h>
89 #include <visp/vpTranslationVector.h>
92 #define GETOPTARGS "h"
94 void usage(
const char *name,
const char *badparam);
95 bool getOptions(
int argc,
const char **argv);
105 void usage(
const char *name,
const char *badparam)
108 Simulation of a 3D visual servoing:\n\
109 - eye-in-hand control law,\n\
110 - velocity computed in the camera frame,\n\
111 - without display.\n\
123 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
135 bool getOptions(
int argc,
const char **argv)
142 case 'h': usage(argv[0], NULL);
return false;
break;
145 usage(argv[0], optarg_);
150 if ((c == 1) || (c == -1)) {
152 usage(argv[0], NULL);
153 std::cerr <<
"ERROR: " << std::endl;
154 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
162 main(
int argc,
const char ** argv)
166 if (getOptions(argc, argv) ==
false) {
173 std::string username;
178 std::string logdirname;
180 logdirname =
"C:/temp/" + username;
182 logdirname =
"/tmp/" + username;
192 std::cerr << std::endl
193 <<
"ERROR:" << std::endl;
194 std::cerr <<
" Cannot create " << logdirname << std::endl;
198 std::string logfilename;
199 logfilename = logdirname +
"/log.dat";
202 std::ofstream flog(logfilename.c_str());
206 std::cout << std::endl ;
207 std::cout <<
"-------------------------------------------------------" << std::endl ;
208 std::cout <<
" Test program without vpServo and vpFeature classes " <<std::endl ;
209 std::cout <<
" Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
210 std::cout <<
" Simulation " << std::endl ;
211 std::cout <<
" task : 3D visual servoing " << std::endl ;
212 std::cout <<
"-------------------------------------------------------" << std::endl ;
213 std::cout << std::endl ;
227 robot.getPosition(wMc) ;
245 unsigned int iter=0 ;
247 while(iter++ < 200) {
248 std::cout <<
"-----------------------------------" << iter <<std::endl ;
251 robot.getPosition(wMc) ;
270 v = -lambda * cRcd * cdtc;
273 w = -lambda * tu_cdRc;
277 for (
unsigned int i=0; i<3; i++) {
279 velocity[i+3] = w[i];
286 std::cout <<
"|| s - s* || = " << cdtc.t() <<
" " << tu_cdRc.t() << std::endl;
289 flog << velocity.t() <<
" " << cdtc.t() <<
" " << tu_cdRc.t() << std::endl;
296 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
vpRotationMatrix inverse() const
invert the rotation matrix
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.
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
Class that consider the case of a translation vector.
Class that consider the case of the parameterization for the rotation.