Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoSimuFourPoints2DPolarCamVelocityDisplay.cpp

Simulation of a 2D visual servoing:

Simulation of a 2D visual servoing:Simulation of a 2D visual servoing:

Interaction matrix is computed as the mean of the current and desired interaction matrix.

/****************************************************************************
*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2023 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
* For using ViSP with software that can not be combined with the GNU
* GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
* See https://visp.inria.fr for more information.
*
* This software was developed at:
* Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
*
* If you have questions regarding the use of this file, please contact
* Inria at visp@inria.fr
*
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
* Simulation of a 2D visual servoing using 4 points with polar
* coordinates as visual feature.
*
*****************************************************************************/
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || 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/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpProjectionDisplay.h>
#include <visp3/io/vpParseArgv.h>
#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeaturePointPolar.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
// List of allowed command line options
#define GETOPTARGS "cdh"
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;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
case 'c':
click_allowed = false;
break;
case 'd':
display = false;
break;
case 'h':
usage(argv[0], NULL);
return false;
default:
usage(argv[0], optarg_);
return false;
}
}
if ((c == 1) || (c == -1)) {
// standalone param or error
usage(argv[0], NULL);
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 {
// Log file creation in /tmp/$USERNAME/log.dat
// This file contains by line:
// - the 6 computed camera velocities (m/s, rad/s) to achieve the task
// - the 6 mesured camera velocities (m/s, rad/s)
// - the 6 mesured joint positions (m, rad)
// - the 8 values of s - s*
std::string username;
// Get the user login name
// Create a log filename to save velocities...
std::string logdirname;
#if defined(_WIN32)
logdirname = "C:/temp/" + username;
#else
logdirname = "/tmp/" + username;
#endif
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
} 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";
// Open the log file name
std::ofstream flog(logfilename.c_str());
bool opt_click_allowed = true;
bool opt_display = true;
// Read the command line options
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
return EXIT_FAILURE;
}
// We open two displays, one for the internal camera view, the other one for
// the external view, using either X11, GTK or GDI.
#if defined(VISP_HAVE_X11)
vpDisplayX displayInt;
vpDisplayX displayExt;
#elif defined(VISP_HAVE_GTK)
vpDisplayGTK displayInt;
vpDisplayGTK displayExt;
#elif defined(VISP_HAVE_GDI)
vpDisplayGDI displayInt;
vpDisplayGDI displayExt;
#elif defined(HAVE_OPENCV_HIGHGUI)
vpDisplayOpenCV displayInt;
vpDisplayOpenCV displayExt;
#endif
// open a display for the visualization
vpImage<unsigned char> Iint(300, 300, 0);
vpImage<unsigned char> Iext(300, 300, 0);
if (opt_display) {
displayInt.init(Iint, 0, 0, "Internal view");
displayExt.init(Iext, 330, 000, "External view");
}
vpProjectionDisplay externalview;
double px = 500, py = 500;
double u0 = 150, v0 = 160;
vpCameraParameters cam(px, py, u0, v0);
vpServo task;
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;
// #define TRANS_Z_PURE
// #define TRANS_X_PURE
// #define ROT_Z_PURE
// #define ROT_X_PURE
#define COMPLEX
//#define PROBLEM
#if defined(TRANS_Z_PURE)
// sets the initial camera location
// sets the desired camera location
#elif defined(TRANS_X_PURE)
// sets the initial camera location
// sets the desired camera location
vpHomogeneousMatrix cMod(0.5, 0.3, 3, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
#elif defined(ROT_Z_PURE)
// sets the initial camera location
// sets the desired camera location
#elif defined(ROT_X_PURE)
// sets the initial camera location
// sets the desired camera location
#elif defined(COMPLEX)
// sets the initial camera location
// sets the desired camera location
vpHomogeneousMatrix cMod(0, 0, 2.5, vpMath::rad(45), vpMath::rad(10), vpMath::rad(30));
#elif defined(PROBLEM)
// Bad behavior with an interaction matrix computed from the desired
// features sets the initial camera location
// sets the desired camera location
vpHomogeneousMatrix cMod(0.4, 0.2, 3, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
#endif
// Compute the position of the object in the world frame
robot.getPosition(wMc);
wMo = wMc * cMo;
vpHomogeneousMatrix cextMo(0, 0, 6, vpMath::rad(40), vpMath::rad(10), vpMath::rad(60));
// sets the point coordinates in the object frame
vpPoint point[4];
point[0].setWorldCoordinates(-0.25, -0.25, 0);
point[1].setWorldCoordinates(0.25, -0.25, 0);
point[2].setWorldCoordinates(0.25, 0.25, 0);
point[3].setWorldCoordinates(-0.25, 0.25, 0);
for (unsigned int i = 0; i < 4; i++)
externalview.insert(point[i]);
// sets the desired position of the feature point s*"
// computes the point coordinates in the desired camera frame and
// its 2D coordinates
for (unsigned int i = 0; i < 4; i++) {
point[i].track(cMod);
// Computes the polar coordinates from the image point
// cartesian coordinates
vpFeatureBuilder::create(pd[i], point[i]);
}
// computes the point coordinates in the camera frame and its 2D
// coordinates
for (unsigned int i = 0; i < 4; i++)
point[i].track(cMo);
// sets the desired position of the point
for (unsigned int i = 0; i < 4; i++) {
// retrieve x,y and Z of the vpPoint structure to initialize the
// visual feature
vpFeatureBuilder::create(p[i], point[i]);
}
// Define the task;
// - we want an eye-in-hand control law
// - articular velocity are computed
// task.setInteractionMatrixType(vpServo::MEAN) ;
// task.setInteractionMatrixType(vpServo::DESIRED) ;
// Set the position of the end-effector frame in the camera frame as identity
task.set_cVe(cVe);
// Set the Jacobian (expressed in the end-effector frame)
vpMatrix eJe;
robot.get_eJe(eJe);
task.set_eJe(eJe);
// we want to see a point on a point
for (unsigned int i = 0; i < 4; i++)
task.addFeature(p[i], pd[i]);
// set the gain
task.setLambda(1);
std::cout << "\nDisplay task information: " << std::endl;
task.print();
unsigned int iter = 0;
// loop
while (iter++ < 200) {
std::cout << "---------------------------------------------" << iter << std::endl;
// Set the Jacobian (expressed in the end-effector frame)
// Since q is modified eJe is modified
robot.get_eJe(eJe);
task.set_eJe(eJe);
// get the robot position
robot.getPosition(wMc);
// Compute the position of the object frame in the camera frame
cMo = wMc.inverse() * wMo;
// Compute new point position
for (unsigned int i = 0; i < 4; i++) {
point[i].track(cMo);
// retrieve x,y and Z of the vpPoint structure to compute the feature
vpFeatureBuilder::create(p[i], point[i]);
}
if (opt_display) {
vpServoDisplay::display(task, cam, Iint);
externalview.display(Iext, cextMo, cMo, cam, vpColor::green);
}
// Compute the control law
v = task.computeControlLaw();
if (iter == 1) {
std::cout << "Display task information: " << std::endl;
task.print();
}
// Send the camera velocity to the controller
// Save velocities applied to the robot in the log file
// v[0], v[1], v[2] correspond to camera translation velocities in m/s
// v[3], v[4], v[5] correspond to camera rotation velocities in rad/s
flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " ";
std::cout << "v: " << v.t() << std::endl;
std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
// Save feature error (s-s*) for the 4 feature points. For each feature
// point, we have 2 errors (along x and y axis). This error is
// expressed in meters in the camera frame
flog << (task.getError()).t() << " "; // s-s* for point 4
std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
// Save current visual feature s = (rho,theta)
for (unsigned int i = 0; i < 4; i++) {
flog << p[i].get_rho() << " " << p[i].get_theta() << " ";
}
// Save current position of the points
for (unsigned int i = 0; i < 4; i++) {
flog << point[i].get_x() << " " << point[i].get_y() << " ";
}
flog << std::endl;
if (iter == 1) {
ip.set_i(10);
ip.set_j(10);
std::cout << "\nClick in the internal camera view to continue..." << std::endl;
vpDisplay::displayText(Iint, ip, "A click to continue...", vpColor::red);
}
}
flog.close(); // Close the log file
// Display task information
task.print();
// Kill the task
std::cout << "Final robot position with respect to the object frame:\n";
cMo.print();
if (opt_display && opt_click_allowed) {
vpDisplay::displayText(Iint, 20, 20, "Click to quit...", vpColor::white);
}
return EXIT_SUCCESS;
} catch (const vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return EXIT_FAILURE;
}
}
#else
int main()
{
std::cout << "You do not have X11, or GTK, or GDI (Graphical Device Interface) 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;
}
#endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
vpRowVector t() const
static const vpColor white
Definition vpColor.h:206
static const vpColor red
Definition vpColor.h:211
static const vpColor green
Definition vpColor.h:214
Display for windows using GDI (available on any windows 32 platform).
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...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition vpDisplayX.h:132
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
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)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emitted by ViSP classes.
Definition vpException.h:59
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines 2D image point visual feature with polar coordinates described in Corke09a.
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void set_j(double jj)
void set_i(double ii)
Definition of the vpImage class member functions.
Definition vpImage.h:135
static bool checkDirectory(const std::string &dirname)
static std::string getUserName()
static void makeDirectory(const std::string &dirname)
static double rad(double deg)
Definition vpMath.h:116
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
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 ...
Definition vpPoint.h:77
double get_y() const
Get the point y coordinate in the image plane.
Definition vpPoint.cpp:469
double get_x() const
Get the point x coordinate in the image plane.
Definition vpPoint.cpp:467
void setWorldCoordinates(double oX, double oY, double oZ)
Definition vpPoint.cpp:110
interface with the image for feature display
void display(vpImage< unsigned char > &I, const vpHomogeneousMatrix &cextMo, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color, const bool &displayTraj=false, unsigned int thickness=1)
void insert(vpForwardProjection &fp)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void get_eJe(vpMatrix &eJe)
@ CAMERA_FRAME
Definition vpRobot.h:80
double sumSquare() const
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)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition vpServo.cpp:564
@ EYEINHAND_L_cVe_eJe
Definition vpServo.h:155
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition vpServo.h:448
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition vpServo.cpp:299
void setLambda(double c)
Definition vpServo.h:403
void set_eJe(const vpMatrix &eJe_)
Definition vpServo.h:506
void setServo(const vpServoType &servo_type)
Definition vpServo.cpp:210
vpColVector getError() const
Definition vpServo.h:276
vpColVector computeControlLaw()
Definition vpServo.cpp:930
@ FEATURE_CURRENT
Definition vpServo.h:208
@ FEATURE_DESIRED
Definition vpServo.h:209
@ CURRENT
Definition vpServo.h:179
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition vpServo.cpp:487
Class that defines the simplest robot: a free flying camera.