75 #include <visp/vpMath.h>
76 #include <visp/vpHomogeneousMatrix.h>
77 #include <visp/vpFeaturePoint.h>
78 #include <visp/vpServo.h>
79 #include <visp/vpRobotCamera.h>
80 #include <visp/vpDebug.h>
81 #include <visp/vpFeatureBuilder.h>
82 #include <visp/vpParseArgv.h>
88 #define GETOPTARGS "h"
98 void usage(
const char *name,
const char *badparam)
101 Simulation of a 2D visual servoing on a point:\n\
102 - eye-in-hand control law,\n\
103 - articular velocity are computed,\n\
104 - without display,\n\
105 - only the X coordinate of the point is selected.\n\
117 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
130 bool getOptions(
int argc,
const char **argv)
137 case 'h': usage(argv[0], NULL);
return false;
break;
140 usage(argv[0], optarg);
145 if ((c == 1) || (c == -1)) {
147 usage(argv[0], NULL);
148 std::cerr <<
"ERROR: " << std::endl;
149 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
157 main(
int argc,
const char ** argv)
160 if (getOptions(argc, argv) ==
false) {
168 std::cout << std::endl ;
169 std::cout <<
"-------------------------------------------------------" << std::endl ;
170 std::cout <<
" Test program for vpServo " <<std::endl ;
171 std::cout <<
" Eye-in-hand task control, articular velocity are computed" << std::endl ;
172 std::cout <<
" Simulation " << std::endl ;
173 std::cout <<
" task : servo a point " << std::endl ;
174 std::cout <<
"-------------------------------------------------------" << std::endl ;
175 std::cout << std::endl ;
178 vpTRACE(
"sets the initial camera location " ) ;
186 vpTRACE(
"sets the point coordinates in the world frame " ) ;
190 vpTRACE(
"project : computes the point coordinates in the camera frame and its 2D coordinates" ) ;
193 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") ;
209 vpTRACE(
"Set the position of the camera in the end-effector frame ") ;
214 vpTRACE(
"Set the Jacobian (expressed in the end-effector frame)") ;
219 vpTRACE(
"\t we want to see a point on a point..") ;
226 vpTRACE(
"Display task information " ) ;
229 unsigned int iter=0 ;
233 std::cout <<
"---------------------------------------------" << iter <<std::endl ;
239 vpTRACE(
"Set the Jacobian (expressed in the end-effector frame)") ;
240 vpTRACE(
"since q is modified eJe is modified") ;
246 if (iter==1)
vpTRACE(
"\t\t get the robot position ") ;
248 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 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.
static unsigned int selectX()
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...