#include <visp/vpMath.h>
#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpPoint.h>
#include <visp/vpFeaturePoint.h>
#include <visp/vpFeatureThetaU.h>
#include <visp/vpGenericFeature.h>
#include <visp/vpServo.h>
#include <visp/vpRobotCamera.h>
#include <visp/vpDebug.h>
#include <visp/vpFeatureBuilder.h>
#include <visp/vpParseArgv.h>
#include <stdlib.h>
#include <stdio.h>
#define GETOPTARGS "h"
void usage(const char *name, const char *badparam)
{
fprintf(stdout, "\n\
Simulation of a 2 1/2 D visual servoing (x,y,logZ, theta U):\n\
- eye-in-hand control law,\n\
- velocity computed in the camera frame,\n\
- without display.\n\
\n\
SYNOPSIS\n\
%s [-h]\n", name);
fprintf(stdout, "\n\
OPTIONS: Default\n\
\n\
-h\n\
Print the help.\n");
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
}
bool getOptions(int argc, const char **argv)
{
const char *optarg;
int c;
switch (c) {
case 'h': usage(argv[0], NULL); return false; break;
default:
usage(argv[0], optarg);
return false; break;
}
}
if ((c == 1) || (c == -1)) {
usage(argv[0], NULL);
std::cerr << "ERROR: " << std::endl;
std::cerr << " Bad argument " << optarg << std::endl << std::endl;
return false;
}
return true;
}
int
main(int argc, const char ** argv)
{
if (getOptions(argc, argv) == false) {
exit (-1);
}
std::cout << std::endl ;
std::cout << "-------------------------------------------------------" << std::endl ;
std::cout << " simulation of a 2 1/2 D visual servoing " << std::endl ;
std::cout << "-------------------------------------------------------" << std::endl ;
std::cout << std::endl ;
vpTRACE(
"sets the initial camera location " ) ;
) ;
vpTRACE(
"sets the desired camera location " ) ;
vpTRACE(
"\tsets the point coordinates in the world frame " ) ;
vpTRACE(
"\tproject : computes the point coordinates in the camera frame and its 2D coordinates" ) ;
vpTRACE(
"2nd feature ThetaUz and 3rd feature t") ;
vpTRACE(
"\tcompute the rotation that the camera has to realize " ) ;
tuz.buildFrom(cdMc) ;
t.buildFrom(cdMc) ;
vpTRACE(
"\tsets the desired rotation (always zero !) ") ;
vpTRACE(
"\tsince s is the rotation that the camera has to realize ") ;
vpTRACE(
"\t we want an eye-in-hand control law") ;
vpTRACE(
"\t robot is controlled in the camera frame") ;
vpTRACE(
"Display task information " ) ;
unsigned int iter=0 ;
while(iter++<200)
{
std::cout << "---------------------------------------------" << iter <<std::endl ;
if (iter==1)
vpTRACE(
"\t\t get the robot position ") ;
if (iter==1)
vpTRACE(
"\t\t update the feature ") ;
t.buildFrom(cdMc) ;
if (iter==1)
vpTRACE(
"\t\t compute the control law ") ;
if (iter==1) task.
print() ;
if (iter==1)
vpTRACE(
"\t\t send the camera velocity to the controller ") ;
std::cout << ( task.
getError() ).sumSquare() <<std::endl ; ;
}
vpTRACE(
"Display task information " ) ;
vpTRACE(
"Final camera location " ) ;
std::cout << cMo << std::endl ;
}