59#include <visp3/core/vpHomogeneousMatrix.h>
60#include <visp3/core/vpMath.h>
61#include <visp3/io/vpParseArgv.h>
62#include <visp3/robot/vpSimulatorCamera.h>
63#include <visp3/visual_features/vpFeatureBuilder.h>
64#include <visp3/visual_features/vpFeaturePoint.h>
65#include <visp3/vs/vpServo.h>
70void usage(
const char *name,
const char *badparam);
71bool getOptions(
int argc,
const char **argv);
81void usage(
const char *name,
const char *badparam)
84Simulation of a 2D visual servoing on a point:\n\
85- eye-in-hand control law,\n\
86- articular velocity are computed,\n\
99 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
112bool getOptions(
int argc,
const char **argv)
120 usage(argv[0], NULL);
124 usage(argv[0], optarg_);
129 if ((c == 1) || (c == -1)) {
131 usage(argv[0], NULL);
132 std::cerr <<
"ERROR: " << std::endl;
133 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
140int main(
int argc,
const char **argv)
142#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
145 if (getOptions(argc, argv) ==
false) {
152 std::cout << std::endl;
153 std::cout <<
"-------------------------------------------------------" << std::endl;
154 std::cout <<
" Test program for vpServo " << std::endl;
155 std::cout <<
" Eye-in-hand task control, articular velocity are computed" << std::endl;
156 std::cout <<
" Simulation " << std::endl;
157 std::cout <<
" task : servo a point " << std::endl;
158 std::cout <<
"-------------------------------------------------------" << std::endl;
159 std::cout << std::endl;
168 robot.getPosition(wMc);
210 unsigned int iter = 0;
212 while (iter++ < 100) {
213 std::cout <<
"---------------------------------------------" << iter << std::endl;
222 robot.getPosition(wMc);
239 std::cout <<
"|| s - s* || = " << (task.
getError()).sumSquare() << std::endl;
246 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
252 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
Implementation of column vector and the associated operations.
error that can be emited by ViSP classes.
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void buildFrom(double x, double y, double Z)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Implementation of a matrix and operations on matrices.
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void get_eJe(vpMatrix &eJe)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
void set_cVe(const vpVelocityTwistMatrix &cVe_)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
void set_eJe(const vpMatrix &eJe_)
void setServo(const vpServoType &servo_type)
vpColVector getError() const
vpColVector computeControlLaw()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Class that defines the simplest robot: a free flying camera.