68 #include <visp/vpDebug.h>
69 #include <visp/vpConfig.h>
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/vpFeatureBuilder.h>
82 #include <visp/vpParseArgv.h>
85 #define GETOPTARGS "h"
95 void usage(
const char *name,
const char *badparam)
98 Simulation of a 2D visual servoing:\n\
99 - servo on 4 points,\n\
100 - eye-in-hand control law,\n\
101 - articular velocity are computed,\n\
102 - without display.\n\
114 fprintf(stderr,
"ERROR: \n" );
115 fprintf(stderr,
"\nBad parameter [%s]\n", badparam);
129 bool getOptions(
int argc,
const char **argv)
136 case 'h': usage(argv[0], NULL);
return false;
break;
139 usage(argv[0], optarg);
144 if ((c == 1) || (c == -1)) {
146 usage(argv[0], NULL);
147 std::cerr <<
"ERROR: " << std::endl;
148 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
156 main(
int argc,
const char ** argv)
159 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 4 points " << std::endl ;
174 std::cout <<
"-------------------------------------------------------" << std::endl ;
175 std::cout << std::endl ;
178 vpTRACE(
"sets the initial camera location with respect to the object" ) ;
186 vpTRACE(
"sets the point coordinates in the object frame " ) ;
193 vpTRACE(
"project : computes the point coordinates in the camera frame and its 2D coordinates" ) ;
194 for (i = 0 ; i < 4 ; i++)
195 point[i].track(cMo) ;
197 vpTRACE(
"sets the desired position of the point ") ;
199 for (i = 0 ; i < 4 ; i++)
203 vpTRACE(
"sets the desired position of the point ") ;
212 vpTRACE(
"\t we want an eye-in-hand control law") ;
213 vpTRACE(
"\t articular velocity are computed") ;
218 vpTRACE(
"Set the position of the camera in the end-effector frame ") ;
223 vpTRACE(
"Set the Jacobian (expressed in the end-effector frame)") ;
228 vpTRACE(
"\t we want to see a point on a point..") ;
229 for (i = 0 ; i < 4 ; i++)
236 vpTRACE(
"Display task information " ) ;
239 unsigned int iter=0 ;
243 std::cout <<
"---------------------------------------------" << iter <<std::endl ;
249 vpTRACE(
"Set the Jacobian (expressed in the end-effector frame)") ;
250 vpTRACE(
"since q is modified eJe is modified") ;
256 if (iter==1)
vpTRACE(
"\t\t get the robot position ") ;
258 if (iter==1)
vpTRACE(
"\t\t new point position ") ;
259 for (i = 0 ; i < 4 ; i++)
261 point[i].
track(cMo) ;
267 if (iter==1)
vpTRACE(
"\t\t compute the control law ") ;
272 vpTRACE(
"Display task information " ) ;
276 if (iter==1)
vpTRACE(
"\t\t send the camera velocity to the controller ") ;
282 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...