#include <visp/vpMath.h>
#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpFeatureThetaU.h>
#include <visp/vpFeatureTranslation.h>
#include <visp/vpServo.h>
#include <visp/vpRobotCamera.h>
#include <visp/vpDebug.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 avisual servoing using theta U visual feature:\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 << " Test program for vpServo " <<std::endl ;
std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
std::cout << " Simulation " << std::endl ;
std::cout << " task : servo using theta U visual feature " << std::endl ;
std::cout << "-------------------------------------------------------" << std::endl ;
std::cout << std::endl ;
vpTRACE(
"sets the initial camera location " ) ;
) ;
vpTRACE(
"sets the desired camera location " ) ;
vpTRACE(
"compute the rotation that the camera has to realize " ) ;
tu.buildFrom(cdMc) ;
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 new rotation to realize ") ;
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 " ) ;
}