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,log Z, 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 (logZ)") ;
255 vpTRACE(
"\tnot necessary to project twice (reuse p)") ;
272 logZ.set_s(log(point.
get_Z()/pointd.
get_Z())) ;
276 vpTRACE(
"3rd feature ThetaU") ;
285 vpTRACE(
"\tcompute the rotation that the camera has to realize " ) ;
294 vpTRACE(
"\tsets the desired rotation (always zero !) ") ;
295 vpTRACE(
"\tsince s is the rotation that the camera has to realize ") ;
303 vpTRACE(
"\t we want an eye-in-hand control law") ;
304 vpTRACE(
"\t robot is controlled in the camera frame") ;
324 vpTRACE(
"Display task information " ) ;
329 unsigned int iter=0 ;
333 std::cout <<
"---------------------------------------------" << iter <<std::endl ;
336 if (iter==1)
vpTRACE(
"\t\t get the robot position ") ;
339 if (iter==1)
vpTRACE(
"\t\t update the feature ") ;
346 if (iter==1)
vpTRACE(
"\t\t there is no feature for logZ, we explicitely "
347 "build the related interaction matrix") ;
348 logZ.set_s(log(point.
get_Z()/pointd.
get_Z())) ;
350 LlogZ[0][0] = LlogZ[0][1] = LlogZ[0][5] = 0 ;
351 LlogZ[0][2] = -1/p.
get_Z() ;
352 LlogZ[0][3] = -p.
get_y() ;
353 LlogZ[0][4] = p.
get_x() ;
355 logZ.setInteractionMatrix(LlogZ) ;
358 if (iter==1)
vpTRACE(
"\t\t compute the control law ") ;
361 if (iter==1) task.
print() ;
363 if (iter==1)
vpTRACE(
"\t\t send the camera velocity to the controller ") ;
369 std::cout << ( task.
getError() ).sumSquare() <<std::endl ; ;
372 vpTRACE(
"Display task information " ) ;
375 vpTRACE(
"Final camera location " ) ;
376 std::cout << cMo << std::endl ;
Definition of the vpMatrix class.
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)
double get_Z() const
Get the point Z coordinate in the camera frame.
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)
Class that enables to define a feature or a set of features which are not implemented in ViSP as a sp...
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...