61 #include <visp/vpMath.h>
62 #include <visp/vpHomogeneousMatrix.h>
63 #include <visp/vpFeaturePoint.h>
64 #include <visp/vpPoint.h>
65 #include <visp/vpServo.h>
66 #include <visp/vpRobotCamera.h>
67 #include <visp/vpFeatureBuilder.h>
71 #include <visp/vpException.h>
72 #include <visp/vpMatrixException.h>
75 #include <visp/vpDebug.h>
76 #include <visp/vpParseArgv.h>
81 #define GETOPTARGS "h"
91 void usage(
const char *name,
const char *badparam)
94 Simulation of a 2D visual servoing on a point:\n\
95 - eye-in-hand control law,\n\
96 - velocity computed in the camera frame,\n\
109 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
122 bool getOptions(
int argc,
const char **argv)
129 case 'h': usage(argv[0], NULL);
return false;
break;
132 usage(argv[0], optarg);
137 if ((c == 1) || (c == -1)) {
139 usage(argv[0], NULL);
140 std::cerr <<
"ERROR: " << std::endl;
141 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
149 main(
int argc,
const char ** argv)
152 if (getOptions(argc, argv) ==
false) {
159 vpTRACE(
"sets the initial camera location " ) ;
167 vpTRACE(
"sets the point coordinates in the world frame " ) ;
171 vpTRACE(
"project : computes the point coordinates in the camera frame and its 2D coordinates" ) ;
174 vpTRACE(
"sets the current position of the visual feature ") ;
178 vpTRACE(
"sets the desired position of the visual feature ") ;
183 vpTRACE(
"\t we want an eye-in-hand control law") ;
184 vpTRACE(
"\t robot is controlled in the camera frame") ;
187 vpTRACE(
"\t we want to see a point on a point..") ;
188 std::cout << std::endl ;
195 vpTRACE(
"Display task information " ) ;
198 unsigned int iter=0 ;
202 std::cout <<
"---------------------------------------------" << iter <<std::endl ;
205 if (iter==1)
vpTRACE(
"\t\t get the robot position ") ;
207 if (iter==1)
vpTRACE(
"\t\t new point position ") ;
213 if (iter==1)
vpTRACE(
"\t\t compute the control law ") ;
217 if (iter==1)
vpTRACE(
"\t\t send the camera velocity to the controller ") ;
220 vpTRACE(
"\t\t || s - s* || ") ;
221 std::cout << ( task.
getError() ).sumSquare() <<std::endl ; ;
224 vpTRACE(
"Display task information " ) ;
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
create a new ste of two visual features
void setLambda(double _lambda)
set the gain lambda
void track(const vpHomogeneousMatrix &cMo)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Class that defines what is a point.
void kill()
destruction (memory deallocation if required)
vpColVector getError() const
vpColVector computeControlLaw()
compute the desired control law
Class that defines the simplest robot: a free flying camera.
void getPosition(vpColVector &q)
void setPosition(const vpRobot::vpControlFrameType, const vpColVector &)
Set a displacement (frame has to be specified) in position control.
void buildFrom(const double x, const double y, const double Z)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class required to compute the visual servoing control law.
void setServo(vpServoType _servo_type)
Choice of the visual servoing control law.
void setWorldCoordinates(const double ox, const double oy, const double oz)
Set the point world coordinates. We mean here the coordinates of the point in the object frame...