66 #include <visp/vpMath.h>
67 #include <visp/vpHomogeneousMatrix.h>
68 #include <visp/vpPoint.h>
69 #include <visp/vpFeaturePoint.h>
70 #include <visp/vpFeatureThetaU.h>
71 #include <visp/vpFeaturePoint3D.h>
72 #include <visp/vpServo.h>
73 #include <visp/vpRobotCamera.h>
74 #include <visp/vpDebug.h>
75 #include <visp/vpFeatureBuilder.h>
76 #include <visp/vpParseArgv.h>
80 #define GETOPTARGS "h"
90 void usage(
const char *name,
const char *badparam)
93 Simulation of a 2 1/2 D visual servoing (x,y,Z,theta U):\n\
94 - eye-in-hand control law,\n\
95 - velocity computed in the camera frame,\n\
108 fprintf(stderr,
"ERROR: \n" );
109 fprintf(stderr,
"\nBad parameter [%s]\n", badparam);
123 bool getOptions(
int argc,
const char **argv)
130 case 'h': usage(argv[0], NULL);
return false;
break;
133 usage(argv[0], optarg);
138 if ((c == 1) || (c == -1)) {
140 usage(argv[0], NULL);
141 std::cerr <<
"ERROR: " << std::endl;
142 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
150 main(
int argc,
const char ** argv)
153 if (getOptions(argc, argv) ==
false) {
160 std::cout << std::endl ;
161 std::cout <<
"-------------------------------------------------------" << std::endl ;
162 std::cout <<
" Test program for vpServo " <<std::endl ;
163 std::cout <<
" task : 2 1/2 D visual servoing " << std::endl ;
164 std::cout <<
"-------------------------------------------------------" << std::endl ;
165 std::cout << std::endl ;
168 vpTRACE(
"sets the initial camera location " ) ;
179 vpTRACE(
"sets the desired camera location " ) ;
185 vpTRACE(
"\tsets the point coordinates in the world frame " ) ;
188 vpTRACE(
"\tproject : computes the point coordinates in the camera frame and its 2D coordinates" ) ;
198 vpTRACE(
"\t want to it at (0,0)") ;
209 vpTRACE(
"\tnot necessary to project twice (reuse p)") ;
213 vpTRACE(
"\twant to see it one meter away (here again use pd)") ;
219 vpTRACE(
"3rd feature ThetaU") ;
220 vpTRACE(
"\tcompute the rotation that the camera has to realize " ) ;
227 vpTRACE(
"\tsets the desired rotation (always zero !) ") ;
228 vpTRACE(
"\tsince s is the rotation that the camera has to realize ") ;
234 vpTRACE(
"\t we want an eye-in-hand control law") ;
235 vpTRACE(
"\t robot is controlled in the camera frame") ;
246 vpTRACE(
"Display task information " ) ;
249 unsigned int iter=0 ;
253 std::cout <<
"---------------------------------------------" << iter <<std::endl ;
256 if (iter==1)
vpTRACE(
"\t\t get the robot position ") ;
259 if (iter==1)
vpTRACE(
"\t\t update the feature ") ;
268 if (iter==1)
vpTRACE(
"\t\t compute the control law ") ;
270 if (iter==1) task.
print() ;
272 if (iter==1)
vpTRACE(
"\t\t send the camera velocity to the controller ") ;
276 std::cout << ( task.
getError() ).sumSquare() <<std::endl ; ;
279 vpTRACE(
"Display task information " ) ;
282 vpTRACE(
"Final camera location " ) ;
283 std::cout << cMo << std::endl ;
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.
Class that defines the 3D point visual feature.
static unsigned int selectZ()
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 vpTranslationVector &t, const vpRotationMatrix &R)
Construction from translation vector and rotation matrix.
static double rad(double deg)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
The pose is a complete representation of every rigid motion in the euclidian space.
vpHomogeneousMatrix inverse() const
Class that defines a 3D visual feature from a axis/angle parametrization that represent the rotatio...
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...