Interaction matrix is computed as the mean of the current and desired interaction matrix.
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
#if defined(VISP_HAVE_THREADS) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GDI)) \
&& (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
#include <stdio.h>
#include <stdlib.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImagePoint.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpParseArgv.h>
#include <visp3/robot/vpSimulatorAfma6.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
#define GETOPTARGS "cdh"
#ifdef ENABLE_VISP_NAMESPACE
#endif
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
void usage(const char *name, const char *badparam)
{
fprintf(stdout, "\n\
Tests a control law with the following characteristics:\n\
- eye-in-hand control\n\
- articular velocity are computed\n\
- servo on 4 points,\n\
- internal and external camera view displays.\n\
\n\
SYNOPSIS\n\
%s [-c] [-d] [-h]\n",
name);
fprintf(stdout, "\n\
OPTIONS: Default\n\
-c\n\
Disable the mouse click. Useful to automate the \n\
execution of this program without human intervention.\n\
\n\
-d \n\
Turn off the display.\n\
\n\
-h\n\
Print the help.\n");
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
}
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
{
const char *optarg_;
int c;
switch (c) {
case 'c':
click_allowed = false;
break;
case 'd':
display = false;
break;
case 'h':
usage(argv[0], nullptr);
return false;
break;
default:
usage(argv[0], optarg_);
return false;
break;
}
}
if ((c == 1) || (c == -1)) {
usage(argv[0], nullptr);
std::cerr << "ERROR: " << std::endl;
std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
return false;
}
return true;
}
int main(int argc, const char **argv)
{
try {
bool opt_click_allowed = true;
bool opt_display = true;
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
return EXIT_FAILURE;
}
#if defined(VISP_HAVE_X11)
vpDisplayX displayInt;
#elif defined(VISP_HAVE_GDI)
#elif defined(HAVE_OPENCV_HIGHGUI)
#endif
if (opt_display) {
displayInt.init(Iint, 700, 0, "Internal view");
}
std::cout << std::endl;
std::cout << "----------------------------------------------" << std::endl;
std::cout << " Test program for vpServo " << std::endl;
std::cout << " Eye-in-hand task control, articular velocity are computed" << std::endl;
std::cout << " Simulation " << std::endl;
std::cout << " task : servo 4 points " << std::endl;
std::cout << "----------------------------------------------" << std::endl;
std::cout << std::endl;
for (unsigned int i = 0; i < 4; i++)
point[i].track(cMo);
for (unsigned int i = 0; i < 4; i++)
for (unsigned int i = 0; i < 4; i++)
point[i].track(cdMo);
for (unsigned int i = 0; i < 4; i++)
for (unsigned int i = 0; i < 4; i++)
vpSimulatorAfma6 robot(opt_display);
robot.initialiseObjectRelativeToCamera(cMo);
robot.setDesiredCameraPosition(cdMo);
robot.getCameraParameters(cam, Iint);
if (opt_display) {
robot.getInternalView(Iint);
}
unsigned int iter = 0;
vpTRACE("\t loop");
while (iter++ < 500) {
std::cout << "---------------------------------------------" << iter << std::endl;
cMo = robot.get_cMo();
if (iter == 1) {
std::cout << "Initial robot position with respect to the object frame:\n";
cMo.print();
}
for (unsigned int i = 0; i < 4; i++) {
}
if (opt_display) {
robot.getInternalView(Iint);
}
if (opt_display && opt_click_allowed && iter == 1) {
std::cout << "Click in the internal view window to continue..." << std::endl;
}
std::cout <<
"|| s - s* || " << (task.
getError()).sumSquare() << std::endl;
}
std::cout << "Final robot position with respect to the object frame:\n";
cMo.print();
if (opt_display && opt_click_allowed) {
std::cout << "Click in the internal view window to end..." << std::endl;
}
return EXIT_SUCCESS;
}
std::cout << "Catch a ViSP exception: " << e << std::endl;
return EXIT_FAILURE;
}
return EXIT_SUCCESS;
}
#elif !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
int main()
{
std::cout << "You do not have X11, or GDI (Graphical Device Interface) of OpenCV functionalities to display images..."
<< std::endl;
std::cout << "Tip if you are on a unix-like system:" << std::endl;
std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
std::cout << "Tip if you are on a windows-like system:" << std::endl;
std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
return EXIT_SUCCESS;
}
#elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
int main()
{
std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
return EXIT_SUCCESS;
}
#else
int main()
{
std::cout << "You do not have threading capabilities" << std::endl;
std::cout << "Tip:" << std::endl;
std::cout << "- Install pthread, configure again ViSP using cmake and build again this example" << std::endl;
return EXIT_SUCCESS;
}
#endif
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
Implementation of column vector and the associated operations.
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
error that can be emitted by ViSP classes.
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpImagePoint &t)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
static double rad(double deg)
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 setWorldCoordinates(double oX, double oY, double oZ)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) VP_OVERRIDE
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
void setServo(const vpServoType &servo_type)
vpColVector getError() const
vpColVector computeControlLaw()
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()