Example of eye-in-hand control law. We control here a real robot, the Afma4 robot (cylindrical robot, with 4 degrees of freedom). The velocity is computed in the camera frame. The visual feature is the center of gravity of a point.In this example we estimate the velocity of the target in order to reduce the tracking error when the target is moving. The velocity of the target is filtered by a Kalman filter with a constant velocity state model, or a constant acceleration state model.
#include <stdlib.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
#if (defined(VISP_HAVE_AFMA4) && defined(VISP_HAVE_DC1394))
#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpImage.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
#include <visp3/blob/vpDot2.h>
#include <visp3/core/vpException.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpPoint.h>
#include <visp3/io/vpParseArgv.h>
#include <visp3/robot/vpRobotAfma4.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpAdaptiveGain.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
#define GETOPTARGS "hK:l:"
#ifdef ENABLE_VISP_NAMESPACE
#endif
typedef enum { K_NONE, K_VELOCITY, K_ACCELERATION } KalmanType;
void usage(const char *name, const char *badparam, KalmanType &kalman)
{
fprintf(stdout, "\n\
Tests a control law with the following characteristics:\n\
- eye-in-hand control\n\
- camera velocity are computed\n\
- servo on 1 points.\n\
- Kalman filtering\n\
\n\
SYNOPSIS\n\
%s [-K <0|1|2|3>] [-h]\n",
name);
fprintf(stdout, "\n\
OPTIONS: Default\n\
-l <%%f> \n\
Set the constant gain. By default adaptive gain. \n\
\n\
-K <0|1|2> %d\n\
Kalman filtering:\n\
0: none\n\
1: velocity model\n\
2: acceleration model\n\
\n\
-h\n\
Print the help.\n",
(int)kalman);
if (badparam) {
fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
}
bool getOptions(int argc, const char **argv, KalmanType &kalman, bool &doAdaptativeGain,
{
const char *optarg;
int c;
switch (c) {
case 'K':
kalman = (KalmanType)atoi(optarg);
break;
case 'l':
doAdaptativeGain = false;
break;
case 'h':
usage(argv[0], nullptr, kalman);
return false;
break;
default:
usage(argv[0], optarg, kalman);
return false;
break;
}
}
if ((c == 1) || (c == -1)) {
usage(argv[0], nullptr, kalman);
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 {
KalmanType opt_kalman = K_NONE;
bool doAdaptativeGain = true;
int opt_cam_frequency = 60;
if (getOptions(argc, argv, opt_kalman, doAdaptativeGain, lambda) == false) {
return EXIT_FAILURE;
}
std::string username;
std::string logdirname;
logdirname = "/tmp/" + username;
try {
}
catch (...) {
std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
return EXIT_FAILURE;
}
}
std::string logfilename;
logfilename = logdirname + "/log.dat";
std::ofstream flog(logfilename.c_str());
switch (opt_cam_frequency) {
case 15:
break;
case 30:
break;
case 60:
break;
}
g.open(I);
for (int i = 0; i < 10; i++)
g.acquire(I);
#ifdef VISP_HAVE_X11
vpDisplayX display(I, 100, 100, "Current image");
#elif defined(HAVE_OPENCV_HIGHGUI)
#elif defined(VISP_HAVE_GTK)
#endif
std::cout << std::endl;
std::cout << "-------------------------------------------------------" << std::endl;
std::cout << "Test program for target motion compensation using a Kalman "
"filter "
<< std::endl;
std::cout << "Eye-in-hand task control, velocity computed in the camera frame" << std::endl;
std::cout << "Task : servo a point \n" << std::endl;
switch (opt_kalman) {
case K_NONE:
std::cout << "Servo with no target motion compensation (see -K option)\n";
break;
case K_VELOCITY:
std::cout << "Servo with target motion compensation using a Kalman filter\n"
<< "with constant velocity modelization (see -K option)\n";
break;
case K_ACCELERATION:
std::cout << "Servo with target motion compensation using a Kalman filter\n"
<< "with constant acceleration modelization (see -K option)\n";
break;
}
std::cout << "-------------------------------------------------------" << std::endl;
std::cout << std::endl;
std::cout << "Click on the dot..." << std::endl;
double px = 1000;
double py = 1000;
std::cout << std::endl;
unsigned int nsignal = 2;
double rho = 0.3;
unsigned int state_size = 0;
switch (opt_kalman) {
case K_VELOCITY: {
sigma_state.
resize(state_size * nsignal);
sigma_state = 0.00001;
sigma_measure = 0.05;
double dummy = 0;
kalman.
initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
break;
}
case K_ACCELERATION: {
sigma_state.
resize(state_size * nsignal);
sigma_state = 0.00001;
sigma_measure = 0.05;
double dt = 1. / opt_cam_frequency;
kalman.
initFilter(nsignal, sigma_state, sigma_measure, rho, dt);
break;
}
default:
break;
}
int iter = 0;
double t_1, Tv_0;
Tv_0 = 0;
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
for (;;) {
double Tv = (double)(t_0 - t_1) / 1000.0;
t_1 = t_0;
g.acquire(I);
vm_0 = vm;
double Tv_1 = Tv_0;
Tv_0 = Tv;
if (iter == 0) {
err_0 = 0;
err_1 = 0;
dedt_mes = 0;
dedt_filt = 0;
}
else {
err_1 = err_0;
err_0 = err;
dedt_mes = (err_0 - err_1) / (Tv_1)-task.
J1 * vm_0;
}
if (iter <= 1) {
dedt_mes = 0;
}
switch (opt_kalman) {
case K_NONE:
dedt_filt = 0;
break;
case K_VELOCITY:
case K_ACCELERATION:
for (unsigned int i = 0; i < nsignal; i++) {
dedt_filt[i] = kalman.
Xest[i * state_size];
}
break;
}
v2 = -J1p * dedt_filt;
v = v1 + v2;
flog << Tv_0 << " ";
flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " ";
flog << task.
error[0] <<
" " << task.
error[1] <<
" ";
flog << cog.
get_u() - cam.get_u0() <<
" " << cog.
get_v() - cam.get_v0() <<
" ";
flog << dedt_mes[0] << " " << dedt_mes[1] << " ";
flog << dedt_filt[0] << " " << dedt_filt[1] << " ";
flog << std::endl;
iter++;
}
flog.close();
return EXIT_SUCCESS;
}
std::cout << "Catch a ViSP exception: " << e << std::endl;
return EXIT_FAILURE;
}
}
#else
int main()
{
std::cout << "You do not have an afma4 robot connected to your computer..." << std::endl;
return EXIT_SUCCESS;
}
#endif
Class for firewire ieee1394 video devices using libdc1394-2.x api.
@ vpVIDEO_MODE_640x480_MONO8
Adaptive gain computation.
void initStandard(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
void initFromConstant(double c)
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
static const vpColor blue
static const vpColor green
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
static void display(const vpImage< unsigned char > &I)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void flush(const vpImage< unsigned char > &I)
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
void track(const vpImage< unsigned char > &I, bool canMakeTheWindowGrow=true)
void setGraphics(bool activate)
vpImagePoint getCog() const
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
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...
vpFeaturePoint & buildFrom(const double &x, const double &y, const double &Z)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
unsigned int getWidth() const
unsigned int getHeight() const
unsigned int getStateSize()
This class provides an implementation of some specific linear Kalman filters.
void filter(vpColVector &z)
void setStateModel(vpStateModel model)
void initFilter(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho, double dt)
@ stateConstVelWithColoredNoise_MeasureVel
@ stateConstAccWithColoredNoise_MeasureVel
Implementation of a matrix and operations on matrices.
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Control of Irisa's cylindrical robot named Afma4.
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
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)
static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
vpMatrix J1
Task Jacobian .
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 computeControlLaw()
vpMatrix getTaskJacobianPseudoInverse() const
VISP_EXPORT double measureTimeMs()