64 #include <visp/vpMath.h>
65 #include <visp/vpHomogeneousMatrix.h>
66 #include <visp/vpPoint.h>
67 #include <visp/vpFeaturePoint.h>
68 #include <visp/vpFeatureThetaU.h>
69 #include <visp/vpGenericFeature.h>
70 #include <visp/vpServo.h>
71 #include <visp/vpRobotCamera.h>
72 #include <visp/vpDebug.h>
73 #include <visp/vpFeatureBuilder.h>
74 #include <visp/vpParseArgv.h>
78 #define GETOPTARGS "h"
88 void usage(
const char *name,
const char *badparam)
91 Simulation of a 2 1/2 D visual servoing (x,y,logZ, theta U):\n\
92 - eye-in-hand control law,\n\
93 - velocity computed in the camera frame,\n\
106 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
118 bool getOptions(
int argc,
const char **argv)
125 case 'h': usage(argv[0], NULL);
return false;
break;
128 usage(argv[0], optarg);
133 if ((c == 1) || (c == -1)) {
135 usage(argv[0], NULL);
136 std::cerr <<
"ERROR: " << std::endl;
137 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
145 main(
int argc,
const char ** argv)
148 if (getOptions(argc, argv) ==
false) {
152 std::cout << std::endl ;
153 std::cout <<
"-------------------------------------------------------" << std::endl ;
154 std::cout <<
" simulation of a 2 1/2 D visual servoing " << std::endl ;
155 std::cout <<
"-------------------------------------------------------" << std::endl ;
156 std::cout << std::endl ;
168 vpTRACE(
"sets the initial camera location " ) ;
189 vpTRACE(
"sets the desired camera location " ) ;
221 vpTRACE(
"\tsets the point coordinates in the world frame " ) ;
227 vpTRACE(
"\tproject : computes the point coordinates in the camera frame and its 2D coordinates" ) ;
254 vpTRACE(
"2nd feature ThetaUz and 3rd feature t") ;
263 vpTRACE(
"\tcompute the rotation that the camera has to realize " ) ;
269 tuz.buildFrom(cdMc) ;
276 vpTRACE(
"\tsets the desired rotation (always zero !) ") ;
277 vpTRACE(
"\tsince s is the rotation that the camera has to realize ") ;
285 vpTRACE(
"\t we want an eye-in-hand control law") ;
286 vpTRACE(
"\t robot is controlled in the camera frame") ;
307 vpTRACE(
"Display task information " ) ;
312 unsigned int iter=0 ;
316 std::cout <<
"---------------------------------------------" << iter <<std::endl ;
319 if (iter==1)
vpTRACE(
"\t\t get the robot position ") ;
322 if (iter==1)
vpTRACE(
"\t\t update the feature ") ;
332 if (iter==1)
vpTRACE(
"\t\t compute the control law ") ;
336 if (iter==1) task.
print() ;
338 if (iter==1)
vpTRACE(
"\t\t send the camera velocity to the controller ") ;
344 std::cout << ( task.
getError() ).sumSquare() <<std::endl ; ;
347 vpTRACE(
"Display task information " ) ;
350 vpTRACE(
"Final camera location " ) ;
351 std::cout << cMo << std::endl ;
Class that defines the translation visual feature .
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 vpTranslationVector &t, const vpRotationMatrix &R)
Construction from translation vector and rotation matrix.
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Set the type of the interaction matrix (current, mean, desired, user).
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...