78#include <visp3/core/vpHomogeneousMatrix.h>
79#include <visp3/core/vpIoTools.h>
80#include <visp3/core/vpMath.h>
81#include <visp3/core/vpThetaUVector.h>
82#include <visp3/core/vpTranslationVector.h>
83#include <visp3/io/vpParseArgv.h>
84#include <visp3/robot/vpSimulatorCamera.h>
89void usage(
const char *name,
const char *badparam);
90bool getOptions(
int argc,
const char **argv);
100void usage(
const char *name,
const char *badparam)
103Simulation of a 3D visual servoing:\n\
104- eye-in-hand control law,\n\
105- velocity computed in the camera frame,\n\
118 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
130bool getOptions(
int argc,
const char **argv)
138 usage(argv[0], NULL);
142 usage(argv[0], optarg_);
147 if ((c == 1) || (c == -1)) {
149 usage(argv[0], NULL);
150 std::cerr <<
"ERROR: " << std::endl;
151 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
158int main(
int argc,
const char **argv)
162 if (getOptions(argc, argv) ==
false) {
169 std::string username;
174 std::string logdirname;
176 logdirname =
"C:/temp/" + username;
178 logdirname =
"/tmp/" + username;
187 std::cerr << std::endl <<
"ERROR:" << std::endl;
188 std::cerr <<
" Cannot create " << logdirname << std::endl;
192 std::string logfilename;
193 logfilename = logdirname +
"/log.dat";
196 std::ofstream flog(logfilename.c_str());
200 std::cout << std::endl;
201 std::cout <<
"-------------------------------------------------------" << std::endl;
202 std::cout <<
" Test program without vpServo and vpFeature classes " << std::endl;
203 std::cout <<
" Eye-in-hand task control, velocity computed in the camera frame" << std::endl;
204 std::cout <<
" Simulation " << std::endl;
205 std::cout <<
" task : 3D visual servoing " << std::endl;
206 std::cout <<
"-------------------------------------------------------" << std::endl;
207 std::cout << std::endl;
220 robot.getPosition(wMc);
238 unsigned int iter = 0;
240 while (iter++ < 200) {
241 std::cout <<
"-----------------------------------" << iter << std::endl;
244 robot.getPosition(wMc);
263 v = -lambda * cRcd * cdtc;
266 w = -lambda * tu_cdRc;
270 for (
unsigned int i = 0; i < 3; i++) {
272 velocity[i + 3] = w[i];
279 std::cout <<
"|| s - s* || = " << cdtc.t() <<
" " << tu_cdRc.t() << std::endl;
282 flog << velocity.t() <<
" " << cdtc.t() <<
" " << tu_cdRc.t() << std::endl;
288 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
Implementation of column vector and the associated operations.
error that can be emited 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)
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.