78 #include <visp/vpMath.h>
79 #include <visp/vpHomogeneousMatrix.h>
80 #include <visp/vpFeaturePoint.h>
81 #include <visp/vpPoint.h>
82 #include <visp/vpServo.h>
83 #include <visp/vpRobotCamera.h>
84 #include <visp/vpDebug.h>
85 #include <visp/vpFeatureBuilder.h>
86 #include <visp/vpParseArgv.h>
90 #define GETOPTARGS "h"
100 void usage(
const char *name,
const char *badparam)
103 Simulation of a 2D visual servoing on a point:\n\
104 - eye-in-hand control law,\n\
105 - articular velocity are computed,\n\
106 - without display.\n\
118 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
131 bool getOptions(
int argc,
const char **argv)
138 case 'h': usage(argv[0], NULL);
return false;
break;
141 usage(argv[0], optarg);
146 if ((c == 1) || (c == -1)) {
148 usage(argv[0], NULL);
149 std::cerr <<
"ERROR: " << std::endl;
150 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
158 main(
int argc,
const char ** argv)
161 if (getOptions(argc, argv) ==
false) {
169 std::cout << std::endl ;
170 std::cout <<
"-------------------------------------------------------" << std::endl ;
171 std::cout <<
" Test program for vpServo " <<std::endl ;
172 std::cout <<
" Eye-in-hand task control, articular velocity are computed" << std::endl ;
173 std::cout <<
" Simulation " << std::endl ;
174 std::cout <<
" task : servo a point " << std::endl ;
175 std::cout <<
"-------------------------------------------------------" << std::endl ;
176 std::cout << std::endl ;
179 vpTRACE(
"sets the initial camera location " ) ;
187 vpTRACE(
"sets the point coordinates in the world frame " ) ;
191 vpTRACE(
"project : computes the point coordinates in the camera frame and its 2D coordinates" ) ;
194 vpTRACE(
"sets the current position of the visual feature ") ;
198 vpTRACE(
"sets the desired position of the visual feature ") ;
204 vpTRACE(
"\t we want an eye-in-hand control law") ;
205 vpTRACE(
"\t articular velocity are computed") ;
210 vpTRACE(
"Set the position of the camera in the end-effector frame ") ;
215 vpTRACE(
"Set the Jacobian (expressed in the end-effector frame)") ;
220 vpTRACE(
"\t we want to see a point on a point..") ;
227 vpTRACE(
"Display task information " ) ;
230 unsigned int iter=0 ;
234 std::cout <<
"---------------------------------------------" << iter <<std::endl ;
240 vpTRACE(
"Set the Jacobian (expressed in the end-effector frame)") ;
241 vpTRACE(
"since q is modified eJe is modified") ;
247 if (iter==1)
vpTRACE(
"\t\t get the robot position ") ;
249 if (iter==1)
vpTRACE(
"\t\t new point position ") ;
255 if (iter==1)
vpTRACE(
"\t\t compute the control law ") ;
260 vpTRACE(
"Display task information " ) ;
264 if (iter==1)
vpTRACE(
"\t\t send the camera velocity to the controller ") ;
267 vpTRACE(
"\t\t || s - s* || ") ;
268 std::cout << ( task.
getError() ).sumSquare() <<std::endl ; ;
271 vpTRACE(
"Display task information " ) ;
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)
void set_cVe(vpVelocityTwistMatrix &_cVe)
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 set_eJe(vpMatrix &_eJe)
void getPosition(vpColVector &q)
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
void setPosition(const vpRobot::vpControlFrameType, const vpColVector &)
Set a displacement (frame has to be specified) in position control.
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Set the type of the interaction matrix (current, mean, desired, user).
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 get_eJe(vpMatrix &_eJe)
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...