74 #include <visp3/core/vpHomogeneousMatrix.h>
75 #include <visp3/core/vpIoTools.h>
76 #include <visp3/core/vpMath.h>
77 #include <visp3/core/vpThetaUVector.h>
78 #include <visp3/core/vpTranslationVector.h>
79 #include <visp3/io/vpParseArgv.h>
80 #include <visp3/robot/vpSimulatorCamera.h>
83 #define GETOPTARGS "h"
85 void usage(
const char *name,
const char *badparam);
86 bool getOptions(
int argc,
const char **argv);
96 void usage(
const char *name,
const char *badparam)
99 Simulation of a 3D visual servoing:\n\
100 - eye-in-hand control law,\n\
101 - velocity computed in the camera frame,\n\
102 - without display.\n\
115 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
127 bool getOptions(
int argc,
const char **argv)
135 usage(argv[0],
nullptr);
139 usage(argv[0], optarg_);
144 if ((c == 1) || (c == -1)) {
146 usage(argv[0],
nullptr);
147 std::cerr <<
"ERROR: " << std::endl;
148 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
155 int main(
int argc,
const char **argv)
159 if (getOptions(argc, argv) ==
false) {
166 std::string username;
171 std::string logdirname;
173 logdirname =
"C:/temp/" + username;
175 logdirname =
"/tmp/" + username;
185 std::cerr << std::endl <<
"ERROR:" << std::endl;
186 std::cerr <<
" Cannot create " << logdirname << std::endl;
190 std::string logfilename;
191 logfilename = logdirname +
"/log.dat";
194 std::ofstream flog(logfilename.c_str());
198 std::cout << std::endl;
199 std::cout <<
"-------------------------------------------------------" << std::endl;
200 std::cout <<
" Test program without vpServo and vpFeature classes " << std::endl;
201 std::cout <<
" Eye-in-hand task control, velocity computed in the camera frame" << std::endl;
202 std::cout <<
" Simulation " << std::endl;
203 std::cout <<
" task : 3D visual servoing " << std::endl;
204 std::cout <<
"-------------------------------------------------------" << std::endl;
205 std::cout << std::endl;
218 robot.getPosition(wMc);
236 unsigned int iter = 0;
238 while (iter++ < 200) {
239 std::cout <<
"-----------------------------------" << iter << std::endl;
242 robot.getPosition(wMc);
261 v = -lambda * cRcd * cdtc;
264 w = -lambda * tu_cdRc;
268 for (
unsigned int i = 0; i < 3; i++) {
270 velocity[i + 3] = w[i];
277 std::cout <<
"|| s - s* || = " << cdtc.t() <<
" " << tu_cdRc.t() << std::endl;
280 flog << velocity.t() <<
" " << cdtc.t() <<
" " << tu_cdRc.t() << std::endl;
287 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
void extract(vpRotationMatrix &R) const
static double rad(double deg)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Implementation of a pose vector and operations on poses.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) vp_override
Implementation of a rotation matrix and operations on such kind of matrices.
vpRotationMatrix inverse() 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.