65 #include <visp/vpMath.h>
66 #include <visp/vpHomogeneousMatrix.h>
67 #include <visp/vpFeaturePoint3D.h>
68 #include <visp/vpPoint.h>
69 #include <visp/vpServo.h>
70 #include <visp/vpRobotCamera.h>
71 #include <visp/vpDebug.h>
72 #include <visp/vpParseArgv.h>
76 #define GETOPTARGS "h"
86 void usage(
const char *name,
const char *badparam)
89 Simulation of a 3D visual servoing:\n\
90 - servo a 3D point,\n\
91 - eye-in-hand control law,\n\
92 - velocity computed in the camera frame,\n\
105 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
117 bool getOptions(
int argc,
const char **argv)
124 case 'h': usage(argv[0], NULL);
return false;
break;
127 usage(argv[0], optarg);
132 if ((c == 1) || (c == -1)) {
134 usage(argv[0], NULL);
135 std::cerr <<
"ERROR: " << std::endl;
136 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
144 main(
int argc,
const char ** argv)
147 if (getOptions(argc, argv) ==
false) {
154 std::cout << std::endl ;
155 std::cout <<
"-------------------------------------------------------" << std::endl ;
156 std::cout <<
" Test program for vpServo " <<std::endl ;
157 std::cout <<
" Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
158 std::cout <<
" Simulation " << std::endl ;
159 std::cout <<
" task : servo a 3D point " << std::endl ;
160 std::cout <<
"-------------------------------------------------------" << std::endl ;
161 std::cout << std::endl ;
164 vpTRACE(
"sets the initial camera location " ) ;
172 vpTRACE(
"sets the point coordinates in the world frame " ) ;
177 vpTRACE(
"project : computes the point coordinates in the camera frame " ) ;
180 std::cout << point.
cP.
t() ;
185 vpTRACE(
"sets the desired position of the point ") ;
192 vpTRACE(
"\t we want an eye-in-hand control law") ;
193 vpTRACE(
"\t robot is controlled in the camera frame") ;
196 vpTRACE(
"\t we want to see a point on a point..") ;
197 std::cout << std::endl ;
204 vpTRACE(
"Display task information " ) ;
207 unsigned int iter=0 ;
211 std::cout <<
"---------------------------------------------" << iter <<std::endl ;
214 if (iter==1)
vpTRACE(
"\t\t get the robot position ") ;
216 if (iter==1)
vpTRACE(
"\t\t new point position ") ;
223 if (iter==1)
vpTRACE(
"\t\t compute the control law ") ;
227 if (iter==1)
vpTRACE(
"\t\t send the camera velocity to the controller ") ;
231 std::cout << ( task.
getError() ).sumSquare() <<std::endl ; ;
234 vpTRACE(
"Display task information " ) ;
void set_XYZ(const double X, const double Y, const double Z)
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)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void buildFrom(const vpPoint &p)
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.
Class that defines the 3D point visual feature.
vpRowVector t() const
transpose of Vector
void getPosition(vpColVector &q)
void setPosition(const vpRobot::vpControlFrameType, const vpColVector &)
Set a displacement (frame has to be specified) in position control.
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)
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...